Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
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
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 */
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
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
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
245 const Eigen::VectorXf &model_coefficients,
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
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModel represents the base model class.
Definition sac_model.h:70
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
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.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
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.
typename SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
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.
typename SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
@ SACMODEL_PLANE
Definition model_types.h:47
A point structure representing Euclidean xyz coordinates, and the RGB color.