FrustumCulling filters points inside a frustum given by pose and field of view of the camera. More...
#include <frustum_culling.h>
Public Types | |
typedef boost::shared_ptr < const FrustumCulling< PointT > > | ConstPtr |
typedef boost::shared_ptr < FrustumCulling< PointT > > | Ptr |
Public Member Functions | |
FrustumCulling (bool extract_removed_indices=false) | |
Eigen::Matrix4f | getCameraPose () const |
Get the pose of the camera w.r.t the origin. | |
float | getFarPlaneDistance () const |
Get the far plane distance. | |
float | getHorizontalFOV () const |
Get the horizontal field of view for the camera in degrees. | |
float | getNearPlaneDistance () const |
Get the near plane distance. | |
float | getVerticalFOV () 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 | setFarPlaneDistance (float fp_dist) |
Set the far plane distance. | |
void | setHorizontalFOV (float hfov) |
Set 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. | |
Protected Member Functions | |
void | applyFilter (PointCloud &output) |
Sample of point indices into a separate PointCloud. | |
void | applyFilter (std::vector< int > &indices) |
Sample of point indices. | |
Private Types | |
typedef Filter< PointT > ::PointCloud | PointCloud |
typedef PointCloud::ConstPtr | PointCloudConstPtr |
typedef PointCloud::Ptr | PointCloudPtr |
Private Attributes | |
Eigen::Matrix4f | camera_pose_ |
The camera pose. | |
float | fp_dist_ |
Far plane distance. | |
float | hfov_ |
Horizontal field of view. | |
float | np_dist_ |
Near plane distance. | |
float | vfov_ |
Vertical field of view. |
FrustumCulling filters points inside a frustum given by pose and field of view of the camera.
Code example:
pcl::PointCloud <pcl::PointXYZ>::Ptr source; // .. read or fill the source cloud pcl::FrustumCulling<pcl::PointXYZ> fc; fc.setInputCloud (source); fc.setVerticalFOV (45); fc.setHorizontalFOV (60); fc.setNearPlaneDistance (5.0); fc.setFarPlaneDistance (15); Eigen::Matrix4f camera_pose; // .. read or input the camera pose from a registration algorithm. fc.setCameraPose (camera_pose); pcl::PointCloud <pcl::PointXYZ> target; fc.filter (target);
Definition at line 78 of file frustum_culling.h.
typedef boost::shared_ptr< const FrustumCulling<PointT> > pcl::FrustumCulling< PointT >::ConstPtr |
Reimplemented from pcl::FilterIndices< PointT >.
Definition at line 87 of file frustum_culling.h.
typedef Filter<PointT>::PointCloud pcl::FrustumCulling< PointT >::PointCloud [private] |
Reimplemented from pcl::FilterIndices< PointT >.
Definition at line 80 of file frustum_culling.h.
typedef PointCloud::ConstPtr pcl::FrustumCulling< PointT >::PointCloudConstPtr [private] |
Reimplemented from pcl::Filter< PointT >.
Definition at line 82 of file frustum_culling.h.
typedef PointCloud::Ptr pcl::FrustumCulling< PointT >::PointCloudPtr [private] |
Reimplemented from pcl::Filter< PointT >.
Definition at line 81 of file frustum_culling.h.
typedef boost::shared_ptr< FrustumCulling<PointT> > pcl::FrustumCulling< PointT >::Ptr |
Reimplemented from pcl::FilterIndices< PointT >.
Definition at line 86 of file frustum_culling.h.
pcl::FrustumCulling< PointT >::FrustumCulling | ( | bool | extract_removed_indices = false | ) | [inline] |
Definition at line 92 of file frustum_culling.h.
void pcl::FrustumCulling< PointT >::applyFilter | ( | PointCloud & | output | ) | [protected, virtual] |
Sample of point indices into a separate PointCloud.
[out] | output | the resultant point cloud |
Implements pcl::Filter< PointT >.
Definition at line 47 of file frustum_culling.hpp.
void pcl::FrustumCulling< PointT >::applyFilter | ( | std::vector< int > & | indices | ) | [protected, virtual] |
Sample of point indices.
[out] | indices | the resultant point cloud indices |
Implements pcl::FilterIndices< PointT >.
Definition at line 76 of file frustum_culling.hpp.
Eigen::Matrix4f pcl::FrustumCulling< PointT >::getCameraPose | ( | ) | const [inline] |
Get the pose of the camera w.r.t the origin.
Definition at line 129 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::getFarPlaneDistance | ( | ) | const [inline] |
Get the far plane distance.
Definition at line 193 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::getHorizontalFOV | ( | ) | const [inline] |
Get the horizontal field of view for the camera in degrees.
Definition at line 145 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::getNearPlaneDistance | ( | ) | const [inline] |
Get the near plane distance.
Definition at line 177 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::getVerticalFOV | ( | ) | const [inline] |
Get the vertical field of view for the camera in degrees.
Definition at line 161 of file frustum_culling.h.
void pcl::FrustumCulling< PointT >::setCameraPose | ( | const Eigen::Matrix4f & | camera_pose | ) | [inline] |
Set the pose of the camera w.r.t the origin.
[in] | camera_pose | the camera pose |
Note: This assumes a coordinate system where X is forward, Y is up, and Z is right. To convert from the traditional camera coordinate system (X right, Y down, Z forward), one can use:
Eigen::Matrix4f pose_orig = //pose in camera coordinates Eigen::Matrix4f cam2robot; cam2robot << 0, 0, 1, 0 0,-1, 0, 0 1, 0, 0, 0 0, 0, 0, 1; Eigen::Matrix4f pose_new = pose_orig * cam2robot; fc.setCameraPose (pose_new);
Definition at line 122 of file frustum_culling.h.
void pcl::FrustumCulling< PointT >::setFarPlaneDistance | ( | float | fp_dist | ) | [inline] |
Set the far plane distance.
[in] | fp_dist | the far plane distance |
Definition at line 186 of file frustum_culling.h.
void pcl::FrustumCulling< PointT >::setHorizontalFOV | ( | float | hfov | ) | [inline] |
Set the horizontal field of view for the camera in degrees.
[in] | hfov | the field of view |
Definition at line 138 of file frustum_culling.h.
void pcl::FrustumCulling< PointT >::setNearPlaneDistance | ( | float | np_dist | ) | [inline] |
Set the near plane distance.
[in] | np_dist | the near plane distance |
Definition at line 170 of file frustum_culling.h.
void pcl::FrustumCulling< PointT >::setVerticalFOV | ( | float | vfov | ) | [inline] |
Set the vertical field of view for the camera in degrees.
[in] | vfov | the field of view |
Definition at line 154 of file frustum_culling.h.
Eigen::Matrix4f pcl::FrustumCulling< PointT >::camera_pose_ [private] |
The camera pose.
Definition at line 223 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::fp_dist_ [private] |
Far plane distance.
Definition at line 231 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::hfov_ [private] |
Horizontal field of view.
Definition at line 225 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::np_dist_ [private] |
Near plane distance.
Definition at line 229 of file frustum_culling.h.
float pcl::FrustumCulling< PointT >::vfov_ [private] |
Vertical field of view.
Definition at line 227 of file frustum_culling.h.