Point Cloud Library (PCL)  1.7.1
sac_model_normal_plane.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
43 
44 #include <pcl/sample_consensus/sac_model_normal_plane.h>
45 
46 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT, typename PointNT> void
49  const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
50 {
51  if (!normals_)
52  {
53  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
54  inliers.clear ();
55  return;
56  }
57 
58  // Check if the model is valid given the user constraints
59  if (!isModelValid (model_coefficients))
60  {
61  inliers.clear ();
62  return;
63  }
64 
65  // Obtain the plane normal
66  Eigen::Vector4f coeff = model_coefficients;
67  coeff[3] = 0;
68 
69  int nr_p = 0;
70  inliers.resize (indices_->size ());
71  error_sqr_dists_.resize (indices_->size ());
72 
73  // Iterate through the 3d points and calculate the distances from them to the plane
74  for (size_t i = 0; i < indices_->size (); ++i)
75  {
76  const PointT &pt = input_->points[(*indices_)[i]];
77  const PointNT &nt = normals_->points[(*indices_)[i]];
78  // Calculate the distance from the point to the plane normal as the dot product
79  // D = (P-A).N/|N|
80  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0);
81  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0);
82  double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
83 
84  // Calculate the angular distance between the point normal and the plane normal
85  double d_normal = fabs (getAngle3D (n, coeff));
86  d_normal = (std::min) (d_normal, M_PI - d_normal);
87 
88  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
89  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
90 
91  double distance = fabs (weight * d_normal + (1.0 - weight) * d_euclid);
92  if (distance < threshold)
93  {
94  // Returns the indices of the points whose distances are smaller than the threshold
95  inliers[nr_p] = (*indices_)[i];
96  error_sqr_dists_[nr_p] = distance;
97  ++nr_p;
98  }
99  }
100  inliers.resize (nr_p);
101  error_sqr_dists_.resize (nr_p);
102 }
103 
104 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
105 template <typename PointT, typename PointNT> int
107  const Eigen::VectorXf &model_coefficients, const double threshold)
108 {
109  if (!normals_)
110  {
111  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
112  return (0);
113  }
114 
115  // Check if the model is valid given the user constraints
116  if (!isModelValid (model_coefficients))
117  return (0);
118 
119  // Obtain the plane normal
120  Eigen::Vector4f coeff = model_coefficients;
121  coeff[3] = 0;
122 
123  int nr_p = 0;
124 
125  // Iterate through the 3d points and calculate the distances from them to the plane
126  for (size_t i = 0; i < indices_->size (); ++i)
127  {
128  const PointT &pt = input_->points[(*indices_)[i]];
129  const PointNT &nt = normals_->points[(*indices_)[i]];
130  // Calculate the distance from the point to the plane normal as the dot product
131  // D = (P-A).N/|N|
132  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0);
133  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0);
134  double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
135 
136  // Calculate the angular distance between the point normal and the plane normal
137  double d_normal = fabs (getAngle3D (n, coeff));
138  d_normal = (std::min) (d_normal, M_PI - d_normal);
139 
140  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
141  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
142 
143  if (fabs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold)
144  nr_p++;
145  }
146  return (nr_p);
147 }
148 
149 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
150 template <typename PointT, typename PointNT> void
152  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
153 {
154  if (!normals_)
155  {
156  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
157  return;
158  }
159 
160  // Check if the model is valid given the user constraints
161  if (!isModelValid (model_coefficients))
162  {
163  distances.clear ();
164  return;
165  }
166 
167  // Obtain the plane normal
168  Eigen::Vector4f coeff = model_coefficients;
169  coeff[3] = 0;
170 
171  distances.resize (indices_->size ());
172 
173  // Iterate through the 3d points and calculate the distances from them to the plane
174  for (size_t i = 0; i < indices_->size (); ++i)
175  {
176  const PointT &pt = input_->points[(*indices_)[i]];
177  const PointNT &nt = normals_->points[(*indices_)[i]];
178  // Calculate the distance from the point to the plane normal as the dot product
179  // D = (P-A).N/|N|
180  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0);
181  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0);
182  double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
183 
184  // Calculate the angular distance between the point normal and the plane normal
185  double d_normal = fabs (getAngle3D (n, coeff));
186  d_normal = (std::min) (d_normal, M_PI - d_normal);
187 
188  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
189  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
190 
191  distances[i] = fabs (weight * d_normal + (1.0 - weight) * d_euclid);
192  }
193 }
194 
195 #define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>;
196 
197 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
198 
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given plane model.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2)
Compute the smallest angle between two vectors in the [ 0, PI ) interval in 3D.
Definition: common.hpp:46
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.