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