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