TEST (SampleConsensusModelParallelLine, RANSAC)
{
PointCloud<PointXYZ> cloud (16, 1);
// Line 1
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;
// Random points
cloud.points[8].getVector3fMap () << -5.0f, 1.57f, 0.75f;
cloud.points[9].getVector3fMap () << 4.0f, 2.00f, 3.00f;
// Line 2 (parallel to the Z axis)
cloud.points[10].getVector3fMap () << -1.00f, 5.00f, 0.0f;
cloud.points[11].getVector3fMap () << -1.05f, 5.01f, 1.0f;
cloud.points[12].getVector3fMap () << -1.01f, 5.05f, 2.0f;
cloud.points[13].getVector3fMap () << -1.05f, 5.01f, 3.0f;
cloud.points[14].getVector3fMap () << -1.01f, 5.05f, 4.0f;
cloud.points[15].getVector3fMap () << -1.05f, 5.01f, 5.0f;
// Create a shared line model pointer directly
SampleConsensusModelParallelLinePtr model (new SampleConsensusModelParallelLine<PointXYZ> (cloud.makeShared ()));
model->setAxis (Eigen::Vector3f (0, 0, 1));
model->setEpsAngle (0.1);
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.1);
// 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 (6, inliers.size ());
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (6, coeff.size ());
EXPECT_NEAR (0, coeff[3], 1e-4);
EXPECT_NEAR (0, coeff[4], 1e-4);
EXPECT_NEAR (1, coeff[5], 1e-4);
// Projection tests
PointCloud<PointXYZ> proj_points;
model->projectPoints (inliers, coeff, proj_points);
EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 3.0), proj_points.points[13], 0.1);
EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 4.0), proj_points.points[14], 0.1);
}
TEST (SampleConsensusModelParallelLine, RANSAC)
{
PointCloud<PointXYZ> cloud (16, 1);
// Line 1
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;
// Random points
cloud.points[8].getVector3fMap () << -5.0f, 1.57f, 0.75f;
cloud.points[9].getVector3fMap () << 4.0f, 2.00f, 3.00f;
// Line 2 (parallel to the Z axis)
cloud.points[10].getVector3fMap () << -1.00f, 5.00f, 0.0f;
cloud.points[11].getVector3fMap () << -1.05f, 5.01f, 1.0f;
cloud.points[12].getVector3fMap () << -1.01f, 5.05f, 2.0f;
cloud.points[13].getVector3fMap () << -1.05f, 5.01f, 3.0f;
cloud.points[14].getVector3fMap () << -1.01f, 5.05f, 4.0f;
cloud.points[15].getVector3fMap () << -1.05f, 5.01f, 5.0f;
// Create a shared line model pointer directly
const double eps = 0.1; //angle eps in radians
const Eigen::Vector3f axis (0, 0, 1);
SampleConsensusModelParallelLinePtr model (new SampleConsensusModelParallelLine<PointXYZ> (cloud.makeShared ()));
model->setAxis (axis);
model->setEpsAngle (eps);
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.1);
// 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 (6, inliers.size ());
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (6, coeff.size ());
// Make sure the returned direction respects the angular constraint
double angle_diff = getAngle3D (axis, coeff.tail<3> ());
angle_diff = std::min (angle_diff, M_PI - angle_diff);
EXPECT_GT (eps, angle_diff);
// Projection tests
PointCloud<PointXYZ> proj_points;
model->projectPoints (inliers, coeff, proj_points);
EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 3.0), proj_points.points[13], 0.1);
EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 4.0), proj_points.points[14], 0.1);
}
