TEST (SampleConsensusModelLine, RANSAC)
{
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
cloud.points.resize (10);
cloud.points[0].getVector3fMap () << 1.0f, 2.00f, 3.00f;
cloud.points[1].getVector3fMap () << 4.0f, 5.00f, 6.00f;
cloud.points[2].getVector3fMap () << 7.0f, 8.00f, 9.00f;
cloud.points[3].getVector3fMap () << 10.0f, 11.00f, 12.00f;
cloud.points[4].getVector3fMap () << 13.0f, 14.00f, 15.00f;
cloud.points[5].getVector3fMap () << 16.0f, 17.00f, 18.00f;
cloud.points[6].getVector3fMap () << 19.0f, 20.00f, 21.00f;
cloud.points[7].getVector3fMap () << 22.0f, 23.00f, 24.00f;
cloud.points[8].getVector3fMap () << -5.0f, 1.57f, 0.75f;
cloud.points[9].getVector3fMap () << 4.0f, 2.00f, 3.00f;
// Create a shared line model pointer directly
SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.001);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_TRUE (result);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (2, sample.size ());
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (8, inliers.size ());
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (6, coeff.size ());
EXPECT_NEAR (1, coeff[4] / coeff[3], 1e-4);
EXPECT_NEAR (1, coeff[5] / coeff[3], 1e-4);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (6, coeff_refined.size ());
EXPECT_NEAR (1, coeff[4] / coeff[3], 1e-4);
EXPECT_NEAR (1, coeff[5] / coeff[3], 1e-4);
// Projection tests
PointCloud<PointXYZ> proj_points;
model->projectPoints (inliers, coeff_refined, proj_points);
EXPECT_XYZ_NEAR (PointXYZ ( 7.0, 8.0, 9.0), proj_points.points[2], 1e-4);
EXPECT_XYZ_NEAR (PointXYZ (10.0, 11.0, 12.0), proj_points.points[3], 1e-4);
EXPECT_XYZ_NEAR (PointXYZ (16.0, 17.0, 18.0), proj_points.points[5], 1e-4);
}
TEST (RANSAC, SampleConsensusModelLine)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
cloud.points.resize (10);
cloud.points[0].x = 1.0f; cloud.points[0].y = 2.0f; cloud.points[0].z = 3.0f;
cloud.points[1].x = 4.0f; cloud.points[1].y = 5.0f; cloud.points[1].z = 6.0f;
cloud.points[2].x = 7.0f; cloud.points[2].y = 8.0f; cloud.points[2].z = 9.0f;
cloud.points[3].x = 10.0f; cloud.points[3].y = 11.0f; cloud.points[3].z = 12.0f;
cloud.points[4].x = 13.0f; cloud.points[4].y = 14.0f; cloud.points[4].z = 15.0f;
cloud.points[5].x = 16.0f; cloud.points[5].y = 17.0f; cloud.points[5].z = 18.0f;
cloud.points[6].x = 19.0f; cloud.points[6].y = 20.0f; cloud.points[6].z = 21.0f;
cloud.points[7].x = 22.0f; cloud.points[7].y = 23.0f; cloud.points[7].z = 24.0f;
cloud.points[8].x = -5.0f; cloud.points[8].y = 1.57f; cloud.points[8].z = 0.75f;
cloud.points[9].x = 4.0f; cloud.points[9].y = 2.0f; cloud.points[9].z = 3.0f;
// Create a shared line model pointer directly
SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.001);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 2);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 8);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 6);
EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4);
EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 6);
EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4);
EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4);
// Projection tests
PointCloud<PointXYZ> proj_points;
model->projectPoints (inliers, coeff_refined, proj_points);
EXPECT_NEAR (proj_points.points[2].x, 7.0, 1e-4);
EXPECT_NEAR (proj_points.points[2].y, 8.0, 1e-4);
EXPECT_NEAR (proj_points.points[2].z, 9.0, 1e-4);
EXPECT_NEAR (proj_points.points[3].x, 10.0, 1e-4);
EXPECT_NEAR (proj_points.points[3].y, 11.0, 1e-4);
EXPECT_NEAR (proj_points.points[3].z, 12.0, 1e-4);
EXPECT_NEAR (proj_points.points[5].x, 16.0, 1e-4);
EXPECT_NEAR (proj_points.points[5].y, 17.0, 1e-4);
EXPECT_NEAR (proj_points.points[5].z, 18.0, 1e-4);
}