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