Point Cloud Library (PCL)  1.9.1
rgb_plane_coefficient_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  *
37  *
38  */
39 
40 #ifndef PCL_RGB_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
41 #define PCL_RGB_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
42 
43 #include <pcl/segmentation/boost.h>
44 #include <pcl/segmentation/plane_coefficient_comparator.h>
45 
46 namespace pcl
47 {
48  /** \brief RGBPlaneCoefficientComparator is a Comparator that operates on plane coefficients,
49  * for use in planar segmentation. Also takes into account RGB, so we can segmented different colored co-planar regions.
50  * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
51  *
52  * \author Alex Trevor
53  */
54  template<typename PointT, typename PointNT>
56  {
57  public:
60 
62  typedef typename PointCloudN::Ptr PointCloudNPtr;
64 
65  typedef boost::shared_ptr<RGBPlaneCoefficientComparator<PointT, PointNT> > Ptr;
66  typedef boost::shared_ptr<const RGBPlaneCoefficientComparator<PointT, PointNT> > ConstPtr;
67 
72 
73  /** \brief Empty constructor for RGBPlaneCoefficientComparator. */
75  : color_threshold_ (50.0f)
76  {
77  }
78 
79  /** \brief Constructor for RGBPlaneCoefficientComparator.
80  * \param[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.
81  */
82  RGBPlaneCoefficientComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
83  : PlaneCoefficientComparator<PointT, PointNT> (plane_coeff_d), color_threshold_ (50.0f)
84  {
85  }
86 
87  /** \brief Destructor for RGBPlaneCoefficientComparator. */
88  virtual
90  {
91  }
92 
93  /** \brief Set the tolerance in color space between neighboring points, to be considered part of the same plane.
94  * \param[in] color_threshold The distance in color space
95  */
96  inline void
97  setColorThreshold (float color_threshold)
98  {
99  color_threshold_ = color_threshold * color_threshold;
100  }
101 
102  /** \brief Get the color threshold between neighboring points, to be considered part of the same plane. */
103  inline float
105  {
106  return (color_threshold_);
107  }
108 
109  /** \brief Compare two neighboring points, by using normal information, euclidean distance, and color information.
110  * \param[in] idx1 The index of the first point.
111  * \param[in] idx2 The index of the second point.
112  */
113  bool
114  compare (int idx1, int idx2) const
115  {
116  float dx = input_->points[idx1].x - input_->points[idx2].x;
117  float dy = input_->points[idx1].y - input_->points[idx2].y;
118  float dz = input_->points[idx1].z - input_->points[idx2].z;
119  float dist = std::sqrt (dx*dx + dy*dy + dz*dz);
120  int dr = input_->points[idx1].r - input_->points[idx2].r;
121  int dg = input_->points[idx1].g - input_->points[idx2].g;
122  int db = input_->points[idx1].b - input_->points[idx2].b;
123  //Note: This is not the best metric for color comparisons, we should probably use HSV space.
124  float color_dist = static_cast<float> (dr*dr + dg*dg + db*db);
125  return ( (dist < distance_threshold_)
126  && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ )
127  && (color_dist < color_threshold_));
128  }
129 
130  protected:
132  };
133 }
134 
135 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
boost::shared_ptr< RGBPlaneCoefficientComparator< PointT, PointNT > > Ptr
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::shared_ptr< const RGBPlaneCoefficientComparator< PointT, PointNT > > ConstPtr
RGBPlaneCoefficientComparator(boost::shared_ptr< std::vector< float > > &plane_coeff_d)
Constructor for RGBPlaneCoefficientComparator.
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
float getColorThreshold() const
Get the color threshold between neighboring points, to be considered part of the same plane.
boost::shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:428
RGBPlaneCoefficientComparator is a Comparator that operates on plane coefficients,...
void setColorThreshold(float color_threshold)
Set the tolerance in color space between neighboring points, to be considered part of the same plane.
boost::shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:429
PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar seg...
RGBPlaneCoefficientComparator()
Empty constructor for RGBPlaneCoefficientComparator.
PointCloud represents the base class in PCL for storing collections of 3D points.
virtual ~RGBPlaneCoefficientComparator()
Destructor for RGBPlaneCoefficientComparator.
PointCloudConstPtr input_
Definition: comparator.h:99
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool compare(int idx1, int idx2) const
Compare two neighboring points, by using normal information, euclidean distance, and color informatio...
Comparator< PointT >::PointCloudConstPtr PointCloudConstPtr