Point Cloud Library (PCL)  1.8.1-dev
frustum_culling.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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  */
37 
38 
39 #ifndef PCL_FILTERS_FRUSTUM_CULLING_H_
40 #define PCL_FILTERS_FRUSTUM_CULLING_H_
41 
42 #include <pcl/point_types.h>
43 #include <pcl/filters/filter_indices.h>
44 #include <pcl/common/transforms.h>
45 #include <pcl/common/eigen.h>
46 
47 namespace pcl
48 {
49  /** \brief FrustumCulling filters points inside a frustum
50  * given by pose and field of view of the camera.
51  *
52  * Code example:
53  *
54  * \code
55  * pcl::PointCloud <pcl::PointXYZ>::Ptr source;
56  * // .. read or fill the source cloud
57  *
58  * pcl::FrustumCulling<pcl::PointXYZ> fc;
59  * fc.setInputCloud (source);
60  * fc.setVerticalFOV (45);
61  * fc.setHorizontalFOV (60);
62  * fc.setNearPlaneDistance (5.0);
63  * fc.setFarPlaneDistance (15);
64  *
65  * Eigen::Matrix4f camera_pose;
66  * // .. read or input the camera pose from a registration algorithm.
67  * fc.setCameraPose (camera_pose);
68  *
69  * pcl::PointCloud <pcl::PointXYZ> target;
70  * fc.filter (target);
71  * \endcode
72  *
73  *
74  * \author Aravindhan K Krishnan
75  * \ingroup filters
76  */
77  template <typename PointT>
78  class FrustumCulling : public FilterIndices<PointT>
79  {
80  typedef typename Filter<PointT>::PointCloud PointCloud;
81  typedef typename PointCloud::Ptr PointCloudPtr;
83 
84  public:
85 
86  typedef boost::shared_ptr< FrustumCulling<PointT> > Ptr;
87  typedef boost::shared_ptr< const FrustumCulling<PointT> > ConstPtr;
88 
89 
91 
92  FrustumCulling (bool extract_removed_indices = false)
93  : FilterIndices<PointT>::FilterIndices (extract_removed_indices)
94  , camera_pose_ (Eigen::Matrix4f::Identity ())
95  , hfov_ (60.0f)
96  , vfov_ (60.0f)
97  , np_dist_ (0.1f)
98  , fp_dist_ (5.0f)
99  {
100  filter_name_ = "FrustumCulling";
101  }
102 
103  /** \brief Set the pose of the camera w.r.t the origin
104  * \param[in] camera_pose the camera pose
105  *
106  * Note: This assumes a coordinate system where X is forward,
107  * Y is up, and Z is right. To convert from the traditional camera
108  * coordinate system (X right, Y down, Z forward), one can use:
109  *
110  * \code
111  * Eigen::Matrix4f pose_orig = //pose in camera coordinates
112  * Eigen::Matrix4f cam2robot;
113  * cam2robot << 0, 0, 1, 0
114  * 0,-1, 0, 0
115  * 1, 0, 0, 0
116  * 0, 0, 0, 1;
117  * Eigen::Matrix4f pose_new = pose_orig * cam2robot;
118  * fc.setCameraPose (pose_new);
119  * \endcode
120  */
121  void
122  setCameraPose (const Eigen::Matrix4f& camera_pose)
123  {
124  camera_pose_ = camera_pose;
125  }
126 
127  /** \brief Get the pose of the camera w.r.t the origin */
128  Eigen::Matrix4f
129  getCameraPose () const
130  {
131  return (camera_pose_);
132  }
133 
134  /** \brief Set the horizontal field of view for the camera in degrees
135  * \param[in] hfov the field of view
136  */
137  void
138  setHorizontalFOV (float hfov)
139  {
140  hfov_ = hfov;
141  }
142 
143  /** \brief Get the horizontal field of view for the camera in degrees */
144  float
146  {
147  return (hfov_);
148  }
149 
150  /** \brief Set the vertical field of view for the camera in degrees
151  * \param[in] vfov the field of view
152  */
153  void
154  setVerticalFOV (float vfov)
155  {
156  vfov_ = vfov;
157  }
158 
159  /** \brief Get the vertical field of view for the camera in degrees */
160  float
161  getVerticalFOV () const
162  {
163  return (vfov_);
164  }
165 
166  /** \brief Set the near plane distance
167  * \param[in] np_dist the near plane distance
168  */
169  void
170  setNearPlaneDistance (float np_dist)
171  {
172  np_dist_ = np_dist;
173  }
174 
175  /** \brief Get the near plane distance. */
176  float
178  {
179  return (np_dist_);
180  }
181 
182  /** \brief Set the far plane distance
183  * \param[in] fp_dist the far plane distance
184  */
185  void
186  setFarPlaneDistance (float fp_dist)
187  {
188  fp_dist_ = fp_dist;
189  }
190 
191  /** \brief Get the far plane distance */
192  float
194  {
195  return (fp_dist_);
196  }
197 
198  protected:
207 
208  /** \brief Sample of point indices into a separate PointCloud
209  * \param[out] output the resultant point cloud
210  */
211  void
212  applyFilter (PointCloud &output);
213 
214  /** \brief Sample of point indices
215  * \param[out] indices the resultant point cloud indices
216  */
217  void
218  applyFilter (std::vector<int> &indices);
219 
220  private:
221 
222  /** \brief The camera pose */
223  Eigen::Matrix4f camera_pose_;
224  /** \brief Horizontal field of view */
225  float hfov_;
226  /** \brief Vertical field of view */
227  float vfov_;
228  /** \brief Near plane distance */
229  float np_dist_;
230  /** \brief Far plane distance */
231  float fp_dist_;
232 
233  public:
234  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
235  };
236 }
237 
238 #ifdef PCL_NO_PRECOMPILE
239 #include <pcl/filters/impl/frustum_culling.hpp>
240 #endif
241 
242 #endif
void setVerticalFOV(float vfov)
Set the vertical field of view for the camera in degrees.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:73
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
float getNearPlaneDistance() const
Get the near plane distance.
void setFarPlaneDistance(float fp_dist)
Set the far plane distance.
float getVerticalFOV() const
Get the vertical field of view for the camera in degrees.
void applyFilter(PointCloud &output)
Sample of point indices into a separate PointCloud.
FilterIndices represents the base class for filters that are about binary point removal.
PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:72
void setCameraPose(const Eigen::Matrix4f &camera_pose)
Set the pose of the camera w.r.t the origin.
Filter represents the base filter class.
Definition: filter.h:84
PCL base class.
Definition: pcl_base.h:68
void setNearPlaneDistance(float np_dist)
Set the near plane distance.
boost::shared_ptr< const FrustumCulling< PointT > > ConstPtr
FrustumCulling filters points inside a frustum given by pose and field of view of the camera...
boost::shared_ptr< FrustumCulling< PointT > > Ptr
void setHorizontalFOV(float hfov)
Set the horizontal field of view for the camera in degrees.
FrustumCulling(bool extract_removed_indices=false)
float getHorizontalFOV() const
Get the horizontal field of view for the camera in degrees.
std::string filter_name_
The filter name.
Definition: filter.h:166
A point structure representing Euclidean xyz coordinates, and the RGB color.
Eigen::Matrix4f getCameraPose() const
Get the pose of the camera w.r.t the origin.
float getFarPlaneDistance() const
Get the far plane distance.