GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving. In conjunction with OrganizedConnectedComponentSegmentation, this allows smooth groundplanes / road surfaces to be segmented from point clouds. More...
#include <ground_plane_comparator.h>
Public Types | |
typedef boost::shared_ptr < const GroundPlaneComparator < PointT, PointNT > > | ConstPtr |
typedef Comparator< PointT > ::PointCloud | PointCloud |
typedef Comparator< PointT > ::PointCloudConstPtr | PointCloudConstPtr |
typedef pcl::PointCloud< PointNT > | PointCloudN |
typedef PointCloudN::ConstPtr | PointCloudNConstPtr |
typedef PointCloudN::Ptr | PointCloudNPtr |
typedef boost::shared_ptr < GroundPlaneComparator < PointT, PointNT > > | Ptr |
Public Member Functions | |
virtual bool | compare (int idx1, int idx2) const |
Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, and the difference between the d component of the normals is less than distance threshold, else false. | |
float | getAngularThreshold () const |
Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. | |
float | getDistanceThreshold () const |
Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. | |
PointCloudNConstPtr | getInputNormals () const |
Get the input normals. | |
const std::vector< float > & | getPlaneCoeffD () const |
Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. | |
GroundPlaneComparator () | |
Empty constructor for GroundPlaneComparator. | |
GroundPlaneComparator (boost::shared_ptr< std::vector< float > > &plane_coeff_d) | |
Constructor for GroundPlaneComparator. | |
virtual void | setAngularThreshold (float angular_threshold) |
Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. | |
void | setDistanceThreshold (float distance_threshold, bool depth_dependent=false) |
Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. | |
void | setExpectedGroundNormal (Eigen::Vector3f normal) |
Set the expected ground plane normal with respect to the sensor. Pixels labeled as ground must be within ground_angular_threshold radians of this normal to be labeled as ground. | |
virtual void | setGroundAngularThreshold (float angular_threshold) |
Set the tolerance in radians for difference in normal direction between a point and the expected ground normal. | |
virtual void | setInputCloud (const PointCloudConstPtr &cloud) |
Provide the input cloud. | |
void | setInputNormals (const PointCloudNConstPtr &normals) |
Provide a pointer to the input normals. | |
void | setPlaneCoeffD (boost::shared_ptr< std::vector< float > > &plane_coeff_d) |
Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud. | |
void | setPlaneCoeffD (std::vector< float > &plane_coeff_d) |
Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud. | |
virtual | ~GroundPlaneComparator () |
Destructor for GroundPlaneComparator. | |
Protected Attributes | |
float | angular_threshold_ |
bool | depth_dependent_ |
Eigen::Vector3f | desired_road_axis_ |
float | distance_threshold_ |
PointCloudNConstPtr | normals_ |
boost::shared_ptr< std::vector < float > > | plane_coeff_d_ |
float | road_angular_threshold_ |
Eigen::Vector3f | z_axis_ |
GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving. In conjunction with OrganizedConnectedComponentSegmentation, this allows smooth groundplanes / road surfaces to be segmented from point clouds.
Definition at line 55 of file ground_plane_comparator.h.
typedef boost::shared_ptr<const GroundPlaneComparator<PointT, PointNT> > pcl::GroundPlaneComparator< PointT, PointNT >::ConstPtr |
Reimplemented from pcl::Comparator< PointT >.
Definition at line 66 of file ground_plane_comparator.h.
typedef Comparator<PointT>::PointCloud pcl::GroundPlaneComparator< PointT, PointNT >::PointCloud |
Reimplemented from pcl::Comparator< PointT >.
Definition at line 58 of file ground_plane_comparator.h.
typedef Comparator<PointT>::PointCloudConstPtr pcl::GroundPlaneComparator< PointT, PointNT >::PointCloudConstPtr |
Reimplemented from pcl::Comparator< PointT >.
Definition at line 59 of file ground_plane_comparator.h.
typedef pcl::PointCloud<PointNT> pcl::GroundPlaneComparator< PointT, PointNT >::PointCloudN |
Definition at line 61 of file ground_plane_comparator.h.
typedef PointCloudN::ConstPtr pcl::GroundPlaneComparator< PointT, PointNT >::PointCloudNConstPtr |
Definition at line 63 of file ground_plane_comparator.h.
typedef PointCloudN::Ptr pcl::GroundPlaneComparator< PointT, PointNT >::PointCloudNPtr |
Definition at line 62 of file ground_plane_comparator.h.
typedef boost::shared_ptr<GroundPlaneComparator<PointT, PointNT> > pcl::GroundPlaneComparator< PointT, PointNT >::Ptr |
Reimplemented from pcl::Comparator< PointT >.
Definition at line 65 of file ground_plane_comparator.h.
pcl::GroundPlaneComparator< PointT, PointNT >::GroundPlaneComparator | ( | ) | [inline] |
Empty constructor for GroundPlaneComparator.
Definition at line 71 of file ground_plane_comparator.h.
pcl::GroundPlaneComparator< PointT, PointNT >::GroundPlaneComparator | ( | boost::shared_ptr< std::vector< float > > & | plane_coeff_d | ) | [inline] |
Constructor for GroundPlaneComparator.
[in] | plane_coeff_d | a reference to a vector of d coefficients of plane equations. Must be the same size as the input cloud and input normals. a, b, and c coefficients are in the input normals. |
Definition at line 86 of file ground_plane_comparator.h.
virtual pcl::GroundPlaneComparator< PointT, PointNT >::~GroundPlaneComparator | ( | ) | [inline, virtual] |
Destructor for GroundPlaneComparator.
Definition at line 100 of file ground_plane_comparator.h.
virtual bool pcl::GroundPlaneComparator< PointT, PointNT >::compare | ( | int | idx1, |
int | idx2 | ||
) | const [inline, virtual] |
Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, and the difference between the d component of the normals is less than distance threshold, else false.
idx1 | The first index for the comparison |
idx2 | The second index for the comparison |
Implements pcl::Comparator< PointT >.
Definition at line 213 of file ground_plane_comparator.h.
float pcl::GroundPlaneComparator< PointT, PointNT >::getAngularThreshold | ( | ) | const [inline] |
Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
Definition at line 183 of file ground_plane_comparator.h.
float pcl::GroundPlaneComparator< PointT, PointNT >::getDistanceThreshold | ( | ) | const [inline] |
Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane.
Definition at line 202 of file ground_plane_comparator.h.
PointCloudNConstPtr pcl::GroundPlaneComparator< PointT, PointNT >::getInputNormals | ( | ) | const [inline] |
Get the input normals.
Definition at line 123 of file ground_plane_comparator.h.
const std::vector<float>& pcl::GroundPlaneComparator< PointT, PointNT >::getPlaneCoeffD | ( | ) | const [inline] |
Get a pointer to the vector of the d-coefficient of the planes' hessian normal form.
Definition at line 148 of file ground_plane_comparator.h.
virtual void pcl::GroundPlaneComparator< PointT, PointNT >::setAngularThreshold | ( | float | angular_threshold | ) | [inline, virtual] |
Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
[in] | angular_threshold | the tolerance in radians |
Definition at line 157 of file ground_plane_comparator.h.
void pcl::GroundPlaneComparator< PointT, PointNT >::setDistanceThreshold | ( | float | distance_threshold, |
bool | depth_dependent = false |
||
) | [inline] |
Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
[in] | distance_threshold | the tolerance in meters (at 1m) |
[in] | depth_dependent | whether to scale the threshold based on range from the sensor (default: false) |
Definition at line 193 of file ground_plane_comparator.h.
void pcl::GroundPlaneComparator< PointT, PointNT >::setExpectedGroundNormal | ( | Eigen::Vector3f | normal | ) | [inline] |
Set the expected ground plane normal with respect to the sensor. Pixels labeled as ground must be within ground_angular_threshold radians of this normal to be labeled as ground.
[in] | normal | The normal direction of the expected ground plane. |
Definition at line 175 of file ground_plane_comparator.h.
virtual void pcl::GroundPlaneComparator< PointT, PointNT >::setGroundAngularThreshold | ( | float | angular_threshold | ) | [inline, virtual] |
Set the tolerance in radians for difference in normal direction between a point and the expected ground normal.
[in] | angular_threshold | the |
Definition at line 166 of file ground_plane_comparator.h.
virtual void pcl::GroundPlaneComparator< PointT, PointNT >::setInputCloud | ( | const PointCloudConstPtr & | cloud | ) | [inline, virtual] |
Provide the input cloud.
[in] | cloud | the input point cloud. |
Reimplemented from pcl::Comparator< PointT >.
Definition at line 107 of file ground_plane_comparator.h.
void pcl::GroundPlaneComparator< PointT, PointNT >::setInputNormals | ( | const PointCloudNConstPtr & | normals | ) | [inline] |
Provide a pointer to the input normals.
[in] | normals | the input normal cloud. |
Definition at line 116 of file ground_plane_comparator.h.
void pcl::GroundPlaneComparator< PointT, PointNT >::setPlaneCoeffD | ( | boost::shared_ptr< std::vector< float > > & | plane_coeff_d | ) | [inline] |
Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud.
[in] | plane_coeff_d | a pointer to the plane coefficients. |
Definition at line 132 of file ground_plane_comparator.h.
void pcl::GroundPlaneComparator< PointT, PointNT >::setPlaneCoeffD | ( | std::vector< float > & | plane_coeff_d | ) | [inline] |
Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud.
[in] | plane_coeff_d | a pointer to the plane coefficients. |
Definition at line 141 of file ground_plane_comparator.h.
float pcl::GroundPlaneComparator< PointT, PointNT >::angular_threshold_ [protected] |
Definition at line 238 of file ground_plane_comparator.h.
bool pcl::GroundPlaneComparator< PointT, PointNT >::depth_dependent_ [protected] |
Definition at line 241 of file ground_plane_comparator.h.
Eigen::Vector3f pcl::GroundPlaneComparator< PointT, PointNT >::desired_road_axis_ [protected] |
Definition at line 243 of file ground_plane_comparator.h.
float pcl::GroundPlaneComparator< PointT, PointNT >::distance_threshold_ [protected] |
Definition at line 240 of file ground_plane_comparator.h.
PointCloudNConstPtr pcl::GroundPlaneComparator< PointT, PointNT >::normals_ [protected] |
Definition at line 236 of file ground_plane_comparator.h.
boost::shared_ptr<std::vector<float> > pcl::GroundPlaneComparator< PointT, PointNT >::plane_coeff_d_ [protected] |
Definition at line 237 of file ground_plane_comparator.h.
float pcl::GroundPlaneComparator< PointT, PointNT >::road_angular_threshold_ [protected] |
Definition at line 239 of file ground_plane_comparator.h.
Eigen::Vector3f pcl::GroundPlaneComparator< PointT, PointNT >::z_axis_ [protected] |
Definition at line 242 of file ground_plane_comparator.h.