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;
}