Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/frustum_culling.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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  */
00037 
00038 
00039 #ifndef PCL_FILTERS_FRUSTUM_CULLING_H_
00040 #define PCL_FILTERS_FRUSTUM_CULLING_H_
00041 
00042 #include <pcl/point_types.h>
00043 #include <pcl/filters/filter_indices.h>
00044 #include <pcl/common/transforms.h>
00045 #include <pcl/common/eigen.h>
00046 
00047 namespace pcl
00048 {
00049   /** \brief FrustumCulling filters points inside a frustum
00050    *  given by pose and field of view of the camera.
00051    *
00052    * Code example:
00053    *
00054    * \code
00055    * pcl::PointCloud <pcl::PointXYZ>::Ptr source; 
00056    * // .. read or fill the source cloud
00057    *
00058    * pcl::FrustumCulling<pcl::PointXYZ> fc;
00059    * fc.setInputCloud (source);
00060    * fc.setVerticalFOV (45);
00061    * fc.setHorizontalFOV (60);
00062    * fc.setNearPlaneDistance (5.0);
00063    * fc.setFarPlaneDistance (15);
00064    *
00065    * Eigen::Matrix4f camera_pose;
00066    * // .. read or input the camera pose from a registration algorithm.
00067    * fc.setCameraPose (camera_pose);
00068    *
00069    * pcl::PointCloud <pcl::PointXYZ> target;
00070    * fc.filter (target);
00071    * \endcode
00072    *
00073    *
00074    * \author Aravindhan K Krishnan
00075    * \ingroup filters
00076    */
00077   template <typename PointT>
00078   class FrustumCulling : public FilterIndices<PointT>
00079   {
00080     typedef typename Filter<PointT>::PointCloud PointCloud;
00081     typedef typename PointCloud::Ptr PointCloudPtr;
00082     typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00083 
00084     public:
00085 
00086       typedef boost::shared_ptr< FrustumCulling<PointT> > Ptr;
00087       typedef boost::shared_ptr< const FrustumCulling<PointT> > ConstPtr;
00088 
00089 
00090       using Filter<PointT>::getClassName;
00091 
00092       FrustumCulling (bool extract_removed_indices = false) 
00093         : FilterIndices<PointT>::FilterIndices (extract_removed_indices)
00094         , camera_pose_ (Eigen::Matrix4f::Identity ())
00095         , hfov_ (60.0f)
00096         , vfov_ (60.0f)
00097         , np_dist_ (0.1f)
00098         , fp_dist_ (5.0f)
00099       {
00100         filter_name_ = "FrustumCulling";
00101       }
00102 
00103       /** \brief Set the pose of the camera w.r.t the origin
00104         * \param[in] camera_pose the camera pose
00105         *
00106         * Note: This assumes a coordinate system where X is forward, 
00107         * Y is up, and Z is right. To convert from the traditional camera 
00108         * coordinate system (X right, Y down, Z forward), one can use:
00109         *
00110         * \code
00111         * Eigen::Matrix4f pose_orig = //pose in camera coordinates
00112         * Eigen::Matrix4f cam2robot;
00113         * cam2robot << 0, 0, 1, 0
00114         *              0,-1, 0, 0
00115         *              1, 0, 0, 0
00116         *              0, 0, 0, 1;
00117         * Eigen::Matrix4f pose_new = pose_orig * cam2robot;
00118         * fc.setCameraPose (pose_new);
00119         * \endcode
00120         */
00121       void 
00122       setCameraPose (const Eigen::Matrix4f& camera_pose)
00123       {
00124         camera_pose_ = camera_pose;
00125       }
00126 
00127       /** \brief Get the pose of the camera w.r.t the origin */
00128       Eigen::Matrix4f
00129       getCameraPose () const
00130       {
00131         return (camera_pose_);
00132       }
00133 
00134       /** \brief Set the horizontal field of view for the camera in degrees
00135         * \param[in] hfov the field of view
00136         */
00137       void 
00138       setHorizontalFOV (float hfov)
00139       {
00140         hfov_ = hfov;
00141       }
00142 
00143       /** \brief Get the horizontal field of view for the camera in degrees */
00144       float 
00145       getHorizontalFOV () const
00146       {
00147         return (hfov_);
00148       }
00149 
00150       /** \brief Set the vertical field of view for the camera in degrees
00151         * \param[in] vfov the field of view
00152         */
00153       void 
00154       setVerticalFOV (float vfov)
00155       {
00156         vfov_ = vfov;
00157       }
00158 
00159       /** \brief Get the vertical field of view for the camera in degrees */
00160       float 
00161       getVerticalFOV () const
00162       {
00163         return (vfov_);
00164       }
00165 
00166       /** \brief Set the near plane distance
00167         * \param[in] np_dist the near plane distance
00168         */
00169       void 
00170       setNearPlaneDistance (float np_dist)
00171       {
00172         np_dist_ = np_dist;
00173       }
00174 
00175       /** \brief Get the near plane distance. */
00176       float
00177       getNearPlaneDistance () const
00178       {
00179         return (np_dist_);
00180       }
00181 
00182       /** \brief Set the far plane distance
00183         * \param[in] fp_dist the far plane distance
00184         */
00185       void 
00186       setFarPlaneDistance (float fp_dist)
00187       {
00188         fp_dist_ = fp_dist;
00189       }
00190 
00191       /** \brief Get the far plane distance */
00192       float 
00193       getFarPlaneDistance () const
00194       {
00195         return (fp_dist_);
00196       }
00197 
00198     protected:
00199       using PCLBase<PointT>::input_;
00200       using PCLBase<PointT>::indices_;
00201       using Filter<PointT>::filter_name_;
00202       using FilterIndices<PointT>::negative_;
00203       using FilterIndices<PointT>::keep_organized_;
00204       using FilterIndices<PointT>::user_filter_value_;
00205       using FilterIndices<PointT>::extract_removed_indices_;
00206       using FilterIndices<PointT>::removed_indices_;
00207 
00208       /** \brief Sample of point indices into a separate PointCloud
00209         * \param[out] output the resultant point cloud
00210         */
00211       void
00212       applyFilter (PointCloud &output);
00213 
00214       /** \brief Sample of point indices
00215         * \param[out] indices the resultant point cloud indices
00216         */
00217       void
00218       applyFilter (std::vector<int> &indices);
00219 
00220     private:
00221 
00222       /** \brief The camera pose */
00223       Eigen::Matrix4f camera_pose_;
00224       /** \brief Horizontal field of view */
00225       float hfov_;
00226       /** \brief Vertical field of view */
00227       float vfov_;
00228       /** \brief Near plane distance */
00229       float np_dist_;
00230       /** \brief Far plane distance */
00231       float fp_dist_;
00232 
00233     public:
00234       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00235   };
00236 }
00237 
00238 #endif