Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * Author : Christian Potthast 00037 * Email : potthast@usc.edu 00038 * 00039 */ 00040 00041 #ifndef PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 00042 #define PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 00043 00044 #include <pcl/filters/voxel_grid.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief VoxelGrid to estimate occluded space in the scene. 00049 * The ray traversal algorithm is implemented by the work of 00050 * 'John Amanatides and Andrew Woo, A Fast Voxel Traversal Algorithm for Ray Tracing' 00051 * 00052 * \author Christian Potthast 00053 * \ingroup filters 00054 */ 00055 template <typename PointT> 00056 class VoxelGridOcclusionEstimation: public VoxelGrid<PointT> 00057 { 00058 protected: 00059 using VoxelGrid<PointT>::min_b_; 00060 using VoxelGrid<PointT>::max_b_; 00061 using VoxelGrid<PointT>::div_b_; 00062 using VoxelGrid<PointT>::leaf_size_; 00063 using VoxelGrid<PointT>::inverse_leaf_size_; 00064 00065 typedef typename Filter<PointT>::PointCloud PointCloud; 00066 typedef typename PointCloud::Ptr PointCloudPtr; 00067 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00068 00069 public: 00070 /** \brief Empty constructor. */ 00071 VoxelGridOcclusionEstimation () 00072 { 00073 initialized_ = false; 00074 this->setSaveLeafLayout (true); 00075 } 00076 00077 /** \brief Destructor. */ 00078 virtual ~VoxelGridOcclusionEstimation () 00079 { 00080 } 00081 00082 /** \brief Initialize the voxel grid, needs to be called first 00083 * Builts the voxel grid and computes additional values for 00084 * the ray traversal algorithm. 00085 */ 00086 void 00087 initializeVoxelGrid (); 00088 00089 /** \brief Returns the state (free = 0, occluded = 1) of the voxel 00090 * after utilizing a ray traversal algorithm to a target voxel 00091 * in (i, j, k) coordinates. 00092 * \param[out] The state of the voxel. 00093 * \param[in] The target voxel coordinate (i, j, k) of the voxel. 00094 */ 00095 int 00096 occlusionEstimation (int& out_state, 00097 const Eigen::Vector3i& in_target_voxel); 00098 00099 /** \brief Returns the state (free = 0, occluded = 1) of the voxel 00100 * after utilizing a ray traversal algorithm to a target voxel 00101 * in (i, j, k) coordinates. Additionally, this function returns 00102 * the voxels penetrated of the ray-traversal algorithm till reaching 00103 * the target voxel. 00104 * \param[out] The state of the voxel. 00105 * \param[out] The voxels penetrated of the ray-traversal algorithm. 00106 * \param[in] The target voxel coordinate (i, j, k) of the voxel. 00107 */ 00108 int 00109 occlusionEstimation (int& out_state, 00110 std::vector<Eigen::Vector3i>& out_ray, 00111 const Eigen::Vector3i& in_target_voxel); 00112 00113 /** \brief Returns the voxel coordinates (i, j, k) of all occluded 00114 * voxels in the voxel gird. 00115 * \param[out] the coordinates (i, j, k) of all occluded voxels 00116 */ 00117 int 00118 occlusionEstimationAll (std::vector<Eigen::Vector3i>& occluded_voxels); 00119 00120 /** \brief Returns the voxel grid filtered point cloud 00121 * \param[out] The voxel grid filtered point cloud 00122 */ 00123 inline PointCloud 00124 getFilteredPointCloud () { return filtered_cloud_; } 00125 00126 00127 /** \brief Returns the minimum bounding of coordinates of the voxel grid (x,y,z). 00128 * \return the minimum coordinates (x,y,z) 00129 */ 00130 inline Eigen::Vector3f 00131 getMinBoundCoordinates () { return (b_min_.head<3> ()); } 00132 00133 /** \brief Returns the maximum bounding of coordinates of the voxel grid (x,y,z). 00134 * \return the maximum coordinates (x,y,z) 00135 */ 00136 inline Eigen::Vector3f 00137 getMaxBoundCoordinates () { return (b_max_.head<3> ()); } 00138 00139 /** \brief Returns the corresponding centroid (x,y,z) coordinates 00140 * in the grid of voxel (i,j,k). 00141 * \param[in] the coordinate (i, j, k) of the voxel 00142 * \return the (x,y,z) coordinate of the voxel centroid 00143 */ 00144 inline Eigen::Vector4f 00145 getCentroidCoordinate (const Eigen::Vector3i& ijk) 00146 { 00147 int i,j,k; 00148 i = ((b_min_[0] < 0) ? (abs (min_b_[0]) + ijk[0]) : (ijk[0] - min_b_[0])); 00149 j = ((b_min_[1] < 0) ? (abs (min_b_[1]) + ijk[1]) : (ijk[1] - min_b_[1])); 00150 k = ((b_min_[2] < 0) ? (abs (min_b_[2]) + ijk[2]) : (ijk[2] - min_b_[2])); 00151 00152 Eigen::Vector4f xyz; 00153 xyz[0] = b_min_[0] + (leaf_size_[0] * 0.5f) + (static_cast<float> (i) * leaf_size_[0]); 00154 xyz[1] = b_min_[1] + (leaf_size_[1] * 0.5f) + (static_cast<float> (j) * leaf_size_[1]); 00155 xyz[2] = b_min_[2] + (leaf_size_[2] * 0.5f) + (static_cast<float> (k) * leaf_size_[2]); 00156 xyz[3] = 0; 00157 return xyz; 00158 } 00159 00160 // inline void 00161 // setSensorOrigin (const Eigen::Vector4f origin) { sensor_origin_ = origin; } 00162 00163 // inline void 00164 // setSensorOrientation (const Eigen::Quaternionf orientation) { sensor_orientation_ = orientation; } 00165 00166 protected: 00167 00168 /** \brief Returns the scaling value (tmin) were the ray intersects with the 00169 * voxel grid bounding box. (p_entry = origin + tmin * orientation) 00170 * \param[in] The sensor origin 00171 * \param[in] The sensor orientation 00172 * \return the scaling value 00173 */ 00174 float 00175 rayBoxIntersection (const Eigen::Vector4f& origin, 00176 const Eigen::Vector4f& direction); 00177 00178 /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) 00179 * unsing a ray traversal algorithm. 00180 * \param[in] The target voxel in the voxel grid with coordinate (i, j, k). 00181 * \param[in] The sensor origin. 00182 * \param[in] The sensor orientation 00183 * \param[in] The scaling value (tmin). 00184 * \return The estimated voxel state. 00185 */ 00186 int 00187 rayTraversal (const Eigen::Vector3i& target_voxel, 00188 const Eigen::Vector4f& origin, 00189 const Eigen::Vector4f& direction, 00190 const float t_min); 00191 00192 /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) and 00193 * the voxels penetrated by the ray unsing a ray traversal algorithm. 00194 * \param[out] The voxels penetrated by the ray in (i, j, k) coordinates 00195 * \param[in] The target voxel in the voxel grid with coordinate (i, j, k). 00196 * \param[in] The sensor origin. 00197 * \param[in] The sensor orientation 00198 * \param[in] The scaling value (tmin). 00199 * \return The estimated voxel state. 00200 */ 00201 int 00202 rayTraversal (std::vector <Eigen::Vector3i>& out_ray, 00203 const Eigen::Vector3i& target_voxel, 00204 const Eigen::Vector4f& origin, 00205 const Eigen::Vector4f& direction, 00206 const float t_min); 00207 00208 /** \brief Returns a rounded value. 00209 * \param[in] value 00210 * \return rounded value 00211 */ 00212 inline float 00213 round (float d) 00214 { 00215 return static_cast<float> (floor (d + 0.5f)); 00216 } 00217 00218 // We use round here instead of floor due to some numerical issues. 00219 /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). 00220 * \param[in] x the X point coordinate to get the (i, j, k) index at 00221 * \param[in] y the Y point coordinate to get the (i, j, k) index at 00222 * \param[in] z the Z point coordinate to get the (i, j, k) index at 00223 */ 00224 inline Eigen::Vector3i 00225 getGridCoordinatesRound (float x, float y, float z) 00226 { 00227 return Eigen::Vector3i (static_cast<int> (round (x * inverse_leaf_size_[0])), 00228 static_cast<int> (round (y * inverse_leaf_size_[1])), 00229 static_cast<int> (round (z * inverse_leaf_size_[2]))); 00230 } 00231 00232 // initialization flag 00233 bool initialized_; 00234 00235 Eigen::Vector4f sensor_origin_; 00236 Eigen::Quaternionf sensor_orientation_; 00237 00238 // minimum and maximum bounding box coordinates 00239 Eigen::Vector4f b_min_, b_max_; 00240 00241 // voxel grid filtered cloud 00242 PointCloud filtered_cloud_; 00243 }; 00244 } 00245 00246 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_