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 (%zu)!\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 (
size_t i = 0; i < indices_->size (); ++i)
119 distances[i] = fabsf (sqrtf (
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 (
size_t i = 0; i < indices_->size (); ++i)
149 float distance = fabsf (sqrtf (
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]);
156 if (distance < threshold)
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>
int
171 const Eigen::VectorXf &model_coefficients,
const double threshold)
174 if (!isModelValid (model_coefficients))
179 for (
size_t i = 0; i < indices_->size (); ++i)
183 float distance = fabsf (sqrtf (
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]);
190 if (distance < threshold)
197 template <
typename Po
intT>
void
199 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
201 optimized_coefficients = model_coefficients;
204 if (model_coefficients.size () != 3)
206 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
211 if (inliers.size () <= 3)
213 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ());
217 tmp_inliers_ = &inliers;
219 OptimizationFunctor functor (static_cast<int> (inliers.size ()),
this);
220 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
221 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float> lm (num_diff);
222 int info = lm.minimize (optimized_coefficients);
225 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",
226 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2]);
230 template <
typename Po
intT>
void
232 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
233 PointCloud &projected_points,
bool copy_data_fields)
236 if (model_coefficients.size () != 3)
238 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
242 projected_points.
header = input_->header;
243 projected_points.
is_dense = input_->is_dense;
246 if (copy_data_fields)
249 projected_points.
points.resize (input_->points.size ());
250 projected_points.
width = input_->width;
251 projected_points.
height = input_->height;
255 for (
size_t i = 0; i < projected_points.
points.size (); ++i)
260 for (
size_t i = 0; i < inliers.size (); ++i)
262 float dx = input_->points[inliers[i]].x - model_coefficients[0];
263 float dy = input_->points[inliers[i]].y - model_coefficients[1];
264 float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
266 projected_points.
points[inliers[i]].x = a * dx + model_coefficients[0];
267 projected_points.
points[inliers[i]].y = a * dy + model_coefficients[1];
273 projected_points.
points.resize (inliers.size ());
274 projected_points.
width =
static_cast<uint32_t
> (inliers.size ());
275 projected_points.
height = 1;
279 for (
size_t i = 0; i < inliers.size (); ++i)
284 for (
size_t i = 0; i < inliers.size (); ++i)
286 float dx = input_->points[inliers[i]].x - model_coefficients[0];
287 float dy = input_->points[inliers[i]].y - model_coefficients[1];
288 float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
290 projected_points.
points[i].x = a * dx + model_coefficients[0];
291 projected_points.
points[i].y = a * dy + model_coefficients[1];
297 template <
typename Po
intT>
bool
299 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
302 if (model_coefficients.size () != 3)
304 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
308 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
312 ( input_->points[*it].x - model_coefficients[0] ) *
313 ( input_->points[*it].x - model_coefficients[0] ) +
314 ( input_->points[*it].y - model_coefficients[1] ) *
315 ( input_->points[*it].y - model_coefficients[1] )
316 ) - model_coefficients[2]) > threshold)
323 template <
typename Po
intT>
bool
327 if (model_coefficients.size () != 3)
329 PCL_ERROR (
"[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
333 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[2] < radius_min_)
335 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[2] > radius_max_)
341 #define PCL_INSTANTIATE_SampleConsensusModelCircle2D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle2D<T>;
343 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_