Point Cloud Library (PCL)  1.11.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_macros.h>
42 #include <pcl/point_types.h>
43 #include <pcl/filters/filter_indices.h>
44 #include <pcl/common/transforms.h>
45 #include <pcl/common/eigen.h>
46 
47 namespace pcl
48 {
49  /** \brief FrustumCulling filters points inside a frustum
50  * given by pose and field of view of the camera.
51  *
52  * Code example:
53  *
54  * \code
55  * pcl::PointCloud <pcl::PointXYZ>::Ptr source;
56  * // .. read or fill the source cloud
57  *
58  * pcl::FrustumCulling<pcl::PointXYZ> fc;
59  * fc.setInputCloud (source);
60  * fc.setVerticalFOV (45);
61  * fc.setHorizontalFOV (60);
62  * fc.setNearPlaneDistance (5.0);
63  * fc.setFarPlaneDistance (15);
64  *
65  * Eigen::Matrix4f camera_pose;
66  * // .. read or input the camera pose from a registration algorithm.
67  * fc.setCameraPose (camera_pose);
68  *
69  * pcl::PointCloud <pcl::PointXYZ> target;
70  * fc.filter (target);
71  * \endcode
72  *
73  *
74  * \author Aravindhan K Krishnan
75  * \ingroup filters
76  */
77  template <typename PointT>
78  class FrustumCulling : public FilterIndices<PointT>
79  {
80  using PointCloud = typename Filter<PointT>::PointCloud;
81  using PointCloudPtr = typename PointCloud::Ptr;
83 
84  public:
85 
86  using Ptr = shared_ptr<FrustumCulling<PointT> >;
87  using ConstPtr = shared_ptr<const FrustumCulling<PointT> >;
88 
89 
91 
92  FrustumCulling (bool extract_removed_indices = false)
93  : FilterIndices<PointT> (extract_removed_indices)
94  , camera_pose_ (Eigen::Matrix4f::Identity ())
95  , hfov_ (60.0f)
96  , vfov_ (60.0f)
97  , np_dist_ (0.1f)
98  , fp_dist_ (5.0f)
99  {
100  filter_name_ = "FrustumCulling";
101  }
102 
103  /** \brief Set the pose of the camera w.r.t the origin
104  * \param[in] camera_pose the camera pose
105  *
106  * Note: This assumes a coordinate system where X is forward,
107  * Y is up, and Z is right. To convert from the traditional camera
108  * coordinate system (X right, Y down, Z forward), one can use:
109  *
110  * \code
111  * Eigen::Matrix4f pose_orig = //pose in camera coordinates
112  * Eigen::Matrix4f cam2robot;
113  * cam2robot << 0, 0, 1, 0
114  * 0,-1, 0, 0
115  * 1, 0, 0, 0
116  * 0, 0, 0, 1;
117  * Eigen::Matrix4f pose_new = pose_orig * cam2robot;
118  * fc.setCameraPose (pose_new);
119  * \endcode
120  */
121  void
122  setCameraPose (const Eigen::Matrix4f& camera_pose)
123  {
124  camera_pose_ = camera_pose;
125  }
126 
127  /** \brief Get the pose of the camera w.r.t the origin */
128  Eigen::Matrix4f
129  getCameraPose () const
130  {
131  return (camera_pose_);
132  }
133 
134  /** \brief Set the horizontal field of view for the camera in degrees
135  * \param[in] hfov the field of view
136  */
137  void
138  setHorizontalFOV (float hfov)
139  {
140  hfov_ = hfov;
141  }
142 
143  /** \brief Get the horizontal field of view for the camera in degrees */
144  float
146  {
147  return (hfov_);
148  }
149 
150  /** \brief Set the vertical field of view for the camera in degrees
151  * \param[in] vfov the field of view
152  */
153  void
154  setVerticalFOV (float vfov)
155  {
156  vfov_ = vfov;
157  }
158 
159  /** \brief Get the vertical field of view for the camera in degrees */
160  float
161  getVerticalFOV () const
162  {
163  return (vfov_);
164  }
165 
166  /** \brief Set the near plane distance
167  * \param[in] np_dist the near plane distance
168  */
169  void
170  setNearPlaneDistance (float np_dist)
171  {
172  np_dist_ = np_dist;
173  }
174 
175  /** \brief Get the near plane distance. */
176  float
178  {
179  return (np_dist_);
180  }
181 
182  /** \brief Set the far plane distance
183  * \param[in] fp_dist the far plane distance
184  */
185  void
186  setFarPlaneDistance (float fp_dist)
187  {
188  fp_dist_ = fp_dist;
189  }
190 
191  /** \brief Get the far plane distance */
192  float
194  {
195  return (fp_dist_);
196  }
197 
198  protected:
207 
208  /** \brief Sample of point indices
209  * \param[out] indices the resultant point cloud indices
210  */
211  void
212  applyFilter (std::vector<int> &indices) override;
213 
214  private:
215 
216  /** \brief The camera pose */
217  Eigen::Matrix4f camera_pose_;
218  /** \brief Horizontal field of view */
219  float hfov_;
220  /** \brief Vertical field of view */
221  float vfov_;
222  /** \brief Near plane distance */
223  float np_dist_;
224  /** \brief Far plane distance */
225  float fp_dist_;
226 
227  public:
229  };
230 }
231 
232 #ifdef PCL_NO_PRECOMPILE
233 #include <pcl/filters/impl/frustum_culling.hpp>
234 #endif
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
Definition: convolution.h:46
point_types.h
pcl::Filter::Ptr
shared_ptr< Filter< PointT > > Ptr
Definition: filter.h:86
pcl::FrustumCulling
FrustumCulling filters points inside a frustum given by pose and field of view of the camera.
Definition: frustum_culling.h:79
Eigen
Definition: bfgs.h:10
pcl::PCLBase::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:77
pcl::PCLBase::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:76
pcl::FrustumCulling::setVerticalFOV
void setVerticalFOV(float vfov)
Set the vertical field of view for the camera in degrees.
Definition: frustum_culling.h:154
pcl::FrustumCulling::setHorizontalFOV
void setHorizontalFOV(float hfov)
Set the horizontal field of view for the camera in degrees.
Definition: frustum_culling.h:138
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:73
pcl::FrustumCulling::setCameraPose
void setCameraPose(const Eigen::Matrix4f &camera_pose)
Set the pose of the camera w.r.t the origin.
Definition: frustum_culling.h:122
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::FrustumCulling::FrustumCulling
FrustumCulling(bool extract_removed_indices=false)
Definition: frustum_culling.h:92
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::FrustumCulling::getNearPlaneDistance
float getNearPlaneDistance() const
Get the near plane distance.
Definition: frustum_culling.h:177
pcl::FrustumCulling::setNearPlaneDistance
void setNearPlaneDistance(float np_dist)
Set the near plane distance.
Definition: frustum_culling.h:170
pcl::FrustumCulling::getCameraPose
Eigen::Matrix4f getCameraPose() const
Get the pose of the camera w.r.t the origin.
Definition: frustum_culling.h:129
pcl::Filter::ConstPtr
shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:87
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
pcl::FrustumCulling::getFarPlaneDistance
float getFarPlaneDistance() const
Get the far plane distance.
Definition: frustum_culling.h:193
pcl::FilterIndices
FilterIndices represents the base class for filters that are about binary point removal.
Definition: filter_indices.h:75
pcl::Filter
Filter represents the base filter class.
Definition: filter.h:84
pcl::FrustumCulling::getHorizontalFOV
float getHorizontalFOV() const
Get the horizontal field of view for the camera in degrees.
Definition: frustum_culling.h:145
pcl::Filter::filter_name_
std::string filter_name_
The filter name.
Definition: filter.h:161
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
pcl::FrustumCulling::getVerticalFOV
float getVerticalFOV() const
Get the vertical field of view for the camera in degrees.
Definition: frustum_culling.h:161
pcl::FrustumCulling::applyFilter
void applyFilter(std::vector< int > &indices) override
Sample of point indices.
Definition: frustum_culling.hpp:46
pcl::FrustumCulling::setFarPlaneDistance
void setFarPlaneDistance(float fp_dist)
Set the far plane distance.
Definition: frustum_culling.h:186
memory.h
Defines functions, macros and traits for allocating and using memory.