41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
44 #include <pcl/sample_consensus/eigen.h>
45 #include <pcl/sample_consensus/sac_model_circle.h>
46 #include <pcl/common/concatenate.h>
49 template <
typename Po
intT>
bool
53 Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
54 Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
55 Eigen::Array2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y);
62 Eigen::Array2d dy1dy2 = p1 / p2;
64 return (dy1dy2[0] != dy1dy2[1]);
68 template <
typename Po
intT>
bool
72 if (samples.size () != 3)
74 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
78 model_coefficients.resize (3);
80 Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
81 Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
82 Eigen::Vector2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y);
84 Eigen::Vector2d u = (p0 + p1) / 2.0;
85 Eigen::Vector2d v = (p1 + p2) / 2.0;
87 Eigen::Vector2d p1p0dif = p1 - p0;
88 Eigen::Vector2d p2p1dif = p2 - p1;
89 Eigen::Vector2d uvdif = u - v;
91 Eigen::Vector2d m (- p1p0dif[0] / p1p0dif[1], - p2p1dif[0] / p2p1dif[1]);
94 model_coefficients[0] = static_cast<float> ((m[0] * u[0] - m[1] * v[0] - uvdif[1] ) / (m[0] - m[1]));
95 model_coefficients[1] = static_cast<float> ((m[0] * m[1] * uvdif[0] + m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1]));
98 model_coefficients[2] = static_cast<float> (sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) +
99 (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1])));
104 template <
typename Po
intT>
void
108 if (!isModelValid (model_coefficients))
113 distances.resize (indices_->size ());
116 for (std::size_t i = 0; i < indices_->size (); ++i)
119 distances[i] = std::abs (std::sqrt (
120 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
121 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
123 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
124 ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
125 ) - model_coefficients[2]);
129 template <
typename Po
intT>
void
131 const Eigen::VectorXf &model_coefficients,
const double threshold,
132 std::vector<int> &inliers)
135 if (!isModelValid (model_coefficients))
141 inliers.resize (indices_->size ());
142 error_sqr_dists_.resize (indices_->size ());
145 for (std::size_t i = 0; i < indices_->size (); ++i)
149 float distance = std::abs (std::sqrt (
150 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
151 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
153 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
154 ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
155 ) - model_coefficients[2]);
159 inliers[nr_p] = (*indices_)[i];
160 error_sqr_dists_[nr_p] = static_cast<double> (
distance);
164 inliers.resize (nr_p);
165 error_sqr_dists_.resize (nr_p);
169 template <
typename Po
intT> std::size_t
171 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
174 if (!isModelValid (model_coefficients))
176 std::size_t nr_p = 0;
179 for (std::size_t i = 0; i < indices_->size (); ++i)
183 float distance = std::abs (std::sqrt (
184 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
185 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
187 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
188 ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
189 ) - model_coefficients[2]);
197 template <
typename Po
intT>
void
199 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
201 optimized_coefficients = model_coefficients;
204 if (model_coefficients.size () != 3)
206 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
211 if (inliers.size () <= 3)
213 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
217 OptimizationFunctor functor (
this, inliers);
218 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
219 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float> lm (num_diff);
220 int info = lm.minimize (optimized_coefficients);
223 PCL_DEBUG (
"[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g\n",
224 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2]);
228 template <
typename Po
intT>
void
230 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
231 PointCloud &projected_points,
bool copy_data_fields)
const
234 if (model_coefficients.size () != 3)
236 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
240 projected_points.
header = input_->header;
241 projected_points.
is_dense = input_->is_dense;
244 if (copy_data_fields)
247 projected_points.
points.resize (input_->points.size ());
248 projected_points.
width = input_->width;
249 projected_points.
height = input_->height;
253 for (std::size_t i = 0; i < projected_points.
points.size (); ++i)
258 for (
const int &inlier : inliers)
260 float dx = input_->points[inlier].x - model_coefficients[0];
261 float dy = input_->points[inlier].y - model_coefficients[1];
262 float a = std::sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
264 projected_points.
points[inlier].x = a * dx + model_coefficients[0];
265 projected_points.
points[inlier].y = a * dy + model_coefficients[1];
271 projected_points.
points.resize (inliers.size ());
272 projected_points.
width = static_cast<std::uint32_t> (inliers.size ());
273 projected_points.
height = 1;
277 for (std::size_t i = 0; i < inliers.size (); ++i)
282 for (std::size_t i = 0; i < inliers.size (); ++i)
284 float dx = input_->points[inliers[i]].x - model_coefficients[0];
285 float dy = input_->points[inliers[i]].y - model_coefficients[1];
286 float a = std::sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
288 projected_points.
points[i].x = a * dx + model_coefficients[0];
289 projected_points.
points[i].y = a * dy + model_coefficients[1];
295 template <
typename Po
intT>
bool
297 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
300 if (model_coefficients.size () != 3)
302 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
306 for (
const int &index : indices)
309 if (std::abs (std::sqrt (
310 ( input_->points[index].x - model_coefficients[0] ) *
311 ( input_->points[index].x - model_coefficients[0] ) +
312 ( input_->points[index].y - model_coefficients[1] ) *
313 ( input_->points[index].y - model_coefficients[1] )
314 ) - model_coefficients[2]) > threshold)
321 template <
typename Po
intT>
bool
327 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[2] < radius_min_)
329 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[2] > radius_max_)
335 #define PCL_INSTANTIATE_SampleConsensusModelCircle2D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle2D<T>;
337 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_