Point Cloud Library (PCL) 1.12.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 , hfov_ (60.0f)
94 , vfov_ (60.0f)
95 , np_dist_ (0.1f)
96 , fp_dist_ (5.0f)
97 {
98 filter_name_ = "FrustumCulling";
99 }
100
101 /** \brief Set the pose of the camera w.r.t the origin
102 * \param[in] camera_pose the camera pose
103 *
104 * Note: This assumes a coordinate system where X is forward,
105 * Y is up, and Z is right. To convert from the traditional camera
106 * coordinate system (X right, Y down, Z forward), one can use:
107 *
108 * \code
109 * Eigen::Matrix4f pose_orig = //pose in camera coordinates
110 * Eigen::Matrix4f cam2robot;
111 * cam2robot << 0, 0, 1, 0
112 * 0,-1, 0, 0
113 * 1, 0, 0, 0
114 * 0, 0, 0, 1;
115 * Eigen::Matrix4f pose_new = pose_orig * cam2robot;
116 * fc.setCameraPose (pose_new);
117 * \endcode
118 */
119 void
120 setCameraPose (const Eigen::Matrix4f& camera_pose)
121 {
122 camera_pose_ = camera_pose;
123 }
124
125 /** \brief Get the pose of the camera w.r.t the origin */
126 Eigen::Matrix4f
128 {
129 return (camera_pose_);
130 }
131
132 /** \brief Set the horizontal field of view for the camera in degrees
133 * \param[in] hfov the field of view
134 */
135 void
136 setHorizontalFOV (float hfov)
137 {
138 hfov_ = hfov;
139 }
140
141 /** \brief Get the horizontal field of view for the camera in degrees */
142 float
144 {
145 return (hfov_);
146 }
147
148 /** \brief Set the vertical field of view for the camera in degrees
149 * \param[in] vfov the field of view
150 */
151 void
152 setVerticalFOV (float vfov)
153 {
154 vfov_ = vfov;
155 }
156
157 /** \brief Get the vertical field of view for the camera in degrees */
158 float
160 {
161 return (vfov_);
162 }
163
164 /** \brief Set the near plane distance
165 * \param[in] np_dist the near plane distance
166 */
167 void
168 setNearPlaneDistance (float np_dist)
169 {
170 np_dist_ = np_dist;
171 }
172
173 /** \brief Get the near plane distance. */
174 float
176 {
177 return (np_dist_);
178 }
179
180 /** \brief Set the far plane distance
181 * \param[in] fp_dist the far plane distance
182 */
183 void
184 setFarPlaneDistance (float fp_dist)
185 {
186 fp_dist_ = fp_dist;
187 }
188
189 /** \brief Get the far plane distance */
190 float
192 {
193 return (fp_dist_);
194 }
195
196 protected:
197 using PCLBase<PointT>::input_;
205
206 /** \brief Sample of point indices
207 * \param[out] indices the resultant point cloud indices
208 */
209 void
210 applyFilter (Indices &indices) override;
211
212 private:
213
214 /** \brief The camera pose */
215 Eigen::Matrix4f camera_pose_;
216 /** \brief Horizontal field of view */
217 float hfov_;
218 /** \brief Vertical field of view */
219 float vfov_;
220 /** \brief Near plane distance */
221 float np_dist_;
222 /** \brief Far plane distance */
223 float fp_dist_;
224
225 public:
227 };
228}
229
230#ifdef PCL_NO_PRECOMPILE
231#include <pcl/filters/impl/frustum_culling.hpp>
232#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 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)
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 setCameraPose(const Eigen::Matrix4f &camera_pose)
Set the pose of the camera w.r.t the origin.
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
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.