Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
45namespace 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;
80 using PointCloudConstPtr = typename PointCloud::ConstPtr;
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
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:
336 using PCLBase<PointT>::input_;
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
Filter represents the base filter class.
Definition filter.h:81
bool extract_removed_indices_
Set to true if we want to return the indices of the removed points.
Definition filter.h:161
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition filter.h:174
std::string filter_name_
The filter name.
Definition filter.h:158
IndicesPtr removed_indices_
Indices of the points that are removed.
Definition filter.h:155
FilterIndices represents the base class for filters that are about binary point removal.
float user_filter_value_
The user given value that the filtered point dimensions should be set to (default = NaN).
bool keep_organized_
False = remove points (default), true = redefine points, keep structure.
bool negative_
False = normal filter behavior (default), true = inverted behavior.
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.
shared_ptr< const FrustumCulling< PointT > > ConstPtr
float getFarPlaneDistance() const
Get the far plane distance.
Eigen::Matrix4f getCameraPose() const
Get the pose of the camera w.r.t the origin.
shared_ptr< FrustumCulling< PointT > > Ptr
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
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
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.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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.