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