Point Cloud Library (PCL) 1.12.1
sac_model_plane.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, 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#pragma once
42
43#ifdef __SSE__
44#include <xmmintrin.h> // for __m128
45#endif // ifdef __SSE__
46#ifdef __AVX__
47#include <immintrin.h> // for __m256
48#endif // ifdef __AVX__
49
50#include <pcl/sample_consensus/sac_model.h>
51#include <pcl/sample_consensus/model_types.h>
52
53namespace pcl
54{
55
56 /** \brief Project a point on a planar model given by a set of normalized coefficients
57 * \param[in] p the input point to project
58 * \param[in] model_coefficients the coefficients of the plane (a, b, c, d) that satisfy ax+by+cz+d=0
59 * \param[out] q the resultant projected point
60 */
61 template <typename Point> inline void
62 projectPoint (const Point &p, const Eigen::Vector4f &model_coefficients, Point &q)
63 {
64 // Calculate the distance from the point to the plane
65 Eigen::Vector4f pp (p.x, p.y, p.z, 1);
66 // use normalized coefficients to calculate the scalar projection
67 float distance_to_plane = pp.dot(model_coefficients);
68
69
70 //TODO: Why doesn't getVector4Map work here?
71 //Eigen::Vector4f q_e = q.getVector4fMap ();
72 //q_e = pp - model_coefficients * distance_to_plane;
73
74 Eigen::Vector4f q_e = pp - distance_to_plane * model_coefficients;
75 q.x = q_e[0];
76 q.y = q_e[1];
77 q.z = q_e[2];
78 }
79
80 /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0
81 * \param p a point
82 * \param a the normalized <i>a</i> coefficient of a plane
83 * \param b the normalized <i>b</i> coefficient of a plane
84 * \param c the normalized <i>c</i> coefficient of a plane
85 * \param d the normalized <i>d</i> coefficient of a plane
86 * \ingroup sample_consensus
87 */
88 template <typename Point> inline double
89 pointToPlaneDistanceSigned (const Point &p, double a, double b, double c, double d)
90 {
91 return (a * p.x + b * p.y + c * p.z + d);
92 }
93
94 /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0
95 * \param p a point
96 * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane
97 * \ingroup sample_consensus
98 */
99 template <typename Point> inline double
100 pointToPlaneDistanceSigned (const Point &p, const Eigen::Vector4f &plane_coefficients)
101 {
102 return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] );
103 }
104
105 /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0
106 * \param p a point
107 * \param a the normalized <i>a</i> coefficient of a plane
108 * \param b the normalized <i>b</i> coefficient of a plane
109 * \param c the normalized <i>c</i> coefficient of a plane
110 * \param d the normalized <i>d</i> coefficient of a plane
111 * \ingroup sample_consensus
112 */
113 template <typename Point> inline double
114 pointToPlaneDistance (const Point &p, double a, double b, double c, double d)
115 {
116 return (std::abs (pointToPlaneDistanceSigned (p, a, b, c, d)) );
117 }
118
119 /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0
120 * \param p a point
121 * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane
122 * \ingroup sample_consensus
123 */
124 template <typename Point> inline double
125 pointToPlaneDistance (const Point &p, const Eigen::Vector4f &plane_coefficients)
126 {
127 return ( std::abs (pointToPlaneDistanceSigned (p, plane_coefficients)) );
128 }
129
130 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131 /** \brief SampleConsensusModelPlane defines a model for 3D plane segmentation.
132 * The model coefficients are defined as:
133 * - \b a : the X coordinate of the plane's normal (normalized)
134 * - \b b : the Y coordinate of the plane's normal (normalized)
135 * - \b c : the Z coordinate of the plane's normal (normalized)
136 * - \b d : the fourth <a href="http://mathworld.wolfram.com/HessianNormalForm.html">Hessian component</a> of the plane's equation
137 *
138 * \author Radu B. Rusu
139 * \ingroup sample_consensus
140 */
141 template <typename PointT>
143 {
144 public:
150
154
155 using Ptr = shared_ptr<SampleConsensusModelPlane<PointT> >;
156 using ConstPtr = shared_ptr<const SampleConsensusModelPlane<PointT>>;
157
158 /** \brief Constructor for base SampleConsensusModelPlane.
159 * \param[in] cloud the input point cloud dataset
160 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
161 */
162 SampleConsensusModelPlane (const PointCloudConstPtr &cloud, bool random = false)
163 : SampleConsensusModel<PointT> (cloud, random)
164 {
165 model_name_ = "SampleConsensusModelPlane";
166 sample_size_ = 3;
167 model_size_ = 4;
168 }
169
170 /** \brief Constructor for base SampleConsensusModelPlane.
171 * \param[in] cloud the input point cloud dataset
172 * \param[in] indices a vector of point indices to be used from \a cloud
173 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
174 */
176 const Indices &indices,
177 bool random = false)
178 : SampleConsensusModel<PointT> (cloud, indices, random)
179 {
180 model_name_ = "SampleConsensusModelPlane";
181 sample_size_ = 3;
182 model_size_ = 4;
183 }
184
185 /** \brief Empty destructor */
187
188 /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
189 * these samples and store them internally in model_coefficients_. The plane coefficients are:
190 * a, b, c, d (ax+by+cz+d=0)
191 * \param[in] samples the point indices found as possible good candidates for creating a valid model
192 * \param[out] model_coefficients the resultant model coefficients
193 */
194 bool
195 computeModelCoefficients (const Indices &samples,
196 Eigen::VectorXf &model_coefficients) const override;
197
198 /** \brief Compute all distances from the cloud data to a given plane model.
199 * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
200 * \param[out] distances the resultant estimated distances
201 */
202 void
203 getDistancesToModel (const Eigen::VectorXf &model_coefficients,
204 std::vector<double> &distances) const override;
205
206 /** \brief Select all the points which respect the given model coefficients as inliers.
207 * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
208 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
209 * \param[out] inliers the resultant model inliers
210 */
211 void
212 selectWithinDistance (const Eigen::VectorXf &model_coefficients,
213 const double threshold,
214 Indices &inliers) override;
215
216 /** \brief Count all the points which respect the given model coefficients as inliers.
217 *
218 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
219 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
220 * \return the resultant number of inliers
221 */
222 std::size_t
223 countWithinDistance (const Eigen::VectorXf &model_coefficients,
224 const double threshold) const override;
225
226 /** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
227 * @note: these are the coefficients of the plane model after refinement (e.g. after SVD)
228 * \param[in] inliers the data inliers found as supporting the model
229 * \param[in] model_coefficients the initial guess for the model coefficients
230 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
231 */
232 void
233 optimizeModelCoefficients (const Indices &inliers,
234 const Eigen::VectorXf &model_coefficients,
235 Eigen::VectorXf &optimized_coefficients) const override;
236
237 /** \brief Create a new point cloud with inliers projected onto the plane model.
238 * \param[in] inliers the data inliers that we want to project on the plane model
239 * \param[in] model_coefficients the *normalized* coefficients of a plane model
240 * \param[out] projected_points the resultant projected points
241 * \param[in] copy_data_fields set to true if we need to copy the other data fields
242 */
243 void
244 projectPoints (const Indices &inliers,
245 const Eigen::VectorXf &model_coefficients,
246 PointCloud &projected_points,
247 bool copy_data_fields = true) const override;
248
249 /** \brief Verify whether a subset of indices verifies the given plane model coefficients.
250 * \param[in] indices the data indices that need to be tested against the plane model
251 * \param[in] model_coefficients the plane model coefficients
252 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
253 */
254 bool
255 doSamplesVerifyModel (const std::set<index_t> &indices,
256 const Eigen::VectorXf &model_coefficients,
257 const double threshold) const override;
258
259 /** \brief Return a unique id for this model (SACMODEL_PLANE). */
260 inline pcl::SacModel
261 getModelType () const override { return (SACMODEL_PLANE); }
262
263 protected:
266
267 /** This implementation uses no SIMD instructions. It is not intended for normal use.
268 * See countWithinDistance which automatically uses the fastest implementation.
269 */
270 std::size_t
271 countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients,
272 const double threshold,
273 std::size_t i = 0) const;
274
275#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
276 /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use.
277 * See countWithinDistance which automatically uses the fastest implementation.
278 */
279 std::size_t
280 countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients,
281 const double threshold,
282 std::size_t i = 0) const;
283#endif
284
285#if defined (__AVX__) && defined (__AVX2__)
286 /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use.
287 * See countWithinDistance which automatically uses the fastest implementation.
288 */
289 std::size_t
290 countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients,
291 const double threshold,
292 std::size_t i = 0) const;
293#endif
294
295#ifdef __AVX__
296 inline __m256 dist8 (const std::size_t i, const __m256 &a_vec, const __m256 &b_vec, const __m256 &c_vec, const __m256 &d_vec, const __m256 &abs_help) const;
297#endif
298
299#ifdef __SSE__
300 inline __m128 dist4 (const std::size_t i, const __m128 &a_vec, const __m128 &b_vec, const __m128 &c_vec, const __m128 &d_vec, const __m128 &abs_help) const;
301#endif
302
303 private:
304 /** \brief Check if a sample of indices results in a good sample of points
305 * indices.
306 * \param[in] samples the resultant index samples
307 */
308 bool
309 isSampleGood (const Indices &samples) const override;
310 };
311}
312
313#ifdef PCL_NO_PRECOMPILE
314#include <pcl/sample_consensus/impl/sac_model_plane.hpp>
315#endif
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
SampleConsensusModel represents the base model class.
Definition: sac_model.h:70
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:77
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:588
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:73
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:556
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition: sac_model.h:553
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
Definition: sac_model.h:527
std::string model_name_
The model name.
Definition: sac_model.h:550
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:591
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:74
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:78
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition: sac_model.h:585
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the plane coefficients using the given inlier set and return them to the user.
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.
~SampleConsensusModelPlane()
Empty destructor.
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given plane model coefficients.
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 projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the plane model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
std::size_t countWithinDistanceStandard(const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i=0) const
This implementation uses no SIMD instructions.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_PLANE).
SampleConsensusModelPlane(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelPlane.
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.
SampleConsensusModelPlane(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelPlane.
double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
double pointToPlaneDistance(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0.
void projectPoint(const Point &p, const Eigen::Vector4f &model_coefficients, Point &q)
Project a point on a planar model given by a set of normalized coefficients.
SacModel
Definition: model_types.h:46
@ SACMODEL_PLANE
Definition: model_types.h:47
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.