Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
sac_model_cone.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2012, 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 */
38
39#pragma once
40
41#include <pcl/sample_consensus/sac_model.h>
42#include <pcl/sample_consensus/model_types.h>
44
45namespace pcl
46{
47 /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation.
48 * The model coefficients are defined as:
49 * <ul>
50 * <li><b>apex.x</b> : the X coordinate of cone's apex
51 * <li><b>apex.y</b> : the Y coordinate of cone's apex
52 * <li><b>apex.z</b> : the Z coordinate of cone's apex
53 * <li><b>axis_direction.x</b> : the X coordinate of the cone's axis direction
54 * <li><b>axis_direction.y</b> : the Y coordinate of the cone's axis direction
55 * <li><b>axis_direction.z</b> : the Z coordinate of the cone's axis direction
56 * <li><b>opening_angle</b> : the cone's opening angle
57 * </ul>
58 * \author Stefan Schrandt
59 * \ingroup sample_consensus
60 */
61 template <typename PointT, typename PointNT>
63 {
64 public:
73
77
78 using Ptr = shared_ptr<SampleConsensusModelCone<PointT, PointNT> >;
79 using ConstPtr = shared_ptr<const SampleConsensusModelCone<PointT, PointNT>>;
80
81 /** \brief Constructor for base SampleConsensusModelCone.
82 * \param[in] cloud the input point cloud dataset
83 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
84 */
85 SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false)
86 : SampleConsensusModel<PointT> (cloud, random)
88 , axis_ (Eigen::Vector3f::Zero ())
89 , eps_angle_ (0)
90 , min_angle_ (-std::numeric_limits<double>::max ())
91 , max_angle_ (std::numeric_limits<double>::max ())
92 {
93 model_name_ = "SampleConsensusModelCone";
94 sample_size_ = 3;
95 model_size_ = 7;
96 }
97
98 /** \brief Constructor for base SampleConsensusModelCone.
99 * \param[in] cloud the input point cloud dataset
100 * \param[in] indices a vector of point indices to be used from \a cloud
101 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
102 */
104 const Indices &indices,
105 bool random = false)
106 : SampleConsensusModel<PointT> (cloud, indices, random)
108 , axis_ (Eigen::Vector3f::Zero ())
109 , eps_angle_ (0)
110 , min_angle_ (-std::numeric_limits<double>::max ())
111 , max_angle_ (std::numeric_limits<double>::max ())
112 {
113 model_name_ = "SampleConsensusModelCone";
114 sample_size_ = 3;
115 model_size_ = 7;
116 }
117
118 /** \brief Copy constructor.
119 * \param[in] source the model to copy into this
120 */
124 eps_angle_ (), min_angle_ (), max_angle_ ()
125 {
126 *this = source;
127 model_name_ = "SampleConsensusModelCone";
128 }
129
130 /** \brief Empty destructor */
132
133 /** \brief Copy constructor.
134 * \param[in] source the model to copy into this
135 */
138 {
141 axis_ = source.axis_;
142 eps_angle_ = source.eps_angle_;
143 min_angle_ = source.min_angle_;
144 max_angle_ = source.max_angle_;
145 return (*this);
146 }
147
148 /** \brief Set the angle epsilon (delta) threshold.
149 * \param[in] ea the maximum allowed difference between the cone's axis and the given axis.
150 */
151 inline void
152 setEpsAngle (double ea) { eps_angle_ = ea; }
153
154 /** \brief Get the angle epsilon (delta) threshold. */
155 inline double
156 getEpsAngle () const { return (eps_angle_); }
157
158 /** \brief Set the axis along which we need to search for a cone direction.
159 * \param[in] ax the axis along which we need to search for a cone direction
160 */
161 inline void
162 setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
163
164 /** \brief Get the axis along which we need to search for a cone direction. */
165 inline Eigen::Vector3f
166 getAxis () const { return (axis_); }
167
168 /** \brief Set the minimum and maximum allowable opening angle for a cone model
169 * given from a user.
170 * \param[in] min_angle the minimum allowable opening angle of a cone model
171 * \param[in] max_angle the maximum allowable opening angle of a cone model
172 */
173 inline void
174 setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
175 {
176 min_angle_ = min_angle;
177 max_angle_ = max_angle;
178 }
179
180 /** \brief Get the opening angle which we need minimum to validate a cone model.
181 * \param[out] min_angle the minimum allowable opening angle of a cone model
182 * \param[out] max_angle the maximum allowable opening angle of a cone model
183 */
184 inline void
185 getMinMaxOpeningAngle (double &min_angle, double &max_angle) const
186 {
187 min_angle = min_angle_;
188 max_angle = max_angle_;
189 }
190
191 /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients
192 * from these samples and store them in model_coefficients. The cone coefficients are: apex,
193 * axis_direction, opening_angle.
194 * \param[in] samples the point indices found as possible good candidates for creating a valid model
195 * \param[out] model_coefficients the resultant model coefficients
196 */
197 bool
198 computeModelCoefficients (const Indices &samples,
199 Eigen::VectorXf &model_coefficients) const override;
200
201 /** \brief Compute all distances from the cloud data to a given cone model.
202 * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
203 * \param[out] distances the resultant estimated distances
204 */
205 void
206 getDistancesToModel (const Eigen::VectorXf &model_coefficients,
207 std::vector<double> &distances) const override;
208
209 /** \brief Select all the points which respect the given model coefficients as inliers.
210 * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
211 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
212 * \param[out] inliers the resultant model inliers
213 */
214 void
215 selectWithinDistance (const Eigen::VectorXf &model_coefficients,
216 const double threshold,
217 Indices &inliers) override;
218
219 /** \brief Count all the points which respect the given model coefficients as inliers.
220 *
221 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
222 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
223 * \return the resultant number of inliers
224 */
225 std::size_t
226 countWithinDistance (const Eigen::VectorXf &model_coefficients,
227 const double threshold) const override;
228
229
230 /** \brief Recompute the cone coefficients using the given inlier set and return them to the user.
231 * @note: these are the coefficients of the cone model after refinement (e.g. after SVD)
232 * \param[in] inliers the data inliers found as supporting the model
233 * \param[in] model_coefficients the initial guess for the optimization
234 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
235 */
236 void
237 optimizeModelCoefficients (const Indices &inliers,
238 const Eigen::VectorXf &model_coefficients,
239 Eigen::VectorXf &optimized_coefficients) const override;
240
241
242 /** \brief Create a new point cloud with inliers projected onto the cone model.
243 * \param[in] inliers the data inliers that we want to project on the cone model
244 * \param[in] model_coefficients the coefficients of a cone model
245 * \param[out] projected_points the resultant projected points
246 * \param[in] copy_data_fields set to true if we need to copy the other data fields
247 */
248 void
249 projectPoints (const Indices &inliers,
250 const Eigen::VectorXf &model_coefficients,
251 PointCloud &projected_points,
252 bool copy_data_fields = true) const override;
253
254 /** \brief Verify whether a subset of indices verifies the given cone model coefficients.
255 * \param[in] indices the data indices that need to be tested against the cone model
256 * \param[in] model_coefficients the cone model coefficients
257 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
258 */
259 bool
260 doSamplesVerifyModel (const std::set<index_t> &indices,
261 const Eigen::VectorXf &model_coefficients,
262 const double threshold) const override;
263
264 /** \brief Return a unique id for this model (SACMODEL_CONE). */
265 inline pcl::SacModel
266 getModelType () const override { return (SACMODEL_CONE); }
267
268 protected:
271
272 /** \brief Get the distance from a point to a line (represented by a point and a direction)
273 * \param[in] pt a point
274 * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
275 */
276 double
277 pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const;
278
279 /** \brief Check whether a model is valid given the user constraints.
280 * \param[in] model_coefficients the set of model coefficients
281 */
282 bool
283 isModelValid (const Eigen::VectorXf &model_coefficients) const override;
284
285 /** \brief Check if a sample of indices results in a good sample of points
286 * indices. Pure virtual.
287 * \param[in] samples the resultant index samples
288 */
289 bool
290 isSampleGood (const Indices &samples) const override;
291
292 private:
293 /** \brief The axis along which we need to search for a cone direction. */
294 Eigen::Vector3f axis_;
295
296 /** \brief The maximum allowed difference between the cone direction and the given axis. */
297 double eps_angle_;
298
299 /** \brief The minimum and maximum allowed opening angles of valid cone model. */
300 double min_angle_;
301 double max_angle_;
302
303 /** \brief Functor for the optimization function */
304 struct OptimizationFunctor : pcl::Functor<float>
305 {
306 /** Functor constructor
307 * \param[in] indices the indices of data points to evaluate
308 * \param[in] estimator pointer to the estimator object
309 */
310 OptimizationFunctor (const pcl::SampleConsensusModelCone<PointT, PointNT> *model, const Indices& indices) :
311 pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
312
313 /** Cost function to be minimized
314 * \param[in] x variables array
315 * \param[out] fvec resultant functions evaluations
316 * \return 0
317 */
318 int
319 operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
320 {
321 Eigen::Vector4f apex (x[0], x[1], x[2], 0);
322 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
323 float opening_angle = x[6];
324
325 float apexdotdir = apex.dot (axis_dir);
326 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
327
328 for (int i = 0; i < values (); ++i)
329 {
330 // dist = f - r
331 Eigen::Vector4f pt = (*model_->input_)[indices_[i]].getVector4fMap();
332 pt[3] = 0;
333
334 // Calculate the point's projection on the cone axis
335 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
336 Eigen::Vector4f pt_proj = apex + k * axis_dir;
337
338 // Calculate the actual radius of the cone at the level of the projected point
339 Eigen::Vector4f height = apex-pt_proj;
340 float actual_cone_radius = tanf (opening_angle) * height.norm ();
341
342 fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius);
343 }
344 return (0);
345 }
346
348 const Indices &indices_;
349 };
350 };
351}
352
353#ifdef PCL_NO_PRECOMPILE
354#include <pcl/sample_consensus/impl/sac_model_cone.hpp>
355#endif
SampleConsensusModelCone defines a model for 3D cone segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the cone coefficients using the given inlier set and return them to the user.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cone direction.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelCone.
SampleConsensusModelCone(const SampleConsensusModelCone &source)
Copy constructor.
typename SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
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 cone model.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_CONE).
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given cone model.
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.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
~SampleConsensusModelCone()
Empty destructor.
SampleConsensusModelCone & operator=(const SampleConsensusModelCone &source)
Copy constructor.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
double pointToAxisDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
Get the distance from a point to a line (represented by a point and a direction)
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
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 cone model coefficients.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
shared_ptr< SampleConsensusModelCone< PointT, PointNT > > Ptr
void setMinMaxOpeningAngle(const double &min_angle, const double &max_angle)
Set the minimum and maximum allowable opening angle for a cone model given from a user.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
typename SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
shared_ptr< const SampleConsensusModelCone< PointT, PointNT > > ConstPtr
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.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition sac_model.h:612
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition sac_model.h:670
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
Definition sac_model.h:665
SampleConsensusModel represents the base model class.
Definition sac_model.h:70
double radius_min_
The minimum and maximum radius limits for the model.
Definition sac_model.h:564
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
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
Define standard C methods to do distance calculations.
double sqrPointToLineDistance(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
Get the square distance from a point to a line (represented by a point and a direction)
Definition distances.h:75
Definition bfgs.h:10
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
@ SACMODEL_CONE
Definition model_types.h:53
Base functor all the models that need non linear optimization must define their own one and implement...
Definition sac_model.h:679
A point structure representing Euclidean xyz coordinates, and the RGB color.