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_CONE_H_
00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
00040
00041 #include <pcl/sample_consensus/sac_model_cone.h>
00042 #include <pcl/common/concatenate.h>
00043 #include <unsupported/Eigen/NonLinearOptimization>
00044
00046 template <typename PointT, typename PointNT> bool
00047 pcl::SampleConsensusModelCone<PointT, PointNT>::isSampleGood(const std::vector<int> &) const
00048 {
00049 return (true);
00050 }
00051
00053 template <typename PointT, typename PointNT> bool
00054 pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
00055 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
00056 {
00057
00058 if (samples.size () != 3)
00059 {
00060 PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
00061 return (false);
00062 }
00063
00064 if (!normals_)
00065 {
00066 PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] No input dataset containing normals was given!\n");
00067 return (false);
00068 }
00069
00070 Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
00071 Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
00072 Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0);
00073
00074 Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
00075 Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
00076 Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0);
00077
00078
00079 Eigen::Vector4f ortho12 = n1.cross3(n2);
00080 Eigen::Vector4f ortho23 = n2.cross3(n3);
00081 Eigen::Vector4f ortho31 = n3.cross3(n1);
00082
00083 float denominator = n1.dot(ortho23);
00084
00085 float d1 = p1.dot (n1);
00086 float d2 = p2.dot (n2);
00087 float d3 = p3.dot (n3);
00088
00089 Eigen::Vector4f apex = (d1 * ortho23 + d2 * ortho31 + d3 * ortho12) / denominator;
00090
00091
00092 Eigen::Vector4f ap1 = p1 - apex;
00093 Eigen::Vector4f ap2 = p2 - apex;
00094 Eigen::Vector4f ap3 = p3 - apex;
00095
00096 Eigen::Vector4f np1 = apex + (ap1/ap1.norm ());
00097 Eigen::Vector4f np2 = apex + (ap2/ap2.norm ());
00098 Eigen::Vector4f np3 = apex + (ap3/ap3.norm ());
00099
00100 Eigen::Vector4f np1np2 = np2 - np1;
00101 Eigen::Vector4f np1np3 = np3 - np1;
00102
00103 Eigen::Vector4f axis_dir = np1np2.cross3 (np1np3);
00104 axis_dir.normalize ();
00105
00106
00107 ap1.normalize ();
00108 ap2.normalize ();
00109 ap3.normalize ();
00110
00111
00112 float opening_angle = ( acosf (ap1.dot (axis_dir)) + acosf (ap2.dot (axis_dir)) + acosf (ap3.dot (axis_dir)) ) / 3.0f;
00113
00114 model_coefficients.resize (7);
00115
00116 model_coefficients[0] = apex[0];
00117 model_coefficients[1] = apex[1];
00118 model_coefficients[2] = apex[2];
00119
00120 model_coefficients[3] = axis_dir[0];
00121 model_coefficients[4] = axis_dir[1];
00122 model_coefficients[5] = axis_dir[2];
00123
00124 model_coefficients[6] = opening_angle;
00125
00126 if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
00127 return (false);
00128 if (model_coefficients[6] != std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
00129 return (false);
00130
00131 return (true);
00132 }
00133
00135 template <typename PointT, typename PointNT> void
00136 pcl::SampleConsensusModelCone<PointT, PointNT>::getDistancesToModel (
00137 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
00138 {
00139
00140 if (!isModelValid (model_coefficients))
00141 {
00142 distances.clear ();
00143 return;
00144 }
00145
00146 distances.resize (indices_->size ());
00147
00148 Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
00149 Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
00150 float opening_angle = model_coefficients[6];
00151
00152 float apexdotdir = apex.dot (axis_dir);
00153 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
00154
00155 for (size_t i = 0; i < indices_->size (); ++i)
00156 {
00157 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
00158 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
00159
00160
00161 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
00162 Eigen::Vector4f pt_proj = apex + k * axis_dir;
00163 Eigen::Vector4f dir = pt - pt_proj;
00164 dir.normalize ();
00165
00166
00167 Eigen::Vector4f height = apex - pt_proj;
00168 float actual_cone_radius = tanf (opening_angle) * height.norm ();
00169 height.normalize ();
00170
00171
00172 Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * dir;
00173
00174
00175
00176 double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
00177
00178
00179 double d_normal = fabs (getAngle3D (n, cone_normal));
00180 d_normal = (std::min) (d_normal, M_PI - d_normal);
00181
00182 distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
00183 }
00184 }
00185
00187 template <typename PointT, typename PointNT> void
00188 pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
00189 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
00190 {
00191
00192 if (!isModelValid (model_coefficients))
00193 {
00194 inliers.clear ();
00195 return;
00196 }
00197
00198 int nr_p = 0;
00199 inliers.resize (indices_->size ());
00200
00201 Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
00202 Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
00203 float opening_angle = model_coefficients[6];
00204
00205 float apexdotdir = apex.dot (axis_dir);
00206 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
00207
00208 for (size_t i = 0; i < indices_->size (); ++i)
00209 {
00210 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
00211 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
00212
00213
00214 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
00215 Eigen::Vector4f pt_proj = apex + k * axis_dir;
00216
00217
00218 Eigen::Vector4f pp_pt_dir = pt - pt_proj;
00219 pp_pt_dir.normalize ();
00220
00221
00222 Eigen::Vector4f height = apex - pt_proj;
00223 double actual_cone_radius = tan(opening_angle) * height.norm ();
00224 height.normalize ();
00225
00226
00227 Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * pp_pt_dir;
00228
00229
00230
00231 double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
00232
00233
00234 double d_normal = fabs (getAngle3D (n, cone_normal));
00235 d_normal = (std::min) (d_normal, M_PI - d_normal);
00236
00237 if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
00238 {
00239
00240 inliers[nr_p] = (*indices_)[i];
00241 nr_p++;
00242 }
00243 }
00244 inliers.resize (nr_p);
00245 }
00246
00248 template <typename PointT, typename PointNT> int
00249 pcl::SampleConsensusModelCone<PointT, PointNT>::countWithinDistance (
00250 const Eigen::VectorXf &model_coefficients, const double threshold)
00251 {
00252
00253
00254 if (!isModelValid (model_coefficients))
00255 return (0);
00256
00257 int nr_p = 0;
00258
00259 Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
00260 Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
00261 float opening_angle = model_coefficients[6];
00262
00263 float apexdotdir = apex.dot (axis_dir);
00264 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
00265
00266 for (size_t i = 0; i < indices_->size (); ++i)
00267 {
00268 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
00269 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
00270
00271
00272 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
00273 Eigen::Vector4f pt_proj = apex + k * axis_dir;
00274
00275
00276 Eigen::Vector4f pp_pt_dir = pt - pt_proj;
00277 pp_pt_dir.normalize ();
00278
00279
00280 Eigen::Vector4f height = apex - pt_proj;
00281 double actual_cone_radius = tan(opening_angle) * height.norm ();
00282 height.normalize ();
00283
00284
00285 Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * pp_pt_dir;
00286
00287
00288
00289 double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
00290
00291
00292 double d_normal = fabs (getAngle3D (n, cone_normal));
00293 d_normal = (std::min) (d_normal, M_PI - d_normal);
00294
00295 if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
00296 nr_p++;
00297 }
00298 return (nr_p);
00299 }
00300
00302 template <typename PointT, typename PointNT> void
00303 pcl::SampleConsensusModelCone<PointT, PointNT>::optimizeModelCoefficients (
00304 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
00305 {
00306 optimized_coefficients = model_coefficients;
00307
00308
00309 if (model_coefficients.size () != 7)
00310 {
00311 PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
00312 return;
00313 }
00314
00315 if (inliers.empty ())
00316 {
00317 PCL_DEBUG ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n");
00318 return;
00319 }
00320
00321 tmp_inliers_ = &inliers;
00322
00323 OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
00324 Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
00325 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
00326 int info = lm.minimize (optimized_coefficients);
00327
00328
00329 PCL_DEBUG ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
00330 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
00331 model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
00332 }
00333
00335 template <typename PointT, typename PointNT> void
00336 pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
00337 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
00338 {
00339
00340 if (model_coefficients.size () != 7)
00341 {
00342 PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
00343 return;
00344 }
00345
00346 projected_points.header = input_->header;
00347 projected_points.is_dense = input_->is_dense;
00348
00349 Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
00350 Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
00351 float opening_angle = model_coefficients[6];
00352
00353 float apexdotdir = apex.dot (axis_dir);
00354 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
00355
00356
00357 if (copy_data_fields)
00358 {
00359
00360 projected_points.points.resize (input_->points.size ());
00361 projected_points.width = input_->width;
00362 projected_points.height = input_->height;
00363
00364 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00365
00366 for (size_t i = 0; i < projected_points.points.size (); ++i)
00367
00368 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
00369
00370
00371 for (size_t i = 0; i < inliers.size (); ++i)
00372 {
00373 Eigen::Vector4f pt (input_->points[inliers[i]].x,
00374 input_->points[inliers[i]].y,
00375 input_->points[inliers[i]].z,
00376 1);
00377
00378 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
00379
00380 pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
00381 pp = apex + k * axis_dir;
00382
00383 Eigen::Vector4f dir = pt - pp;
00384 dir.normalize ();
00385
00386
00387 Eigen::Vector4f height = apex - pp;
00388 float actual_cone_radius = tanf (opening_angle) * height.norm ();
00389
00390
00391 pp += dir * actual_cone_radius;
00392 }
00393 }
00394 else
00395 {
00396
00397 projected_points.points.resize (inliers.size ());
00398 projected_points.width = static_cast<uint32_t> (inliers.size ());
00399 projected_points.height = 1;
00400
00401 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00402
00403 for (size_t i = 0; i < inliers.size (); ++i)
00404
00405 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
00406
00407
00408 for (size_t i = 0; i < inliers.size (); ++i)
00409 {
00410 pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
00411 pcl::Vector4fMapConst pt = input_->points[inliers[i]].getVector4fMap ();
00412
00413 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
00414
00415 pp = apex + k * axis_dir;
00416
00417 Eigen::Vector4f dir = pt - pp;
00418 dir.normalize ();
00419
00420
00421 Eigen::Vector4f height = apex - pp;
00422 float actual_cone_radius = tanf (opening_angle) * height.norm ();
00423
00424
00425 pp += dir * actual_cone_radius;
00426 }
00427 }
00428 }
00429
00431 template <typename PointT, typename PointNT> bool
00432 pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
00433 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
00434 {
00435
00436 if (model_coefficients.size () != 7)
00437 {
00438 PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
00439 return (false);
00440 }
00441
00442 Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
00443 Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
00444 float openning_angle = model_coefficients[6];
00445
00446 float apexdotdir = apex.dot (axis_dir);
00447 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
00448
00449
00450 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
00451 {
00452 Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);
00453
00454
00455 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
00456 Eigen::Vector4f pt_proj = apex + k * axis_dir;
00457 Eigen::Vector4f dir = pt - pt_proj;
00458 dir.normalize ();
00459
00460
00461 Eigen::Vector4f height = apex - pt_proj;
00462 double actual_cone_radius = tan (openning_angle) * height.norm ();
00463
00464
00465
00466 if (fabs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
00467 return (false);
00468 }
00469
00470 return (true);
00471 }
00472
00474 template <typename PointT, typename PointNT> double
00475 pcl::SampleConsensusModelCone<PointT, PointNT>::pointToAxisDistance (
00476 const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients)
00477 {
00478 Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
00479 Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
00480 return sqrt(pcl::sqrPointToLineDistance (pt, apex, axis_dir));
00481 }
00482
00484 template <typename PointT, typename PointNT> bool
00485 pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
00486 {
00487
00488 if (model_coefficients.size () != 7)
00489 {
00490 PCL_ERROR ("[pcl::SampleConsensusModelCone::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
00491 return (false);
00492 }
00493
00494
00495 if (eps_angle_ > 0.0)
00496 {
00497
00498 Eigen::Vector4f coeff;
00499 coeff[0] = model_coefficients[3];
00500 coeff[1] = model_coefficients[4];
00501 coeff[2] = model_coefficients[5];
00502 coeff[3] = 0;
00503
00504 Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
00505 double angle_diff = fabs (getAngle3D (axis, coeff));
00506 angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
00507
00508 if (angle_diff > eps_angle_)
00509 return (false);
00510 }
00511
00512 if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
00513 return (false);
00514 if (model_coefficients[6] != std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
00515 return (false);
00516
00517 return (true);
00518 }
00519
00520 #define PCL_INSTANTIATE_SampleConsensusModelCone(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCone<PointT, PointNT>;
00521
00522 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
00523