Point Cloud Library (PCL)  1.7.0
voxel_grid_occlusion_estimation.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author : Christian Potthast
37  * Email : potthast@usc.edu
38  *
39  */
40 
41 #ifndef PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
42 #define PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
43 
44 #include <pcl/filters/voxel_grid.h>
45 
46 namespace pcl
47 {
48  /** \brief VoxelGrid to estimate occluded space in the scene.
49  * The ray traversal algorithm is implemented by the work of
50  * 'John Amanatides and Andrew Woo, A Fast Voxel Traversal Algorithm for Ray Tracing'
51  *
52  * \author Christian Potthast
53  * \ingroup filters
54  */
55  template <typename PointT>
57  {
58  protected:
64 
66  typedef typename PointCloud::Ptr PointCloudPtr;
68 
69  public:
70  /** \brief Empty constructor. */
72  {
73  initialized_ = false;
74  this->setSaveLeafLayout (true);
75  }
76 
77  /** \brief Destructor. */
79  {
80  }
81 
82  /** \brief Initialize the voxel grid, needs to be called first
83  * Builts the voxel grid and computes additional values for
84  * the ray traversal algorithm.
85  */
86  void
88 
89  /** \brief Returns the state (free = 0, occluded = 1) of the voxel
90  * after utilizing a ray traversal algorithm to a target voxel
91  * in (i, j, k) coordinates.
92  * \param[out] The state of the voxel.
93  * \param[in] The target voxel coordinate (i, j, k) of the voxel.
94  */
95  int
96  occlusionEstimation (int& out_state,
97  const Eigen::Vector3i& in_target_voxel);
98 
99  /** \brief Returns the state (free = 0, occluded = 1) of the voxel
100  * after utilizing a ray traversal algorithm to a target voxel
101  * in (i, j, k) coordinates. Additionally, this function returns
102  * the voxels penetrated of the ray-traversal algorithm till reaching
103  * the target voxel.
104  * \param[out] The state of the voxel.
105  * \param[out] The voxels penetrated of the ray-traversal algorithm.
106  * \param[in] The target voxel coordinate (i, j, k) of the voxel.
107  */
108  int
109  occlusionEstimation (int& out_state,
110  std::vector<Eigen::Vector3i>& out_ray,
111  const Eigen::Vector3i& in_target_voxel);
112 
113  /** \brief Returns the voxel coordinates (i, j, k) of all occluded
114  * voxels in the voxel gird.
115  * \param[out] the coordinates (i, j, k) of all occluded voxels
116  */
117  int
118  occlusionEstimationAll (std::vector<Eigen::Vector3i>& occluded_voxels);
119 
120  /** \brief Returns the voxel grid filtered point cloud
121  * \param[out] The voxel grid filtered point cloud
122  */
123  inline PointCloud
125 
126 
127  /** \brief Returns the minimum bounding of coordinates of the voxel grid (x,y,z).
128  * \return the minimum coordinates (x,y,z)
129  */
130  inline Eigen::Vector3f
131  getMinBoundCoordinates () { return (b_min_.head<3> ()); }
132 
133  /** \brief Returns the maximum bounding of coordinates of the voxel grid (x,y,z).
134  * \return the maximum coordinates (x,y,z)
135  */
136  inline Eigen::Vector3f
137  getMaxBoundCoordinates () { return (b_max_.head<3> ()); }
138 
139  /** \brief Returns the corresponding centroid (x,y,z) coordinates
140  * in the grid of voxel (i,j,k).
141  * \param[in] the coordinate (i, j, k) of the voxel
142  * \return the (x,y,z) coordinate of the voxel centroid
143  */
144  inline Eigen::Vector4f
145  getCentroidCoordinate (const Eigen::Vector3i& ijk)
146  {
147  int i,j,k;
148  i = ((b_min_[0] < 0) ? (abs (min_b_[0]) + ijk[0]) : (ijk[0] - min_b_[0]));
149  j = ((b_min_[1] < 0) ? (abs (min_b_[1]) + ijk[1]) : (ijk[1] - min_b_[1]));
150  k = ((b_min_[2] < 0) ? (abs (min_b_[2]) + ijk[2]) : (ijk[2] - min_b_[2]));
151 
152  Eigen::Vector4f xyz;
153  xyz[0] = b_min_[0] + (leaf_size_[0] * 0.5f) + (static_cast<float> (i) * leaf_size_[0]);
154  xyz[1] = b_min_[1] + (leaf_size_[1] * 0.5f) + (static_cast<float> (j) * leaf_size_[1]);
155  xyz[2] = b_min_[2] + (leaf_size_[2] * 0.5f) + (static_cast<float> (k) * leaf_size_[2]);
156  xyz[3] = 0;
157  return xyz;
158  }
159 
160  // inline void
161  // setSensorOrigin (const Eigen::Vector4f origin) { sensor_origin_ = origin; }
162 
163  // inline void
164  // setSensorOrientation (const Eigen::Quaternionf orientation) { sensor_orientation_ = orientation; }
165 
166  protected:
167 
168  /** \brief Returns the scaling value (tmin) were the ray intersects with the
169  * voxel grid bounding box. (p_entry = origin + tmin * orientation)
170  * \param[in] The sensor origin
171  * \param[in] The sensor orientation
172  * \return the scaling value
173  */
174  float
175  rayBoxIntersection (const Eigen::Vector4f& origin,
176  const Eigen::Vector4f& direction);
177 
178  /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied)
179  * unsing a ray traversal algorithm.
180  * \param[in] The target voxel in the voxel grid with coordinate (i, j, k).
181  * \param[in] The sensor origin.
182  * \param[in] The sensor orientation
183  * \param[in] The scaling value (tmin).
184  * \return The estimated voxel state.
185  */
186  int
187  rayTraversal (const Eigen::Vector3i& target_voxel,
188  const Eigen::Vector4f& origin,
189  const Eigen::Vector4f& direction,
190  const float t_min);
191 
192  /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) and
193  * the voxels penetrated by the ray unsing a ray traversal algorithm.
194  * \param[out] The voxels penetrated by the ray in (i, j, k) coordinates
195  * \param[in] The target voxel in the voxel grid with coordinate (i, j, k).
196  * \param[in] The sensor origin.
197  * \param[in] The sensor orientation
198  * \param[in] The scaling value (tmin).
199  * \return The estimated voxel state.
200  */
201  int
202  rayTraversal (std::vector <Eigen::Vector3i>& out_ray,
203  const Eigen::Vector3i& target_voxel,
204  const Eigen::Vector4f& origin,
205  const Eigen::Vector4f& direction,
206  const float t_min);
207 
208  /** \brief Returns a rounded value.
209  * \param[in] value
210  * \return rounded value
211  */
212  inline float
213  round (float d)
214  {
215  return static_cast<float> (floor (d + 0.5f));
216  }
217 
218  // We use round here instead of floor due to some numerical issues.
219  /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
220  * \param[in] x the X point coordinate to get the (i, j, k) index at
221  * \param[in] y the Y point coordinate to get the (i, j, k) index at
222  * \param[in] z the Z point coordinate to get the (i, j, k) index at
223  */
224  inline Eigen::Vector3i
225  getGridCoordinatesRound (float x, float y, float z)
226  {
227  return Eigen::Vector3i (static_cast<int> (round (x * inverse_leaf_size_[0])),
228  static_cast<int> (round (y * inverse_leaf_size_[1])),
229  static_cast<int> (round (z * inverse_leaf_size_[2])));
230  }
231 
232  // initialization flag
234 
235  Eigen::Vector4f sensor_origin_;
236  Eigen::Quaternionf sensor_orientation_;
237 
238  // minimum and maximum bounding box coordinates
239  Eigen::Vector4f b_min_, b_max_;
240 
241  // voxel grid filtered cloud
243  };
244 }
245 
246 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
Eigen::Vector3i getGridCoordinatesRound(float x, float y, float z)
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
float rayBoxIntersection(const Eigen::Vector4f &origin, const Eigen::Vector4f &direction)
Returns the scaling value (tmin) were the ray intersects with the voxel grid bounding box...
Eigen::Vector3f getMaxBoundCoordinates()
Returns the maximum bounding of coordinates of the voxel grid (x,y,z).
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
float round(float d)
Returns a rounded value.
int occlusionEstimation(int &out_state, const Eigen::Vector3i &in_target_voxel)
Returns the state (free = 0, occluded = 1) of the voxel after utilizing a ray traversal algorithm to ...
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition: voxel_grid.h:268
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
Eigen::Vector4f getCentroidCoordinate(const Eigen::Vector3i &ijk)
Returns the corresponding centroid (x,y,z) coordinates in the grid of voxel (i,j,k).
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition: voxel_grid.h:448
VoxelGrid to estimate occluded space in the scene.
int occlusionEstimationAll(std::vector< Eigen::Vector3i > &occluded_voxels)
Returns the voxel coordinates (i, j, k) of all occluded voxels in the voxel gird. ...
Eigen::Vector3f getMinBoundCoordinates()
Returns the minimum bounding of coordinates of the voxel grid (x,y,z).
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:445
int rayTraversal(const Eigen::Vector3i &target_voxel, const Eigen::Vector4f &origin, const Eigen::Vector4f &direction, const float t_min)
Returns the state of the target voxel (0 = visible, 1 = occupied) unsing a ray traversal algorithm...
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
Definition: voxel_grid.h:460
PointCloud getFilteredPointCloud()
Returns the voxel grid filtered point cloud.
void initializeVoxelGrid()
Initialize the voxel grid, needs to be called first Builts the voxel grid and computes additional val...