Point Cloud Library (PCL)  1.12.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/memory.h>
41 #include <pcl/pcl_config.h> // for PCL_NO_PRECOMPILE
42 #include <pcl/point_types.h>
43 #include <pcl/filters/filter_indices.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  using PointCloud = typename Filter<PointT>::PointCloud;
79  using PointCloudPtr = typename PointCloud::Ptr;
81 
82  public:
83 
84  using Ptr = shared_ptr<FrustumCulling<PointT> >;
85  using ConstPtr = shared_ptr<const FrustumCulling<PointT> >;
86 
87 
89 
90  FrustumCulling (bool extract_removed_indices = false)
91  : FilterIndices<PointT> (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  , roi_x_ (0.5f)
98  , roi_y_ (0.5f)
99  , roi_w_ (1.0f)
100  , roi_h_ (1.0f)
101  {
102  filter_name_ = "FrustumCulling";
103  }
104 
105  /** \brief Set the pose of the camera w.r.t the origin
106  * \param[in] camera_pose the camera pose
107  *
108  * Note: This assumes a coordinate system where X is forward,
109  * Y is up, and Z is right. To convert from the traditional camera
110  * coordinate system (X right, Y down, Z forward), one can use:
111  *
112  * \code
113  * Eigen::Matrix4f pose_orig = //pose in camera coordinates
114  * Eigen::Matrix4f cam2robot;
115  * cam2robot << 0, 0, 1, 0
116  * 0,-1, 0, 0
117  * 1, 0, 0, 0
118  * 0, 0, 0, 1;
119  * Eigen::Matrix4f pose_new = pose_orig * cam2robot;
120  * fc.setCameraPose (pose_new);
121  * \endcode
122  */
123  void
124  setCameraPose (const Eigen::Matrix4f& camera_pose)
125  {
126  camera_pose_ = camera_pose;
127  }
128 
129  /** \brief Get the pose of the camera w.r.t the origin */
130  Eigen::Matrix4f
131  getCameraPose () const
132  {
133  return (camera_pose_);
134  }
135 
136  /** \brief Set the horizontal field of view for the camera in degrees
137  * \param[in] hfov the field of view
138  */
139  void
140  setHorizontalFOV (float hfov)
141  {
142  hfov_ = hfov;
143  }
144 
145  /** \brief Get the horizontal field of view for the camera in degrees */
146  float
148  {
149  return (hfov_);
150  }
151 
152  /** \brief Set the vertical field of view for the camera in degrees
153  * \param[in] vfov the field of view
154  */
155  void
156  setVerticalFOV (float vfov)
157  {
158  vfov_ = vfov;
159  }
160 
161  /** \brief Get the vertical field of view for the camera in degrees */
162  float
163  getVerticalFOV () const
164  {
165  return (vfov_);
166  }
167 
168  /** \brief Set the near plane distance
169  * \param[in] np_dist the near plane distance
170  */
171  void
172  setNearPlaneDistance (float np_dist)
173  {
174  np_dist_ = np_dist;
175  }
176 
177  /** \brief Get the near plane distance. */
178  float
180  {
181  return (np_dist_);
182  }
183 
184  /** \brief Set the far plane distance
185  * \param[in] fp_dist the far plane distance
186  */
187  void
188  setFarPlaneDistance (float fp_dist)
189  {
190  fp_dist_ = fp_dist;
191  }
192 
193  /** \brief Get the far plane distance */
194  float
196  {
197  return (fp_dist_);
198  }
199 
200  /** \brief Set the region of interest (ROI) in normalized values
201  *
202  * Default value of ROI: roi_{x, y} = 0.5, roi_{w, h} = 1.0
203  * This corresponds to maximal FoV and returns all the points in the frustum
204  * Can be used to cut out objects based on 2D bounding boxes by object detection.
205  *
206  * \param[in] roi_x X center position of ROI
207  * \param[in] roi_y Y center position of ROI
208  * \param[in] roi_w Width of ROI
209  * \param[in] roi_h Height of ROI
210  */
211  void
212  setRegionOfInterest (float roi_x, float roi_y, float roi_w, float roi_h)
213  {
214  if ((roi_x > 1.0f) || (roi_x < 0.0f) ||
215  (roi_y > 1.0f) || (roi_y < 0.0f) ||
216  (roi_w <= 0.0f) || (roi_w > 1.0f) ||
217  (roi_h <= 0.0f) || (roi_h > 1.0f))
218  {
219  throw PCLException ("ROI X-Y values should be between 0 and 1. "
220  "Width and height must not be zero.",
221  "frustum_culling.h", "setRegionOfInterest");
222  }
223  roi_x_ = roi_x;
224  roi_y_ = roi_y;
225  roi_w_ = roi_w;
226  roi_h_ = roi_h;
227  }
228 
229  /** \brief Get the region of interest (ROI) in normalized values
230  * \param[in] roi_x X center position of ROI
231  * \param[in] roi_y Y center position of ROI
232  * \param[in] roi_w Width of ROI
233  * \param[in] roi_h Height of ROI
234  */
235  void
236  getRegionOfInterest (float &roi_x, float &roi_y, float &roi_w, float &roi_h) const
237  {
238  roi_x = roi_x_;
239  roi_y = roi_y_;
240  roi_w = roi_w_;
241  roi_h = roi_h_;
242  }
243 
244  protected:
253 
254  /** \brief Sample of point indices
255  * \param[out] indices the resultant point cloud indices
256  */
257  void
258  applyFilter (Indices &indices) override;
259 
260  private:
261 
262  /** \brief The camera pose */
263  Eigen::Matrix4f camera_pose_;
264  /** \brief Horizontal field of view */
265  float hfov_;
266  /** \brief Vertical field of view */
267  float vfov_;
268  /** \brief Near plane distance */
269  float np_dist_;
270  /** \brief Far plane distance */
271  float fp_dist_;
272  /** \brief Region of interest x center position (normalized)*/
273  float roi_x_;
274  /** \brief Region of interest y center position (normalized)*/
275  float roi_y_;
276  /** \brief Region of interest width (normalized)*/
277  float roi_w_;
278  /** \brief Region of interest height (normalized)*/
279  float roi_h_;
280 
281  public:
283  };
284 }
285 
286 #ifdef PCL_NO_PRECOMPILE
287 #include <pcl/filters/impl/frustum_culling.hpp>
288 #endif
Filter represents the base filter class.
Definition: filter.h:81
shared_ptr< Filter< PointT > > Ptr
Definition: filter.h:83
shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:84
std::string filter_name_
The filter name.
Definition: filter.h:158
FilterIndices represents the base class for filters that are about binary point removal.
FrustumCulling filters points inside a frustum given by pose and field of view of the camera.
float getVerticalFOV() const
Get the vertical field of view for the camera in degrees.
float getNearPlaneDistance() const
Get the near plane distance.
float getHorizontalFOV() const
Get the horizontal field of view for the camera in degrees.
void setNearPlaneDistance(float np_dist)
Set the near plane distance.
void setRegionOfInterest(float roi_x, float roi_y, float roi_w, float roi_h)
Set the region of interest (ROI) in normalized values.
void setVerticalFOV(float vfov)
Set the vertical field of view for the camera in degrees.
void setFarPlaneDistance(float fp_dist)
Set the far plane distance.
void setHorizontalFOV(float hfov)
Set the horizontal field of view for the camera in degrees.
FrustumCulling(bool extract_removed_indices=false)
float getFarPlaneDistance() const
Get the far plane distance.
Eigen::Matrix4f getCameraPose() const
Get the pose of the camera w.r.t the origin.
void setCameraPose(const Eigen::Matrix4f &camera_pose)
Set the pose of the camera w.r.t the origin.
void getRegionOfInterest(float &roi_x, float &roi_y, float &roi_w, float &roi_h) const
Get the region of interest (ROI) in normalized values.
void applyFilter(Indices &indices) override
Sample of point indices.
PCL base class.
Definition: pcl_base.h:70
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Defines all the PCL implemented PointT point type structures.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
Definition: bfgs.h:10
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.