Point Cloud Library (PCL)  1.11.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 #include <pcl/common/common.h> // for getAngle3D
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT, typename PointNT> void
50  const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
51 {
52  if (!normals_)
53  {
54  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
55  inliers.clear ();
56  return;
57  }
58 
59  // Check if the model is valid given the user constraints
60  if (!isModelValid (model_coefficients))
61  {
62  inliers.clear ();
63  return;
64  }
65 
66  // Obtain the plane normal
67  Eigen::Vector4f coeff = model_coefficients;
68  coeff[3] = 0.0f;
69 
70  inliers.clear ();
71  error_sqr_dists_.clear ();
72  inliers.reserve (indices_->size ());
73  error_sqr_dists_.reserve (indices_->size ());
74 
75  // Iterate through the 3d points and calculate the distances from them to the plane
76  for (std::size_t i = 0; i < indices_->size (); ++i)
77  {
78  const PointT &pt = (*input_)[(*indices_)[i]];
79  const PointNT &nt = (*normals_)[(*indices_)[i]];
80  // Calculate the distance from the point to the plane normal as the dot product
81  // D = (P-A).N/|N|
82  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
83  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
84  double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
85 
86  // Calculate the angular distance between the point normal and the plane normal
87  double d_normal = std::abs (getAngle3D (n, coeff));
88  d_normal = (std::min) (d_normal, M_PI - d_normal);
89 
90  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
91  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
92 
93  double distance = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
94  if (distance < threshold)
95  {
96  // Returns the indices of the points whose distances are smaller than the threshold
97  inliers.push_back ((*indices_)[i]);
98  error_sqr_dists_.push_back (distance);
99  }
100  }
101 }
102 
103 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104 template <typename PointT, typename PointNT> std::size_t
106  const Eigen::VectorXf &model_coefficients, const double threshold) const
107 {
108  if (!normals_)
109  {
110  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
111  return (0);
112  }
113 
114  // Check if the model is valid given the user constraints
115  if (!isModelValid (model_coefficients))
116  return (0);
117 
118  // Obtain the plane normal
119  Eigen::Vector4f coeff = model_coefficients;
120  coeff[3] = 0.0f;
121 
122  std::size_t nr_p = 0;
123 
124  // Iterate through the 3d points and calculate the distances from them to the plane
125  for (std::size_t i = 0; i < indices_->size (); ++i)
126  {
127  const PointT &pt = (*input_)[(*indices_)[i]];
128  const PointNT &nt = (*normals_)[(*indices_)[i]];
129  // Calculate the distance from the point to the plane normal as the dot product
130  // D = (P-A).N/|N|
131  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
132  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
133  double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
134 
135  // Calculate the angular distance between the point normal and the plane normal
136  double d_normal = std::abs (getAngle3D (n, coeff));
137  d_normal = (std::min) (d_normal, M_PI - d_normal);
138 
139  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
140  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
141 
142  if (std::abs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold)
143  nr_p++;
144  }
145  return (nr_p);
146 }
147 
148 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointT, typename PointNT> void
151  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
152 {
153  if (!normals_)
154  {
155  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
156  return;
157  }
158 
159  // Check if the model is valid given the user constraints
160  if (!isModelValid (model_coefficients))
161  {
162  distances.clear ();
163  return;
164  }
165 
166  // Obtain the plane normal
167  Eigen::Vector4f coeff = model_coefficients;
168  coeff[3] = 0.0f;
169 
170  distances.resize (indices_->size ());
171 
172  // Iterate through the 3d points and calculate the distances from them to the plane
173  for (std::size_t i = 0; i < indices_->size (); ++i)
174  {
175  const PointT &pt = (*input_)[(*indices_)[i]];
176  const PointNT &nt = (*normals_)[(*indices_)[i]];
177  // Calculate the distance from the point to the plane normal as the dot product
178  // D = (P-A).N/|N|
179  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
180  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
181  double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
182 
183  // Calculate the angular distance between the point normal and the plane normal
184  double d_normal = std::abs (getAngle3D (n, coeff));
185  d_normal = (std::min) (d_normal, M_PI - d_normal);
186 
187  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
188  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
189 
190  distances[i] = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
191  }
192 }
193 
194 #define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>;
195 
196 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
197 
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given plane model.
Define standard C methods and C++ classes that are common to all methods.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
Definition: common.hpp:47
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:141
#define M_PI
Definition: pcl_macros.h:192
A point structure representing Euclidean xyz coordinates, and the RGB color.