Point Cloud Library (PCL) 1.14.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 namespace internal {
48 int optimizeModelCoefficientsCone (Eigen::VectorXf& coeff, const Eigen::ArrayXf& pts_x, const Eigen::ArrayXf& pts_y, const Eigen::ArrayXf& pts_z);
49 } // namespace internal
50
51 /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation.
52 * The model coefficients are defined as:
53 * <ul>
54 * <li><b>apex.x</b> : the X coordinate of cone's apex
55 * <li><b>apex.y</b> : the Y coordinate of cone's apex
56 * <li><b>apex.z</b> : the Z coordinate of cone's apex
57 * <li><b>axis_direction.x</b> : the X coordinate of the cone's axis direction
58 * <li><b>axis_direction.y</b> : the Y coordinate of the cone's axis direction
59 * <li><b>axis_direction.z</b> : the Z coordinate of the cone's axis direction
60 * <li><b>opening_angle</b> : the cone's opening angle
61 * </ul>
62 * \author Stefan Schrandt
63 * \ingroup sample_consensus
64 */
65 template <typename PointT, typename PointNT>
67 {
68 public:
77
81
82 using Ptr = shared_ptr<SampleConsensusModelCone<PointT, PointNT> >;
83 using ConstPtr = shared_ptr<const SampleConsensusModelCone<PointT, PointNT>>;
84
85 /** \brief Constructor for base SampleConsensusModelCone.
86 * \param[in] cloud the input point cloud dataset
87 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
88 */
89 SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false)
90 : SampleConsensusModel<PointT> (cloud, random)
92 , axis_ (Eigen::Vector3f::Zero ())
93 , eps_angle_ (0)
94 , min_angle_ (-std::numeric_limits<double>::max ())
95 , max_angle_ (std::numeric_limits<double>::max ())
96 {
97 model_name_ = "SampleConsensusModelCone";
98 sample_size_ = 3;
99 model_size_ = 7;
100 }
101
102 /** \brief Constructor for base SampleConsensusModelCone.
103 * \param[in] cloud the input point cloud dataset
104 * \param[in] indices a vector of point indices to be used from \a cloud
105 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
106 */
108 const Indices &indices,
109 bool random = false)
110 : SampleConsensusModel<PointT> (cloud, indices, random)
112 , axis_ (Eigen::Vector3f::Zero ())
113 , eps_angle_ (0)
114 , min_angle_ (-std::numeric_limits<double>::max ())
115 , max_angle_ (std::numeric_limits<double>::max ())
116 {
117 model_name_ = "SampleConsensusModelCone";
118 sample_size_ = 3;
119 model_size_ = 7;
120 }
121
122 /** \brief Copy constructor.
123 * \param[in] source the model to copy into this
124 */
128 eps_angle_ (), min_angle_ (), max_angle_ ()
129 {
130 *this = source;
131 model_name_ = "SampleConsensusModelCone";
132 }
133
134 /** \brief Empty destructor */
135 ~SampleConsensusModelCone () override = default;
136
137 /** \brief Copy constructor.
138 * \param[in] source the model to copy into this
139 */
142 {
145 axis_ = source.axis_;
146 eps_angle_ = source.eps_angle_;
147 min_angle_ = source.min_angle_;
148 max_angle_ = source.max_angle_;
149 return (*this);
150 }
151
152 /** \brief Set the angle epsilon (delta) threshold.
153 * \param[in] ea the maximum allowed difference between the cone's axis and the given axis.
154 */
155 inline void
156 setEpsAngle (double ea) { eps_angle_ = ea; }
157
158 /** \brief Get the angle epsilon (delta) threshold. */
159 inline double
160 getEpsAngle () const { return (eps_angle_); }
161
162 /** \brief Set the axis along which we need to search for a cone direction.
163 * \param[in] ax the axis along which we need to search for a cone direction
164 */
165 inline void
166 setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
167
168 /** \brief Get the axis along which we need to search for a cone direction. */
169 inline Eigen::Vector3f
170 getAxis () const { return (axis_); }
171
172 /** \brief Set the minimum and maximum allowable opening angle for a cone model
173 * given from a user.
174 * \param[in] min_angle the minimum allowable opening angle of a cone model
175 * \param[in] max_angle the maximum allowable opening angle of a cone model
176 */
177 inline void
178 setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
179 {
180 min_angle_ = min_angle;
181 max_angle_ = max_angle;
182 }
183
184 /** \brief Get the opening angle which we need minimum to validate a cone model.
185 * \param[out] min_angle the minimum allowable opening angle of a cone model
186 * \param[out] max_angle the maximum allowable opening angle of a cone model
187 */
188 inline void
189 getMinMaxOpeningAngle (double &min_angle, double &max_angle) const
190 {
191 min_angle = min_angle_;
192 max_angle = max_angle_;
193 }
194
195 /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients
196 * from these samples and store them in model_coefficients. The cone coefficients are: apex,
197 * axis_direction, opening_angle.
198 * \param[in] samples the point indices found as possible good candidates for creating a valid model
199 * \param[out] model_coefficients the resultant model coefficients
200 */
201 bool
202 computeModelCoefficients (const Indices &samples,
203 Eigen::VectorXf &model_coefficients) const override;
204
205 /** \brief Compute all distances from the cloud data to a given cone model.
206 * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
207 * \param[out] distances the resultant estimated distances
208 */
209 void
210 getDistancesToModel (const Eigen::VectorXf &model_coefficients,
211 std::vector<double> &distances) const override;
212
213 /** \brief Select all the points which respect the given model coefficients as inliers.
214 * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
215 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
216 * \param[out] inliers the resultant model inliers
217 */
218 void
219 selectWithinDistance (const Eigen::VectorXf &model_coefficients,
220 const double threshold,
221 Indices &inliers) override;
222
223 /** \brief Count all the points which respect the given model coefficients as inliers.
224 *
225 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
226 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
227 * \return the resultant number of inliers
228 */
229 std::size_t
230 countWithinDistance (const Eigen::VectorXf &model_coefficients,
231 const double threshold) const override;
232
233
234 /** \brief Recompute the cone coefficients using the given inlier set and return them to the user.
235 * @note: these are the coefficients of the cone model after refinement (e.g. after SVD)
236 * \param[in] inliers the data inliers found as supporting the model
237 * \param[in] model_coefficients the initial guess for the optimization
238 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
239 */
240 void
241 optimizeModelCoefficients (const Indices &inliers,
242 const Eigen::VectorXf &model_coefficients,
243 Eigen::VectorXf &optimized_coefficients) const override;
244
245
246 /** \brief Create a new point cloud with inliers projected onto the cone model.
247 * \param[in] inliers the data inliers that we want to project on the cone model
248 * \param[in] model_coefficients the coefficients of a cone model
249 * \param[out] projected_points the resultant projected points
250 * \param[in] copy_data_fields set to true if we need to copy the other data fields
251 */
252 void
253 projectPoints (const Indices &inliers,
254 const Eigen::VectorXf &model_coefficients,
255 PointCloud &projected_points,
256 bool copy_data_fields = true) const override;
257
258 /** \brief Verify whether a subset of indices verifies the given cone model coefficients.
259 * \param[in] indices the data indices that need to be tested against the cone model
260 * \param[in] model_coefficients the cone model coefficients
261 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
262 */
263 bool
264 doSamplesVerifyModel (const std::set<index_t> &indices,
265 const Eigen::VectorXf &model_coefficients,
266 const double threshold) const override;
267
268 /** \brief Return a unique id for this model (SACMODEL_CONE). */
269 inline pcl::SacModel
270 getModelType () const override { return (SACMODEL_CONE); }
271
272 protected:
275
276 /** \brief Get the distance from a point to a line (represented by a point and a direction)
277 * \param[in] pt a point
278 * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
279 */
280 double
281 pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const;
282
283 /** \brief Check whether a model is valid given the user constraints.
284 * \param[in] model_coefficients the set of model coefficients
285 */
286 bool
287 isModelValid (const Eigen::VectorXf &model_coefficients) const override;
288
289 /** \brief Check if a sample of indices results in a good sample of points
290 * indices. Pure virtual.
291 * \param[in] samples the resultant index samples
292 */
293 bool
294 isSampleGood (const Indices &samples) const override;
295
296 private:
297 /** \brief The axis along which we need to search for a cone direction. */
298 Eigen::Vector3f axis_;
299
300 /** \brief The maximum allowed difference between the cone direction and the given axis. */
301 double eps_angle_;
302
303 /** \brief The minimum and maximum allowed opening angles of valid cone model. */
304 double min_angle_;
305 double max_angle_;
306 };
307}
308
309#ifdef PCL_NO_PRECOMPILE
310#include <pcl/sample_consensus/impl/sac_model_cone.hpp>
311#endif
PointCloud represents the base class in PCL for storing collections of 3D points.
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
~SampleConsensusModelCone() override=default
Empty destructor.
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 & 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)
shared_ptr< const SampleConsensusModelCone< PointT, PointNT > > ConstPtr
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
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:613
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition sac_model.h:671
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:666
SampleConsensusModel represents the base model class.
Definition sac_model.h:71
double radius_min_
The minimum and maximum radius limits for the model.
Definition sac_model.h:565
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition sac_model.h:589
typename PointCloud::ConstPtr PointCloudConstPtr
Definition sac_model.h:74
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition sac_model.h:557
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition sac_model.h:554
std::string model_name_
The model name.
Definition sac_model.h:551
unsigned int model_size_
The number of coefficients in the model.
Definition sac_model.h:592
typename PointCloud::Ptr PointCloudPtr
Definition sac_model.h:75
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition sac_model.h:586
Define standard C methods to do distance calculations.
Definition bfgs.h:10
int optimizeModelCoefficientsCone(Eigen::VectorXf &coeff, const Eigen::ArrayXf &pts_x, const Eigen::ArrayXf &pts_y, const Eigen::ArrayXf &pts_z)
@ SACMODEL_CONE
Definition model_types.h:53
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.