Point Cloud Library (PCL)  1.8.0
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.
PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:73
float getVerticalFOV() const
Get the vertical field of view for the camera in degrees.
float getHorizontalFOV() const
Get the horizontal field of view for the camera in degrees.
float getFarPlaneDistance() const
Get the far plane distance.
void setFarPlaneDistance(float fp_dist)
Set the far plane distance.
Definition: bfgs.h:10
void applyFilter(PointCloud &output)
Sample of point indices into a separate PointCloud.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
FilterIndices represents the base class for filters that are about binary point removal.
Eigen::Matrix4f getCameraPose() const
Get the pose of the camera w.r.t the origin.
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
Defines all the PCL implemented PointT point type structures.
PCL base class.
Definition: pcl_base.h:68
void setNearPlaneDistance(float np_dist)
Set the near plane distance.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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)
std::string filter_name_
The filter name.
Definition: filter.h:166
A point structure representing Euclidean xyz coordinates, and the RGB color.
float getNearPlaneDistance() const
Get the near plane distance.