Point Cloud Library (PCL)  1.7.1
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::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition: voxel_grid.h:448
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
VoxelGrid to estimate occluded space in the scene.
Eigen::Vector3f getMaxBoundCoordinates()
Returns the maximum bounding of coordinates of the voxel grid (x,y,z).
void initializeVoxelGrid()
Initialize the voxel grid, needs to be called first Builts the voxel grid and computes additional val...
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
Eigen::Vector4f getCentroidCoordinate(const Eigen::Vector3i &ijk)
Returns the corresponding centroid (x,y,z) coordinates in the grid of voxel (i,j,k).
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
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 ...
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...
PointCloud getFilteredPointCloud()
Returns the voxel grid filtered point cloud.
int occlusionEstimationAll(std::vector< Eigen::Vector3i > &occluded_voxels)
Returns the voxel coordinates (i, j, k) of all occluded voxels in the voxel gird. ...
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
Definition: voxel_grid.h:460
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 round(float d)
Returns a rounded value.
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:445
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 getMinBoundCoordinates()
Returns the minimum bounding of coordinates of the voxel grid (x,y,z).