void ransac3Dplane_distance(
const CMatrixTemplateNumeric<T> &allData,
const vector< CMatrixTemplateNumeric<T> > & testModels,
const T distanceThreshold,
unsigned int & out_bestModelIndex,
vector_size_t & out_inlierIndices )
{
ASSERT_( testModels.size()==1 )
out_bestModelIndex = 0;
const CMatrixTemplateNumeric<T> &M = testModels[0];
ASSERT_( size(M,1)==1 && size(M,2)==4 )
TPlane plane;
plane.coefs[0] = M(0,0);
plane.coefs[1] = M(0,1);
plane.coefs[2] = M(0,2);
plane.coefs[3] = M(0,3);
const size_t N = size(allData,2);
out_inlierIndices.clear();
out_inlierIndices.reserve(100);
for (size_t i=0;i<N;i++)
{
const double d = plane.distance( TPoint3D( allData.get_unsafe(0,i),allData.get_unsafe(1,i),allData.get_unsafe(2,i) ) );
if (d<distanceThreshold)
out_inlierIndices.push_back(i);
}
}
void ransac2Dline_distance(
const CMatrixTemplateNumeric<T> &allData,
const vector< CMatrixTemplateNumeric<T> > & testModels,
const T distanceThreshold,
unsigned int & out_bestModelIndex,
vector_size_t & out_inlierIndices )
{
out_inlierIndices.clear();
out_bestModelIndex = 0;
if (testModels.empty()) return; // No model, no inliers.
ASSERTMSG_( testModels.size()==1, format("Expected testModels.size()=1, but it's = %u",static_cast<unsigned int>(testModels.size()) ) )
const CMatrixTemplateNumeric<T> &M = testModels[0];
ASSERT_( size(M,1)==1 && size(M,2)==3 )
TLine2D line;
line.coefs[0] = M(0,0);
line.coefs[1] = M(0,1);
line.coefs[2] = M(0,2);
const size_t N = size(allData,2);
out_inlierIndices.reserve(100);
for (size_t i=0;i<N;i++)
{
const double d = line.distance( TPoint2D( allData.get_unsafe(0,i),allData.get_unsafe(1,i) ) );
if (d<distanceThreshold)
out_inlierIndices.push_back(i);
}
}
void ransac3Dplane_fit(
const CMatrixTemplateNumeric<T> &allData,
const vector_size_t &useIndices,
vector< CMatrixTemplateNumeric<T> > &fitModels )
{
ASSERT_(useIndices.size()==3);
TPoint3D p1( allData(0,useIndices[0]),allData(1,useIndices[0]),allData(2,useIndices[0]) );
TPoint3D p2( allData(0,useIndices[1]),allData(1,useIndices[1]),allData(2,useIndices[1]) );
TPoint3D p3( allData(0,useIndices[2]),allData(1,useIndices[2]),allData(2,useIndices[2]) );
try
{
TPlane plane( p1,p2,p3 );
fitModels.resize(1);
CMatrixTemplateNumeric<T> &M = fitModels[0];
M.setSize(1,4);
for (size_t i=0;i<4;i++)
M(0,i)=plane.coefs[i];
}
catch(exception &)
{
fitModels.clear();
return;
}
}
bool fitModel( const vector_size_t& useIndices, TPlane3D& model ) const
{
ASSERT_(useIndices.size()==3);
TPoint3D p1( allData[useIndices[0]] );
TPoint3D p2( allData[useIndices[1]] );
TPoint3D p3( allData[useIndices[2]] );
try
{
model = TPlane( p1,p2,p3 );
}
catch(exception &)
{
return false;
}
return true;
}