Point Cloud Library (PCL)  1.9.1-dev
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  typedef typename PointCloud::Ptr PointCloudPtr;
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) ? (abs (min_b_[0]) + ijk[0]) : (ijk[0] - min_b_[0]));
151  j = ((b_min_[1] < 0) ? (abs (min_b_[1]) + ijk[1]) : (ijk[1] - min_b_[1]));
152  k = ((b_min_[2] < 0) ? (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> (floor (d + 0.5f));
218  }
219 
220  // We use round here instead of 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
244  PointCloud 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).
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.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition: voxel_grid.h:280
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).
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition: voxel_grid.h:460
VoxelGrid to estimate occluded space in the scene.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:428
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:457
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:472
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...