void mrpt::math::ransac_detect_3D_planes(
const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &x,
const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &y,
const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &z,
vector<pair<size_t,TPlane> > &out_detected_planes,
const double threshold,
const size_t min_inliers_for_valid_plane
)
{
MRPT_START
ASSERT_(x.size()==y.size() && x.size()==z.size())
out_detected_planes.clear();
if (x.empty())
return;
// The running lists of remaining points after each plane, as a matrix:
CMatrixTemplateNumeric<NUMTYPE> remainingPoints( 3, x.size() );
remainingPoints.insertRow(0,x);
remainingPoints.insertRow(1,y);
remainingPoints.insertRow(2,z);
// ---------------------------------------------
// For each plane:
// ---------------------------------------------
for (;;)
{
mrpt::vector_size_t this_best_inliers;
CMatrixTemplateNumeric<NUMTYPE> this_best_model;
math::RANSAC_Template<NUMTYPE>::execute(
remainingPoints,
ransac3Dplane_fit,
ransac3Dplane_distance,
ransac3Dplane_degenerate,
threshold,
3, // Minimum set of points
this_best_inliers,
this_best_model,
true, // Verbose
0.999 // Prob. of good result
);
// Is this plane good enough?
if (this_best_inliers.size()>=min_inliers_for_valid_plane)
{
// Add this plane to the output list:
out_detected_planes.push_back(
std::make_pair<size_t,TPlane>(
this_best_inliers.size(),
TPlane( this_best_model(0,0), this_best_model(0,1),this_best_model(0,2),this_best_model(0,3) )
) );
out_detected_planes.rbegin()->second.unitarize();
// Discard the selected points so they are not used again for finding subsequent planes:
remainingPoints.removeColumns(this_best_inliers);
}
else
{
break; // Do not search for more planes.
}
}
MRPT_END
}
