Point Cloud Library (PCL)  1.11.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 #pragma once
42 
43 #include <pcl/filters/voxel_grid.h>
44 
45 namespace pcl
46 {
47  /** \brief VoxelGrid to estimate occluded space in the scene.
48  * The ray traversal algorithm is implemented by the work of
49  * 'John Amanatides and Andrew Woo, A Fast Voxel Traversal Algorithm for Ray Tracing'
50  *
51  * \author Christian Potthast
52  * \ingroup filters
53  */
54  template <typename PointT>
56  {
57  protected:
63 
65  using PointCloudPtr = typename PointCloud::Ptr;
67 
68  public:
69  /** \brief Empty constructor. */
71  {
72  initialized_ = false;
73  this->setSaveLeafLayout (true);
74  }
75 
76  /** \brief Destructor. */
78  {
79  }
80 
81  /** \brief Initialize the voxel grid, needs to be called first
82  * Builts the voxel grid and computes additional values for
83  * the ray traversal algorithm.
84  */
85  void
87 
88  /** \brief Computes the state (free = 0, occluded = 1) of the voxel
89  * after utilizing a ray traversal algorithm to a target voxel
90  * in (i, j, k) coordinates.
91  * \param[out] out_state The state of the voxel.
92  * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel.
93  * \return 0 upon success and -1 if an error occurs
94  */
95  int
96  occlusionEstimation (int& out_state,
97  const Eigen::Vector3i& in_target_voxel);
98 
99  /** \brief Computes 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] out_state The state of the voxel.
105  * \param[out] out_ray The voxels penetrated of the ray-traversal algorithm.
106  * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel.
107  * \return 0 upon success and -1 if an error occurs
108  */
109  int
110  occlusionEstimation (int& out_state,
111  std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
112  const Eigen::Vector3i& in_target_voxel);
113 
114  /** \brief Computes the voxel coordinates (i, j, k) of all occluded
115  * voxels in the voxel grid.
116  * \param[out] occluded_voxels the coordinates (i, j, k) of all occluded voxels
117  * \return 0 upon success and -1 if an error occurs
118  */
119  int
120  occlusionEstimationAll (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& occluded_voxels);
121 
122  /** \brief Returns the voxel grid filtered point cloud
123  * \return The voxel grid filtered point cloud
124  */
125  inline PointCloud
127 
128 
129  /** \brief Returns the minimum bounding of coordinates of the voxel grid (x,y,z).
130  * \return the minimum coordinates (x,y,z)
131  */
132  inline Eigen::Vector3f
133  getMinBoundCoordinates () { return (b_min_.head<3> ()); }
134 
135  /** \brief Returns the maximum bounding of coordinates of the voxel grid (x,y,z).
136  * \return the maximum coordinates (x,y,z)
137  */
138  inline Eigen::Vector3f
139  getMaxBoundCoordinates () { return (b_max_.head<3> ()); }
140 
141  /** \brief Returns the corresponding centroid (x,y,z) coordinates
142  * in the grid of voxel (i,j,k).
143  * \param[in] ijk the coordinate (i, j, k) of the voxel
144  * \return the (x,y,z) coordinate of the voxel centroid
145  */
146  inline Eigen::Vector4f
147  getCentroidCoordinate (const Eigen::Vector3i& ijk)
148  {
149  int i,j,k;
150  i = ((b_min_[0] < 0) ? (std::abs (min_b_[0]) + ijk[0]) : (ijk[0] - min_b_[0]));
151  j = ((b_min_[1] < 0) ? (std::abs (min_b_[1]) + ijk[1]) : (ijk[1] - min_b_[1]));
152  k = ((b_min_[2] < 0) ? (std::abs (min_b_[2]) + ijk[2]) : (ijk[2] - min_b_[2]));
153 
154  Eigen::Vector4f xyz;
155  xyz[0] = b_min_[0] + (leaf_size_[0] * 0.5f) + (static_cast<float> (i) * leaf_size_[0]);
156  xyz[1] = b_min_[1] + (leaf_size_[1] * 0.5f) + (static_cast<float> (j) * leaf_size_[1]);
157  xyz[2] = b_min_[2] + (leaf_size_[2] * 0.5f) + (static_cast<float> (k) * leaf_size_[2]);
158  xyz[3] = 0;
159  return xyz;
160  }
161 
162  // inline void
163  // setSensorOrigin (const Eigen::Vector4f origin) { sensor_origin_ = origin; }
164 
165  // inline void
166  // setSensorOrientation (const Eigen::Quaternionf orientation) { sensor_orientation_ = orientation; }
167 
168  protected:
169 
170  /** \brief Returns the scaling value (tmin) were the ray intersects with the
171  * voxel grid bounding box. (p_entry = origin + tmin * orientation)
172  * \param[in] origin The sensor origin
173  * \param[in] direction The sensor orientation
174  * \return the scaling value
175  */
176  float
177  rayBoxIntersection (const Eigen::Vector4f& origin,
178  const Eigen::Vector4f& direction);
179 
180  /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied)
181  * unsing a ray traversal algorithm.
182  * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k).
183  * \param[in] origin The sensor origin.
184  * \param[in] direction The sensor orientation
185  * \param[in] t_min The scaling value (tmin).
186  * \return The estimated voxel state.
187  */
188  int
189  rayTraversal (const Eigen::Vector3i& target_voxel,
190  const Eigen::Vector4f& origin,
191  const Eigen::Vector4f& direction,
192  const float t_min);
193 
194  /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) and
195  * the voxels penetrated by the ray unsing a ray traversal algorithm.
196  * \param[out] out_ray The voxels penetrated by the ray in (i, j, k) coordinates
197  * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k).
198  * \param[in] origin The sensor origin.
199  * \param[in] direction The sensor orientation
200  * \param[in] t_min The scaling value (tmin).
201  * \return The estimated voxel state.
202  */
203  int
204  rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
205  const Eigen::Vector3i& target_voxel,
206  const Eigen::Vector4f& origin,
207  const Eigen::Vector4f& direction,
208  const float t_min);
209 
210  /** \brief Returns a rounded value.
211  * \param[in] d
212  * \return rounded value
213  */
214  inline float
215  round (float d)
216  {
217  return static_cast<float> (std::floor (d + 0.5f));
218  }
219 
220  // We use round here instead of std::floor due to some numerical issues.
221  /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
222  * \param[in] x the X point coordinate to get the (i, j, k) index at
223  * \param[in] y the Y point coordinate to get the (i, j, k) index at
224  * \param[in] z the Z point coordinate to get the (i, j, k) index at
225  */
226  inline Eigen::Vector3i
227  getGridCoordinatesRound (float x, float y, float z)
228  {
229  return Eigen::Vector3i (static_cast<int> (round (x * inverse_leaf_size_[0])),
230  static_cast<int> (round (y * inverse_leaf_size_[1])),
231  static_cast<int> (round (z * inverse_leaf_size_[2])));
232  }
233 
234  // initialization flag
236 
237  Eigen::Vector4f sensor_origin_;
238  Eigen::Quaternionf sensor_orientation_;
239 
240  // minimum and maximum bounding box coordinates
241  Eigen::Vector4f b_min_, b_max_;
242 
243  // voxel grid filtered cloud
245  };
246 }
247 
248 #ifdef PCL_NO_PRECOMPILE
249 #include <pcl/filters/impl/voxel_grid_occlusion_estimation.hpp>
250 #endif
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).
typename Filter< PointT >::PointCloud PointCloud
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
float round(float d)
Returns a rounded value.
int occlusionEstimation(int &out_state, const Eigen::Vector3i &in_target_voxel)
Computes the state (free = 0, occluded = 1) of the voxel after utilizing a ray traversal algorithm to...
int occlusionEstimationAll(std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > &occluded_voxels)
Computes the voxel coordinates (i, j, k) of all occluded voxels in the voxel grid.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition: voxel_grid.h:279
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:177
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:459
VoxelGrid to estimate occluded space in the scene.
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
Eigen::Vector3f getMinBoundCoordinates()
Returns the minimum bounding of coordinates of the voxel grid (x,y,z).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:456
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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:471
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...
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74