Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h
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_