Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
45namespace 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
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
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition voxel_grid.h:177
Eigen::Vector4i max_b_
Definition voxel_grid.h:470
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Definition voxel_grid.h:470
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition voxel_grid.h:278
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition voxel_grid.h:455
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition voxel_grid.h:458
Eigen::Vector4i div_b_
Definition voxel_grid.h:470
VoxelGrid to estimate occluded space in the scene.
typename Filter< PointT >::PointCloud PointCloud
void initializeVoxelGrid()
Initialize the voxel grid, needs to be called first Builts the voxel grid and computes additional val...
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.
float round(float d)
Returns a rounded value.
Eigen::Vector3i getGridCoordinatesRound(float x, float y, float z)
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
PointCloud getFilteredPointCloud()
Returns the voxel grid filtered point cloud.
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).
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.
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...
Eigen::Vector3f getMinBoundCoordinates()
Returns the minimum bounding of coordinates of the voxel grid (x,y,z).
Eigen::Vector4f getCentroidCoordinate(const Eigen::Vector3i &ijk)
Returns the corresponding centroid (x,y,z) coordinates in the grid of voxel (i,j,k).
A point structure representing Euclidean xyz coordinates, and the RGB color.