00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
00040
00041 #include "pcl/sample_consensus/sac_model_circle.h"
00042
00044 template <typename PointT> void
00045 pcl::SampleConsensusModelCircle2D<PointT>::getSamples (int &iterations, std::vector<int> &samples)
00046 {
00047
00048 if (indices_->empty ())
00049 {
00050 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::getSamples] Empty set of indices given!");
00051 return;
00052 }
00053
00054 samples.resize (3);
00055 double trand = indices_->size () / (RAND_MAX + 1.0);
00056
00057
00058 if (samples.size () > indices_->size ())
00059 {
00060 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::getSamples] Can not select %zu unique points out of %zu!", samples.size (), indices_->size ());
00061
00062 samples.clear ();
00063 iterations = INT_MAX - 1;
00064 return;
00065 }
00066
00067
00068 int idx = (int)(rand () * trand);
00069
00070 samples[0] = (*indices_)[idx];
00071
00072
00073 do
00074 {
00075 idx = (int)(rand () * trand);
00076 samples[1] = (*indices_)[idx];
00077
00078 } while (samples[1] == samples[0]);
00079
00080
00081
00082 Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
00083 Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
00084
00085
00086 p1 -= p0;
00087
00088 Eigen::Array2d dy1dy2;
00089 int iter = 0;
00090 do
00091 {
00092
00093 do
00094 {
00095 idx = (int)(rand () * trand);
00096 samples[2] = (*indices_)[idx];
00097
00098 } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) );
00099
00100
00101 Eigen::Array2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y);
00102
00103
00104 p2 -= p0;
00105
00106 dy1dy2 = p1 / p2;
00107 ++iter;
00108 if (iter > MAX_ITERATIONS_COLLINEAR )
00109 {
00110 ROS_DEBUG ("[pcl::SampleConsensusModelCircle2D::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR);
00111 break;
00112 }
00113
00114 }
00115 while ( (dy1dy2[0] == dy1dy2[1]) );
00116
00117 }
00118
00120 template <typename PointT> bool
00121 pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
00122 {
00123
00124 if (samples.size () != 3)
00125 {
00126 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%zu)!", samples.size ());
00127 return (false);
00128 }
00129
00130 model_coefficients.resize (3);
00131
00132 Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
00133 Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
00134 Eigen::Vector2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y);
00135
00136 Eigen::Vector2d u = (p0 + p1) / 2.0;
00137 Eigen::Vector2d v = (p1 + p2) / 2.0;
00138
00139 Eigen::Vector2d p1p0dif = p1 - p0;
00140 Eigen::Vector2d p2p1dif = p2 - p1;
00141 Eigen::Vector2d uvdif = u - v;
00142
00143 Eigen::Vector2d m (- p1p0dif[0] / p1p0dif[1], - p2p1dif[0] / p2p1dif[1]);
00144
00145
00146 model_coefficients[0] = (m[0] * u[0] - m[1] * v[0] - uvdif[1] ) / (m[0] - m[1]);
00147 model_coefficients[1] = (m[0] * m[1] * uvdif[0] + m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1]);
00148
00149
00150 model_coefficients[2] = sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) +
00151 (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1]));
00152 return (true);
00153 }
00154
00156 template <typename PointT> void
00157 pcl::SampleConsensusModelCircle2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
00158 {
00159
00160 if (model_coefficients.size () != 3)
00161 {
00162 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::getDistancesToModel] Invalid number of model coefficients given (%zu)!", model_coefficients.size ());
00163 return;
00164 }
00165
00166
00167 if (!isModelValid (model_coefficients))
00168 {
00169 distances.clear ();
00170 return;
00171 }
00172 distances.resize (indices_->size ());
00173
00174
00175 for (size_t i = 0; i < indices_->size (); ++i)
00176
00177
00178 distances[i] = fabs (sqrt (
00179 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
00180 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
00181
00182 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
00183 ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
00184 ) - model_coefficients[2]);
00185 }
00186
00188 template <typename PointT> void
00189 pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
00190 const Eigen::VectorXf &model_coefficients, double threshold,
00191 std::vector<int> &inliers)
00192 {
00193
00194 if (model_coefficients.size () != 3)
00195 {
00196 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::selectWithinDistance] Invalid number of model coefficients given (%zu)!", model_coefficients.size ());
00197 return;
00198 }
00199
00200
00201 if (!isModelValid (model_coefficients))
00202 {
00203 inliers.clear ();
00204 return;
00205 }
00206 int nr_p = 0;
00207 inliers.resize (indices_->size ());
00208
00209
00210 for (size_t i = 0; i < indices_->size (); ++i)
00211 {
00212
00213
00214 float distance = fabs (sqrt (
00215 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
00216 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
00217
00218 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
00219 ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
00220 ) - model_coefficients[2]);
00221 if (distance < threshold)
00222 {
00223
00224 inliers[nr_p] = (*indices_)[i];
00225 nr_p++;
00226 }
00227 }
00228 inliers.resize (nr_p);
00229 }
00230
00232 template <typename PointT> void
00233 pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
00234 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
00235 {
00236 boost::mutex::scoped_lock lock (tmp_mutex_);
00237
00238 const int n_unknowns = 3;
00239
00240 if (model_coefficients.size () != n_unknowns)
00241 {
00242 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!", model_coefficients.size ());
00243 optimized_coefficients = model_coefficients;
00244 return;
00245 }
00246
00247
00248 if (inliers.size () <= 3)
00249 {
00250 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.", inliers.size ());
00251 optimized_coefficients = model_coefficients;
00252 return;
00253 }
00254
00255 tmp_inliers_ = &inliers;
00256
00257 int m = inliers.size ();
00258
00259 double *fvec = new double[m];
00260
00261 int iwa[n_unknowns];
00262
00263 int lwa = m * n_unknowns + 5 * n_unknowns + m;
00264 double *wa = new double[lwa];
00265
00266
00267 double x[n_unknowns];
00268 for (int d = 0; d < n_unknowns; ++d)
00269 x[d] = model_coefficients[d];
00270
00271
00272 double tol = sqrt (dpmpar (1));
00273
00274
00275 int info = lmdif1 (&pcl::SampleConsensusModelCircle2D<PointT>::functionToOptimize, this, m, n_unknowns, x, fvec, tol, iwa, wa, lwa);
00276
00277
00278 ROS_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",
00279 info, enorm (m, fvec), model_coefficients[0], model_coefficients[1], model_coefficients[2], x[0], x[1], x[2]);
00280
00281 optimized_coefficients = Eigen::Vector3f (x[0], x[1], x[2]);
00282
00283 free (wa); free (fvec);
00284 }
00285
00287 template <typename PointT> int
00288 pcl::SampleConsensusModelCircle2D<PointT>::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag)
00289 {
00290 SampleConsensusModelCircle2D *model = (SampleConsensusModelCircle2D*)p;
00291
00292 for (int i = 0; i < m; ++i)
00293 {
00294
00295 double xt = model->input_->points[(*model->tmp_inliers_)[i]].x - x[0];
00296 double yt = model->input_->points[(*model->tmp_inliers_)[i]].y - x[1];
00297
00298
00299 fvec[i] = sqrt (xt * xt + yt * yt) - x[2];
00300 }
00301 return (0);
00302 }
00303
00305 template <typename PointT> void
00306 pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
00307 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
00308 PointCloud &projected_points, bool copy_data_fields)
00309 {
00310
00311 if (model_coefficients.size () != 3)
00312 {
00313 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%zu)!", model_coefficients.size ());
00314 return;
00315 }
00316
00317 projected_points.header = input_->header;
00318 projected_points.is_dense = input_->is_dense;
00319
00320
00321 if (copy_data_fields)
00322 {
00323
00324 projected_points.points.resize (input_->points.size ());
00325 projected_points.width = input_->width;
00326 projected_points.height = input_->height;
00327
00328 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00329
00330 for (size_t i = 0; i < projected_points.points.size (); ++i)
00331
00332 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
00333
00334
00335 for (size_t i = 0; i < inliers.size (); ++i)
00336 {
00337 float dx = input_->points[inliers[i]].x - model_coefficients[0];
00338 float dy = input_->points[inliers[i]].y - model_coefficients[1];
00339 float a = sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
00340
00341 projected_points.points[inliers[i]].x = a * dx + model_coefficients[0];
00342 projected_points.points[inliers[i]].y = a * dy + model_coefficients[1];
00343 }
00344 }
00345 else
00346 {
00347
00348 projected_points.points.resize (inliers.size ());
00349 projected_points.width = inliers.size ();
00350 projected_points.height = 1;
00351
00352
00353 for (size_t i = 0; i < inliers.size (); ++i)
00354 {
00355 float dx = input_->points[inliers[i]].x - model_coefficients[0];
00356 float dy = input_->points[inliers[i]].y - model_coefficients[1];
00357 float a = sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
00358
00359 projected_points.points[i].x = a * dx + model_coefficients[0];
00360 projected_points.points[i].y = a * dy + model_coefficients[1];
00361 }
00362 }
00363 }
00364
00366 template <typename PointT> bool
00367 pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (
00368 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, double threshold)
00369 {
00370
00371 if (model_coefficients.size () != 3)
00372 {
00373 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!", model_coefficients.size ());
00374 return (false);
00375 }
00376
00377 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
00378
00379
00380 if (fabs (sqrt (
00381 ( input_->points[*it].x - model_coefficients[0] ) *
00382 ( input_->points[*it].x - model_coefficients[0] ) +
00383 ( input_->points[*it].y - model_coefficients[1] ) *
00384 ( input_->points[*it].y - model_coefficients[1] )
00385 ) - model_coefficients[2]) > threshold)
00386 return (false);
00387
00388 return (true);
00389 }
00390
00392 template <typename PointT> bool
00393 pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
00394 {
00395
00396 if (model_coefficients.size () != 3)
00397 {
00398 ROS_ERROR ("[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%zu)!", model_coefficients.size ());
00399 return (false);
00400 }
00401
00402 if (radius_min_ != -DBL_MAX && model_coefficients[2] < radius_min_)
00403 return (false);
00404 if (radius_max_ != DBL_MAX && model_coefficients[2] > radius_max_)
00405 return (false);
00406
00407 return (true);
00408 }
00409
00410 #define PCL_INSTANTIATE_SampleConsensusModelCircle2D(T) template class pcl::SampleConsensusModelCircle2D<T>;
00411
00412 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
00413