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