Point Cloud Library (PCL)  1.14.1
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  {
94  filter_name_ = "FrustumCulling";
95  }
96 
97  /** \brief Set the pose of the camera w.r.t the origin
98  * \param[in] camera_pose the camera pose
99  *
100  * Note: This assumes a coordinate system where X is forward,
101  * Y is up, and Z is right. To convert from the traditional camera
102  * coordinate system (X right, Y down, Z forward), one can use:
103  *
104  * \code
105  * Eigen::Matrix4f pose_orig = //pose in camera coordinates
106  * Eigen::Matrix4f cam2robot;
107  * cam2robot << 0, 0, 1, 0
108  * 0,-1, 0, 0
109  * 1, 0, 0, 0
110  * 0, 0, 0, 1;
111  * Eigen::Matrix4f pose_new = pose_orig * cam2robot;
112  * fc.setCameraPose (pose_new);
113  * \endcode
114  */
115  void
116  setCameraPose (const Eigen::Matrix4f& camera_pose)
117  {
118  camera_pose_ = camera_pose;
119  }
120 
121  /** \brief Get the pose of the camera w.r.t the origin */
122  Eigen::Matrix4f
123  getCameraPose () const
124  {
125  return (camera_pose_);
126  }
127 
128  /** \brief Set the horizontal field of view for the camera in degrees
129  * \param[in] hfov the field of view
130  * Note: setHorizontalFOV(60.0) is equivalent to setHorizontalFOV(-30.0, 30.0).
131  */
132  void
133  setHorizontalFOV (float hfov)
134  {
135  if (hfov <= 0 || hfov >= 180)
136  {
137  throw PCLException ("Horizontal field of view should be between 0 and 180(excluded).",
138  "frustum_culling.h", "setHorizontalFOV");
139  }
140  fov_left_bound_ = -hfov / 2;
141  fov_right_bound_ = hfov / 2;
142  }
143 
144  /** \brief Set the horizontal field of view for the camera in degrees
145  * \param[in] fov_left_bound the left bound of horizontal field of view
146  * \param[in] fov_right_bound the right bound of horizontal field of view
147  * Note: Bounds can be either positive or negative values.
148  * Negative value means the camera would look to its left,
149  * and positive value means the camera would look to its right.
150  * In general cases, fov_left_bound should be set to a negative value,
151  * if it is set to a positive value, the camera would only look to its right.
152  * Also note that setHorizontalFOV(-30.0, 30.0) is equivalent to setHorizontalFOV(60.0).
153  */
154  void
155  setHorizontalFOV (float fov_left_bound, float fov_right_bound)
156  {
157  if (fov_left_bound <= -90 || fov_right_bound >= 90 || fov_left_bound >= fov_right_bound)
158  {
159  throw PCLException ("Horizontal field of view bounds should be between -90 and 90(excluded). "
160  "And left bound should be smaller than right bound.",
161  "frustum_culling.h", "setHorizontalFOV");
162  }
163  fov_left_bound_ = fov_left_bound;
164  fov_right_bound_ = fov_right_bound;
165  }
166 
167  /** \brief Get the horizontal field of view for the camera in degrees */
168  float
170  {
171  if (std::fabs(fov_right_bound_) != std::fabs(fov_left_bound_)) {
172  PCL_WARN("Your horizontal field of view is asymmetrical: "
173  "left bound's absolute value(%f) != right bound's absolute value(%f)! "
174  "Please use getHorizontalFOV (float& fov_left_bound, float& fov_right_bound) instead.\n",
175  std::fabs(fov_left_bound_), std::fabs(fov_right_bound_));
176  }
177  return (fov_right_bound_ - fov_left_bound_);
178  }
179 
180  /** \brief Get the horizontal field of view for the camera in degrees */
181  void
182  getHorizontalFOV (float& fov_left_bound, float& fov_right_bound) const
183  {
184  fov_left_bound = fov_left_bound_;
185  fov_right_bound = fov_right_bound_;
186  }
187 
188  /** \brief Set the vertical field of view for the camera in degrees
189  * \param[in] vfov the field of view
190  * Note: setVerticalFOV(60.0) is equivalent to setVerticalFOV(-30.0, 30.0).
191  */
192  void
193  setVerticalFOV (float vfov)
194  {
195  if (vfov <= 0 || vfov >= 180)
196  {
197  throw PCLException ("Vertical field of view should be between 0 and 180(excluded).",
198  "frustum_culling.h", "setVerticalFOV");
199  }
200  fov_lower_bound_ = -vfov / 2;
201  fov_upper_bound_ = vfov / 2;
202  }
203 
204  /** \brief Set the vertical field of view for the camera in degrees
205  * \param[in] fov_lower_bound the lower bound of vertical field of view
206  * \param[in] fov_upper_bound the upper bound of vertical field of view
207  * Note: Bounds can be either positive or negative values.
208  * Negative value means the camera would look down,
209  * and positive value means the camera would look up.
210  * In general cases, fov_lower_bound should be set to a negative value,
211  * if it is set to a positive value, the camera would only look up.
212  * Also note that setVerticalFOV(-30.0, 30.0) is equivalent to setVerticalFOV(60.0).
213  */
214  void
215  setVerticalFOV (float fov_lower_bound, float fov_upper_bound)
216  {
217  if (fov_lower_bound <= -90 || fov_upper_bound >= 90 || fov_lower_bound >= fov_upper_bound)
218  {
219  throw PCLException ("Vertical field of view bounds should be between -90 and 90(excluded). "
220  "And lower bound should be smaller than upper bound.",
221  "frustum_culling.h", "setVerticalFOV");
222  }
223  fov_lower_bound_ = fov_lower_bound;
224  fov_upper_bound_ = fov_upper_bound;
225  }
226 
227  /** \brief Get the vertical field of view for the camera in degrees */
228  float
230  {
231  if (std::fabs(fov_upper_bound_) != std::fabs(fov_lower_bound_)) {
232  PCL_WARN("Your vertical field of view is asymmetrical: "
233  "lower bound's absolute value(%f) != upper bound's absolute value(%f)! "
234  "Please use getVerticalFOV (float& fov_lower_bound, float& fov_upper_bound) instead.\n",
235  std::fabs(fov_lower_bound_), std::fabs(fov_upper_bound_));
236  }
237  return (fov_upper_bound_ - fov_lower_bound_);
238  }
239 
240  /** \brief Get the vertical field of view for the camera in degrees */
241  void
242  getVerticalFOV (float& fov_lower_bound, float& fov_upper_bound) const
243  {
244  fov_lower_bound = fov_lower_bound_;
245  fov_upper_bound = fov_upper_bound_;
246  }
247 
248  /** \brief Set the near plane distance
249  * \param[in] np_dist the near plane distance. You can set this to 0 to disable near-plane filtering and extract a rectangular pyramid instead of a frustum.
250  */
251  void
252  setNearPlaneDistance (float np_dist)
253  {
254  if (np_dist < 0.0f)
255  {
256  throw PCLException ("Near plane distance should be greater than or equal to 0.",
257  "frustum_culling.h", "setNearPlaneDistance");
258  }
259  np_dist_ = np_dist;
260  }
261 
262  /** \brief Get the near plane distance. */
263  float
265  {
266  return (np_dist_);
267  }
268 
269  /** \brief Set the far plane distance
270  * \param[in] fp_dist the far plane distance.
271  * You can set this to std::numeric_limits<float>::max(), then points will not be filtered by the far plane.
272  */
273  void
274  setFarPlaneDistance (float fp_dist)
275  {
276  if (fp_dist <= 0.0f)
277  {
278  throw PCLException ("Far plane distance should be greater than 0.",
279  "frustum_culling.h", "setFarPlaneDistance");
280  }
281  fp_dist_ = fp_dist;
282  }
283 
284  /** \brief Get the far plane distance */
285  float
287  {
288  return (fp_dist_);
289  }
290 
291  /** \brief Set the region of interest (ROI) in normalized values
292  *
293  * Default value of ROI: roi_{x, y} = 0.5, roi_{w, h} = 1.0
294  * This corresponds to maximal FoV and returns all the points in the frustum
295  * Can be used to cut out objects based on 2D bounding boxes by object detection.
296  *
297  * \param[in] roi_x X center position of ROI
298  * \param[in] roi_y Y center position of ROI
299  * \param[in] roi_w Width of ROI
300  * \param[in] roi_h Height of ROI
301  */
302  void
303  setRegionOfInterest (float roi_x, float roi_y, float roi_w, float roi_h)
304  {
305  if ((roi_x > 1.0f) || (roi_x < 0.0f) ||
306  (roi_y > 1.0f) || (roi_y < 0.0f) ||
307  (roi_w <= 0.0f) || (roi_w > 1.0f) ||
308  (roi_h <= 0.0f) || (roi_h > 1.0f))
309  {
310  throw PCLException ("ROI X-Y values should be between 0 and 1. "
311  "Width and height must not be zero.",
312  "frustum_culling.h", "setRegionOfInterest");
313  }
314  roi_x_ = roi_x;
315  roi_y_ = roi_y;
316  roi_w_ = roi_w;
317  roi_h_ = roi_h;
318  }
319 
320  /** \brief Get the region of interest (ROI) in normalized values
321  * \param[in] roi_x X center position of ROI
322  * \param[in] roi_y Y center position of ROI
323  * \param[in] roi_w Width of ROI
324  * \param[in] roi_h Height of ROI
325  */
326  void
327  getRegionOfInterest (float &roi_x, float &roi_y, float &roi_w, float &roi_h) const
328  {
329  roi_x = roi_x_;
330  roi_y = roi_y_;
331  roi_w = roi_w_;
332  roi_h = roi_h_;
333  }
334 
335  protected:
344 
345  /** \brief Sample of point indices
346  * \param[out] indices the resultant point cloud indices
347  */
348  void
349  applyFilter (Indices &indices) override;
350 
351  private:
352 
353  /** \brief The camera pose */
354  Eigen::Matrix4f camera_pose_;
355  /** \brief The left bound of horizontal field of view */
356  float fov_left_bound_{-30.0f};
357  /** \brief The right bound of horizontal field of view */
358  float fov_right_bound_{30.0f};
359  /** \brief The lower bound of vertical field of view */
360  float fov_lower_bound_{-30.0f};
361  /** \brief The upper bound of vertical field of view */
362  float fov_upper_bound_{30.0f};
363  /** \brief Near plane distance */
364  float np_dist_{0.1f};
365  /** \brief Far plane distance */
366  float fp_dist_{5.0f};
367  /** \brief Region of interest x center position (normalized)*/
368  float roi_x_{0.5f};
369  /** \brief Region of interest y center position (normalized)*/
370  float roi_y_{0.5f};
371  /** \brief Region of interest width (normalized)*/
372  float roi_w_{1.0f};
373  /** \brief Region of interest height (normalized)*/
374  float roi_h_{1.0f};
375 
376  public:
378  };
379 }
380 
381 #ifdef PCL_NO_PRECOMPILE
382 #include <pcl/filters/impl/frustum_culling.hpp>
383 #endif
void setVerticalFOV(float vfov)
Set the vertical field of view for the camera in degrees.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:65
float getNearPlaneDistance() const
Get the near plane distance.
void getVerticalFOV(float &fov_lower_bound, float &fov_upper_bound) const
Get the vertical field of view for the camera in degrees.
void getHorizontalFOV(float &fov_left_bound, float &fov_right_bound) const
Get the horizontal field of view for the camera in degrees.
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: memory.h:63
Definition: bfgs.h:10
float getVerticalFOV() const
Get the vertical field of view for the camera in degrees.
void setHorizontalFOV(float fov_left_bound, float fov_right_bound)
Set the horizontal 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:80
shared_ptr< Filter< PointT > > Ptr
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
void setRegionOfInterest(float roi_x, float roi_y, float roi_w, float roi_h)
Set the region of interest (ROI) in normalized values.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: distances.h:55
void applyFilter(Indices &indices) override
Sample of point indices.
void getRegionOfInterest(float &roi_x, float &roi_y, float &roi_w, float &roi_h) const
Get the region of interest (ROI) in normalized values.
FrustumCulling filters points inside a frustum given by pose and field of view of the camera...
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
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:158
shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:84
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.
void setVerticalFOV(float fov_lower_bound, float fov_upper_bound)
Set the vertical field of view for the camera in degrees.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
float getFarPlaneDistance() const
Get the far plane distance.