voxel_grid_occlusion_estimation.h
Go to the documentation of this file.
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 {
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:
00071       VoxelGridOcclusionEstimation ()
00072       {
00073         initialized_ = false;
00074         this->setSaveLeafLayout (true);
00075       }
00076 
00078       virtual ~VoxelGridOcclusionEstimation ()
00079       {
00080       }
00081 
00086       void
00087       initializeVoxelGrid ();
00088 
00095       int
00096       occlusionEstimation (int& out_state,
00097                            const Eigen::Vector3i& in_target_voxel);
00098 
00108       int
00109       occlusionEstimation (int& out_state,
00110                            std::vector<Eigen::Vector3i>& out_ray,
00111                            const Eigen::Vector3i& in_target_voxel);
00112 
00117       int
00118       occlusionEstimationAll (std::vector<Eigen::Vector3i>& occluded_voxels);
00119 
00123       inline PointCloud
00124       getFilteredPointCloud () { return filtered_cloud_; }
00125 
00126       
00130       inline Eigen::Vector3f
00131       getMinBoundCoordinates () { return (b_min_.head<3> ()); }
00132 
00136       inline Eigen::Vector3f
00137       getMaxBoundCoordinates () { return (b_max_.head<3> ()); }
00138 
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 
00174       float
00175       rayBoxIntersection (const Eigen::Vector4f& origin, 
00176                           const Eigen::Vector4f& direction);
00177 
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 
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 
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.
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_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:38:39