Point Cloud Library (PCL)  1.14.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 #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  * Example code:
51  * \code
52  * pcl::VoxelGridOcclusionEstimation<pcl::PointXYZ> vg;
53  * vg.setInputCloud (input_cloud);
54  * vg.setLeafSize (leaf_x, leaf_y, leaf_z);
55  * vg.initializeVoxelGrid ();
56  * std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > occluded_voxels;
57  * vg.occlusionEstimationAll (occluded_voxels);
58  * \endcode
59  * \author Christian Potthast
60  * \ingroup filters
61  */
62  template <typename PointT>
64  {
65  protected:
71 
73  using PointCloudPtr = typename PointCloud::Ptr;
75 
76  public:
77 
79 
80  /** \brief Empty constructor. */
82  {
83  initialized_ = false;
84  this->setSaveLeafLayout (true);
85  }
86 
87  /** \brief Destructor. */
88  ~VoxelGridOcclusionEstimation () override = default;
89 
90  /** \brief Initialize the voxel grid, needs to be called first
91  * Builts the voxel grid and computes additional values for
92  * the ray traversal algorithm.
93  */
94  void
96 
97  /** \brief Computes the state (free = 0, occluded = 1) of the voxel
98  * after utilizing a ray traversal algorithm to a target voxel
99  * in (i, j, k) coordinates.
100  * \param[out] out_state The state of the voxel.
101  * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel.
102  * \return 0 upon success and -1 if an error occurs
103  */
104  int
105  occlusionEstimation (int& out_state,
106  const Eigen::Vector3i& in_target_voxel);
107 
108  /** \brief Computes the state (free = 0, occluded = 1) of the voxel
109  * after utilizing a ray traversal algorithm to a target voxel
110  * in (i, j, k) coordinates. Additionally, this function returns
111  * the voxels penetrated of the ray-traversal algorithm till reaching
112  * the target voxel.
113  * \param[out] out_state The state of the voxel.
114  * \param[out] out_ray The voxels penetrated of the ray-traversal algorithm.
115  * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel.
116  * \return 0 upon success and -1 if an error occurs
117  */
118  int
119  occlusionEstimation (int& out_state,
120  std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
121  const Eigen::Vector3i& in_target_voxel);
122 
123  /** \brief Computes the voxel coordinates (i, j, k) of all occluded
124  * voxels in the voxel grid.
125  * \param[out] occluded_voxels the coordinates (i, j, k) of all occluded voxels
126  * \return 0 upon success and -1 if an error occurs
127  */
128  int
129  occlusionEstimationAll (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& occluded_voxels);
130 
131  /** \brief Returns the voxel grid filtered point cloud
132  * \return The voxel grid filtered point cloud
133  */
134  inline PointCloud
136 
137 
138  /** \brief Returns the minimum bounding of coordinates of the voxel grid (x,y,z).
139  * \return the minimum coordinates (x,y,z)
140  */
141  inline Eigen::Vector3f
142  getMinBoundCoordinates () { return (b_min_.head<3> ()); }
143 
144  /** \brief Returns the maximum bounding of coordinates of the voxel grid (x,y,z).
145  * \return the maximum coordinates (x,y,z)
146  */
147  inline Eigen::Vector3f
148  getMaxBoundCoordinates () { return (b_max_.head<3> ()); }
149 
150  /** \brief Returns the corresponding centroid (x,y,z) coordinates
151  * in the grid of voxel (i,j,k).
152  * \param[in] ijk the coordinate (i, j, k) of the voxel
153  * \return the (x,y,z) coordinate of the voxel centroid
154  */
155  inline Eigen::Vector4f
156  getCentroidCoordinate (const Eigen::Vector3i& ijk)
157  {
158  int i,j,k;
159  i = ((b_min_[0] < 0) ? (std::abs (min_b_[0]) + ijk[0]) : (ijk[0] - min_b_[0]));
160  j = ((b_min_[1] < 0) ? (std::abs (min_b_[1]) + ijk[1]) : (ijk[1] - min_b_[1]));
161  k = ((b_min_[2] < 0) ? (std::abs (min_b_[2]) + ijk[2]) : (ijk[2] - min_b_[2]));
162 
163  Eigen::Vector4f xyz;
164  xyz[0] = b_min_[0] + (leaf_size_[0] * 0.5f) + (static_cast<float> (i) * leaf_size_[0]);
165  xyz[1] = b_min_[1] + (leaf_size_[1] * 0.5f) + (static_cast<float> (j) * leaf_size_[1]);
166  xyz[2] = b_min_[2] + (leaf_size_[2] * 0.5f) + (static_cast<float> (k) * leaf_size_[2]);
167  xyz[3] = 0;
168  return xyz;
169  }
170 
171  // inline void
172  // setSensorOrigin (const Eigen::Vector4f origin) { sensor_origin_ = origin; }
173 
174  // inline void
175  // setSensorOrientation (const Eigen::Quaternionf orientation) { sensor_orientation_ = orientation; }
176 
177  protected:
178 
179  /** \brief Returns the scaling value (tmin) were the ray intersects with the
180  * voxel grid bounding box. (p_entry = origin + tmin * orientation)
181  * \param[in] origin The sensor origin
182  * \param[in] direction The sensor orientation
183  * \return the scaling value
184  */
185  float
186  rayBoxIntersection (const Eigen::Vector4f& origin,
187  const Eigen::Vector4f& direction);
188 
189  /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied)
190  * using a ray traversal algorithm.
191  * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k).
192  * \param[in] origin The sensor origin.
193  * \param[in] direction The sensor orientation
194  * \param[in] t_min The scaling value (tmin).
195  * \return The estimated voxel state.
196  */
197  int
198  rayTraversal (const Eigen::Vector3i& target_voxel,
199  const Eigen::Vector4f& origin,
200  const Eigen::Vector4f& direction,
201  const float t_min);
202 
203  /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) and
204  * the voxels penetrated by the ray using a ray traversal algorithm.
205  * \param[out] out_ray The voxels penetrated by the ray in (i, j, k) coordinates
206  * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k).
207  * \param[in] origin The sensor origin.
208  * \param[in] direction The sensor orientation
209  * \param[in] t_min The scaling value (tmin).
210  * \return The estimated voxel state.
211  */
212  int
213  rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
214  const Eigen::Vector3i& target_voxel,
215  const Eigen::Vector4f& origin,
216  const Eigen::Vector4f& direction,
217  const float t_min);
218 
219  /** \brief Returns a value rounded to the nearest integer
220  * \param[in] d
221  * \return rounded value
222  */
223  inline float
224  round (float d)
225  {
226  return static_cast<float> (std::floor (d + 0.5f));
227  }
228 
229  /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
230  * \param[in] x the X point coordinate to get the (i, j, k) index at
231  * \param[in] y the Y point coordinate to get the (i, j, k) index at
232  * \param[in] z the Z point coordinate to get the (i, j, k) index at
233  */
234  inline Eigen::Vector3i
235  getGridCoordinatesRound (float x, float y, float z)
236  {
237  return {static_cast<int> (round (x * inverse_leaf_size_[0])),
238  static_cast<int> (round (y * inverse_leaf_size_[1])),
239  static_cast<int> (round (z * inverse_leaf_size_[2]))};
240  }
241 
242  // initialization flag
244 
245  Eigen::Vector4f sensor_origin_;
246  Eigen::Quaternionf sensor_orientation_;
247 
248  // minimum and maximum bounding box coordinates
249  Eigen::Vector4f b_min_, b_max_;
250 
251  // voxel grid filtered cloud
253  };
254 }
255 
256 #ifdef PCL_NO_PRECOMPILE
257 #include <pcl/filters/impl/voxel_grid_occlusion_estimation.hpp>
258 #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:413
float round(float d)
Returns a value rounded to the nearest integer.
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.
PCL_MAKE_ALIGNED_OPERATOR_NEW VoxelGridOcclusionEstimation()
Empty constructor.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition: voxel_grid.h:304
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:209
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
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:486
VoxelGrid to estimate occluded space in the scene.
~VoxelGridOcclusionEstimation() override=default
Destructor.
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. ...
Definition: distances.h:55
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:483
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
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) using 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:498
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