Point Cloud Library (PCL) 1.12.0
edge_aware_plane_comparator.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, 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 * $Id: extract_clusters.h 5027 2012-03-12 03:10:45Z rusu $
37 *
38 */
39
40#pragma once
41
42#include <pcl/segmentation/plane_coefficient_comparator.h>
43
44namespace pcl
45{
46 /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients,
47 * for use in planar segmentation.
48 * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
49 *
50 * \author Stefan Holzer, Alex Trevor
51 */
52 template<typename PointT, typename PointNT>
54 {
55 public:
58
62
63 using Ptr = shared_ptr<EdgeAwarePlaneComparator<PointT, PointNT> >;
64 using ConstPtr = shared_ptr<const EdgeAwarePlaneComparator<PointT, PointNT> >;
65
73
74 /** \brief Empty constructor for PlaneCoefficientComparator. */
79 {
80 }
81
82 /** \brief Empty constructor for PlaneCoefficientComparator.
83 * \param[in] distance_map the distance map to use
84 */
85 EdgeAwarePlaneComparator (const float *distance_map) :
86 distance_map_ (distance_map),
90 {
91 }
92
93 /** \brief Destructor for PlaneCoefficientComparator. */
94
96 {
97 }
98
99 /** \brief Set a distance map to use. For an example of a valid distance map see
100 * IntegralImageNormalEstimation::getDistanceMap
101 * \param[in] distance_map the distance map to use
102 */
103 inline void
104 setDistanceMap (const float *distance_map)
105 {
106 distance_map_ = distance_map;
107 }
108
109 /** \brief Return the distance map used. */
110 const float*
112 {
113 return (distance_map_);
114 }
115
116 /** \brief Set the curvature threshold for creating a new segment
117 * \param[in] curvature_threshold a threshold for the curvature
118 */
119 void
120 setCurvatureThreshold (float curvature_threshold)
121 {
122 curvature_threshold_ = curvature_threshold;
123 }
124
125 /** \brief Get the curvature threshold. */
126 inline float
128 {
129 return (curvature_threshold_);
130 }
131
132 /** \brief Set the distance map threshold -- the number of pixel away from a border / nan
133 * \param[in] distance_map_threshold the distance map threshold
134 */
135 void
136 setDistanceMapThreshold (float distance_map_threshold)
137 {
138 distance_map_threshold_ = distance_map_threshold;
139 }
140
141 /** \brief Get the distance map threshold (in pixels). */
142 inline float
144 {
146 }
147
148 /** \brief Set the euclidean distance threshold.
149 * \param[in] euclidean_distance_threshold the euclidean distance threshold in meters
150 */
151 void
152 setEuclideanDistanceThreshold (float euclidean_distance_threshold)
153 {
154 euclidean_distance_threshold_ = euclidean_distance_threshold;
155 }
156
157 /** \brief Get the euclidean distance threshold. */
158 inline float
160 {
162 }
163
164 protected:
165 /** \brief Compare two neighboring points, by using normal information, curvature, and euclidean distance information.
166 * \param[in] idx1 The index of the first point.
167 * \param[in] idx2 The index of the second point.
168 */
169 bool
170 compare (int idx1, int idx2) const override
171 {
172 // Note: there are two distance thresholds here that make sense to scale with depth.
173 // dist_threshold is on the perpendicular distance to the plane, as in plane comparator
174 // We additionally check euclidean distance to ensure that we don't have neighboring coplanar points
175 // that aren't close in euclidean space (think two tables separated by a meter, viewed from an angle
176 // where the surfaces are adjacent in image space).
177 float dist_threshold = distance_threshold_;
178 float euclidean_dist_threshold = euclidean_distance_threshold_;
180 {
181 Eigen::Vector3f vec = (*input_)[idx1].getVector3fMap ();
182 float z = vec.dot (z_axis_);
183 dist_threshold *= z * z;
184 euclidean_dist_threshold *= z * z;
185 }
186
187 float dx = (*input_)[idx1].x - (*input_)[idx2].x;
188 float dy = (*input_)[idx1].y - (*input_)[idx2].y;
189 float dz = (*input_)[idx1].z - (*input_)[idx2].z;
190 float dist = std::sqrt (dx*dx + dy*dy + dz*dz);
191
192 bool normal_ok = ((*normals_)[idx1].getNormalVector3fMap ().dot ((*normals_)[idx2].getNormalVector3fMap () ) > angular_threshold_ );
193 bool dist_ok = (dist < euclidean_dist_threshold);
194
195 bool curvature_ok = (*normals_)[idx1].curvature < curvature_threshold_;
196 bool plane_d_ok = std::abs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < dist_threshold;
197
199 curvature_ok = false;
200
201 return (dist_ok && normal_ok && curvature_ok && plane_d_ok);
202 }
203
204 protected:
205 const float* distance_map_;
209 };
210}
shared_ptr< Comparator< PointT > > Ptr
Definition: comparator.h:61
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:59
shared_ptr< const Comparator< PointT > > ConstPtr
Definition: comparator.h:62
EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients, for use in planar segme...
float getEuclideanDistanceThreshold() const
Get the euclidean distance threshold.
bool compare(int idx1, int idx2) const override
Compare two neighboring points, by using normal information, curvature, and euclidean distance inform...
void setDistanceMapThreshold(float distance_map_threshold)
Set the distance map threshold – the number of pixel away from a border / nan.
float getCurvatureThreshold() const
Get the curvature threshold.
void setCurvatureThreshold(float curvature_threshold)
Set the curvature threshold for creating a new segment.
void setDistanceMap(const float *distance_map)
Set a distance map to use.
typename PointCloudN::ConstPtr PointCloudNConstPtr
~EdgeAwarePlaneComparator()
Destructor for PlaneCoefficientComparator.
void setEuclideanDistanceThreshold(float euclidean_distance_threshold)
Set the euclidean distance threshold.
typename PointCloudN::Ptr PointCloudNPtr
EdgeAwarePlaneComparator()
Empty constructor for PlaneCoefficientComparator.
EdgeAwarePlaneComparator(const float *distance_map)
Empty constructor for PlaneCoefficientComparator.
const float * getDistanceMap() const
Return the distance map used.
float getDistanceMapThreshold() const
Get the distance map threshold (in pixels).
PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar seg...
shared_ptr< std::vector< float > > plane_coeff_d_
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:414