void
callback (const sensor_msgs::Image::ConstPtr &depth,
const sensor_msgs::Image::ConstPtr &rgb,
const sensor_msgs::CameraInfo::ConstPtr &info)
{
//typedef pcl_cuda::SampleConsensusModel<pcl_cuda::Host>::Indices Indices;
//pcl_cuda::PointCloudAOS<pcl_cuda::Host>::Ptr data (new pcl_cuda::PointCloudAOS<pcl_cuda::Host>);
PointCloudAOS<Device>::Ptr data;
d2c.callback (depth, rgb, info, data);
SampleConsensusModelPlane<Device>::Ptr sac_model (new SampleConsensusModelPlane<Device> (data));
// SampleConsensusModelPlane<Host>::Ptr sac_model (new pcl_cuda::SampleConsensusModelPlane<pcl_cuda::Host> (data));
RandomSampleConsensus<Device> sac (sac_model);
sac.setDistanceThreshold (0.05);
if (!sac.computeModel (2))
std::cerr << "Failed to compute model" << std::endl;
// Convert data from Thrust/HOST to PCL
pcl::PointCloud<pcl::PointXYZRGB> output;
// pcl_cuda::toPCL (*data, output);
output.header.stamp = ros::Time::now ();
pub.publish (output);
}
TEST (ModelOutlierRemoval, Model_Outlier_Filter)
{
PointCloud<PointXYZ>::Ptr cloud_filter_out (new PointCloud<PointXYZ>);
std::vector<int> ransac_inliers;
float thresh = 0.01;
//run ransac
Eigen::VectorXf model_coefficients;
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud_in));
RandomSampleConsensus < pcl::PointXYZ > ransac (model_p);
ransac.setDistanceThreshold (thresh);
ransac.computeModel ();
ransac.getInliers (ransac_inliers);
ransac.getModelCoefficients (model_coefficients);
// test ransacs result
EXPECT_EQ (model_coefficients.size (), 4);
if (model_coefficients.size () != 4)
return;
//run filter
pcl::ModelCoefficients model_coeff;
model_coeff.values.resize (4);
for (int i = 0; i < 4; i++)
model_coeff.values[i] = model_coefficients[i];
pcl::ModelOutlierRemoval < pcl::PointXYZ > filter;
filter.setModelCoefficients (model_coeff);
filter.setThreshold (thresh);
filter.setModelType (pcl::SACMODEL_PLANE);
filter.setInputCloud (cloud_in);
filter.filter (*cloud_filter_out);
//compare results
EXPECT_EQ (cloud_filter_out->points.size (), ransac_inliers.size ());
//TODO: also compare content
}
TEST (RANSAC, SampleConsensusModelCircle2D)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
cloud.points.resize (18);
cloud.points[0].x = 3.587751f; cloud.points[0].y = -4.190982f; cloud.points[0].z = 0.0f;
cloud.points[1].x = 3.808883f; cloud.points[1].y = -4.412265f; cloud.points[1].z = 0.0f;
cloud.points[2].x = 3.587525f; cloud.points[2].y = -5.809143f; cloud.points[2].z = 0.0f;
cloud.points[3].x = 2.999913f; cloud.points[3].y = -5.999980f; cloud.points[3].z = 0.0f;
cloud.points[4].x = 2.412224f; cloud.points[4].y = -5.809090f; cloud.points[4].z = 0.0f;
cloud.points[5].x = 2.191080f; cloud.points[5].y = -5.587682f; cloud.points[5].z = 0.0f;
cloud.points[6].x = 2.048941f; cloud.points[6].y = -5.309003f; cloud.points[6].z = 0.0f;
cloud.points[7].x = 2.000397f; cloud.points[7].y = -4.999944f; cloud.points[7].z = 0.0f;
cloud.points[8].x = 2.999953f; cloud.points[8].y = -6.000056f; cloud.points[8].z = 0.0f;
cloud.points[9].x = 2.691127f; cloud.points[9].y = -5.951136f; cloud.points[9].z = 0.0f;
cloud.points[10].x = 2.190892f; cloud.points[10].y = -5.587838f; cloud.points[10].z = 0.0f;
cloud.points[11].x = 2.048874f; cloud.points[11].y = -5.309052f; cloud.points[11].z = 0.0f;
cloud.points[12].x = 1.999990f; cloud.points[12].y = -5.000147f; cloud.points[12].z = 0.0f;
cloud.points[13].x = 2.049026f; cloud.points[13].y = -4.690918f; cloud.points[13].z = 0.0f;
cloud.points[14].x = 2.190956f; cloud.points[14].y = -4.412162f; cloud.points[14].z = 0.0f;
cloud.points[15].x = 2.412231f; cloud.points[15].y = -4.190918f; cloud.points[15].z = 0.0f;
cloud.points[16].x = 2.691027f; cloud.points[16].y = -4.049060f; cloud.points[16].z = 0.0f;
cloud.points[17].x = 2.0f; cloud.points[17].y = -3.0f; cloud.points[17].z = 0.0f;
// Create a shared 2d circle model pointer directly
SampleConsensusModelCircle2DPtr model (new SampleConsensusModelCircle2D<PointXYZ> (cloud.makeShared ()));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 3);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 17);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 3);
EXPECT_NEAR (coeff[0], 3, 1e-3);
EXPECT_NEAR (coeff[1], -5, 1e-3);
EXPECT_NEAR (coeff[2], 1, 1e-3);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 3);
EXPECT_NEAR (coeff_refined[0], 3, 1e-3);
EXPECT_NEAR (coeff_refined[1], -5, 1e-3);
EXPECT_NEAR (coeff_refined[2], 1, 1e-3);
}
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 (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);
}
int main(int argc, char** argv)
{
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
PointCloud<PointXYZ>::Ptr final (new PointCloud<PointXYZ>);
cloud->width = 500;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize(cloud->width * cloud->height);
for (size_t i=0; i<cloud->points.size(); ++i)
{
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.00);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.00);
if (console::find_argument(argc, argv, "-s") >= 0 || console::find_argument(argc, argv, "-sf") >= 0)
{
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.00);
else if (i % 2 == 0)
cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = -sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{
if (i % 2 == 0)
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.00);
else
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
}
}
vector<int> inliers;
SampleConsensusModelSphere<PointXYZ>::Ptr
model_s(new SampleConsensusModelSphere<PointXYZ>(cloud));
SampleConsensusModelPlane<PointXYZ>::Ptr
model_p(new SampleConsensusModelPlane<PointXYZ>(cloud));
if (console::find_argument(argc, argv, "-f") >= 0)
{
RandomSampleConsensus<PointXYZ> ransac (model_p);
ransac.setDistanceThreshold(0.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
else if (console::find_argument(argc, argv, "-sf") >= 0)
{
RandomSampleConsensus<PointXYZ> ransac (model_s);
ransac.setDistanceThreshold(0.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
copyPointCloud<PointXYZ>(*cloud, inliers, *final);
boost::shared_ptr<visualization::PCLVisualizer> viewer;
if (console::find_argument(argc, argv, "-f") >= 0 || console::find_argument(argc, argv, "-sf") >= 0)
viewer = simpleVis(final);
else
TEST (RANSAC, SampleConsensusModelSphere)
{
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.7068f; cloud.points[0].y = 1.0684f; cloud.points[0].z = 2.2147f;
cloud.points[1].x = 2.4708f; cloud.points[1].y = 2.3081f; cloud.points[1].z = 1.1736f;
cloud.points[2].x = 2.7609f; cloud.points[2].y = 1.9095f; cloud.points[2].z = 1.3574f;
cloud.points[3].x = 2.8016f; cloud.points[3].y = 1.6704f; cloud.points[3].z = 1.5009f;
cloud.points[4].x = 1.8517f; cloud.points[4].y = 2.0276f; cloud.points[4].z = 1.0112f;
cloud.points[5].x = 1.8726f; cloud.points[5].y = 1.3539f; cloud.points[5].z = 2.7523f;
cloud.points[6].x = 2.5179f; cloud.points[6].y = 2.3218f; cloud.points[6].z = 1.2074f;
cloud.points[7].x = 2.4026f; cloud.points[7].y = 2.5114f; cloud.points[7].z = 2.7588f;
cloud.points[8].x = 2.6999f; cloud.points[8].y = 2.5606f; cloud.points[8].z = 1.5571f;
cloud.points[9].x = 0.0f; cloud.points[9].y = 0.0f; cloud.points[9].z = 0.0f;
// Create a shared sphere model pointer directly
SampleConsensusModelSpherePtr model (new SampleConsensusModelSphere<PointXYZ> (cloud.makeShared ()));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 4);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 9);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 4);
EXPECT_NEAR (coeff[0]/coeff[3], 2, 1e-2);
EXPECT_NEAR (coeff[1]/coeff[3], 2, 1e-2);
EXPECT_NEAR (coeff[2]/coeff[3], 2, 1e-2);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 4);
EXPECT_NEAR (coeff_refined[0]/coeff_refined[3], 2, 1e-2);
EXPECT_NEAR (coeff_refined[1]/coeff_refined[3], 2, 1e-2);
EXPECT_NEAR (coeff_refined[2]/coeff_refined[3], 2, 1e-2);
}
TEST (SampleConsensusModelLine, OnGroundPlane)
{
PointCloud<PointXYZ> cloud;
cloud.points.resize (10);
// All the points are on the ground plane (z=0).
// The line is parallel to the x axis, so all the inlier points have the same z and y coordinates.
cloud.points[0].getVector3fMap () << 0.0f, 0.0f, 0.0f;
cloud.points[1].getVector3fMap () << 1.0f, 0.0f, 0.0f;
cloud.points[2].getVector3fMap () << 2.0f, 0.0f, 0.0f;
cloud.points[3].getVector3fMap () << 3.0f, 0.0f, 0.0f;
cloud.points[4].getVector3fMap () << 4.0f, 0.0f, 0.0f;
cloud.points[5].getVector3fMap () << 5.0f, 0.0f, 0.0f;
// Outliers
cloud.points[6].getVector3fMap () << 2.1f, 2.0f, 0.0f;
cloud.points[7].getVector3fMap () << 5.0f, 4.1f, 0.0f;
cloud.points[8].getVector3fMap () << 0.4f, 1.3f, 0.0f;
cloud.points[9].getVector3fMap () << 3.3f, 0.1f, 0.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_TRUE (result);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (6, inliers.size ());
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (6, coeff.size ());
EXPECT_NE (0, coeff[3]);
EXPECT_NEAR (0, coeff[4], 1e-4);
EXPECT_NEAR (0, coeff[5], 1e-4);
}
TEST (RANSAC, Base)
{
// Create a shared plane model pointer directly
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (cloud_));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Basic tests
ASSERT_EQ (sac.getDistanceThreshold (), 0.03);
sac.setDistanceThreshold (0.03);
ASSERT_EQ (sac.getDistanceThreshold (), 0.03);
sac.setProbability (0.99);
ASSERT_EQ (sac.getProbability (), 0.99);
sac.setMaxIterations (10000);
ASSERT_EQ (sac.getMaxIterations (), 10000);
}
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);
}
TEST (RANSAC, SampleConsensusModelCircle3D)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
cloud.points.resize (20);
cloud.points[0].x = 1.0f; cloud.points[0].y = 5.0f; cloud.points[0].z = -2.9000001f;
cloud.points[1].x = 1.034202f; cloud.points[1].y = 5.0f; cloud.points[1].z = -2.9060307f;
cloud.points[2].x = 1.0642787f; cloud.points[2].y = 5.0f; cloud.points[2].z = -2.9233956f;
cloud.points[3].x = 1.0866026f; cloud.points[3].y = 5.0f; cloud.points[3].z = -2.95f;
cloud.points[4].x = 1.0984808f; cloud.points[4].y = 5.0f; cloud.points[4].z = -2.9826353f;
cloud.points[5].x = 1.0984808f; cloud.points[5].y = 5.0f; cloud.points[5].z = -3.0173647f;
cloud.points[6].x = 1.0866026f; cloud.points[6].y = 5.0f; cloud.points[6].z = -3.05f;
cloud.points[7].x = 1.0642787f; cloud.points[7].y = 5.0f; cloud.points[7].z = -3.0766044f;
cloud.points[8].x = 1.034202f; cloud.points[8].y = 5.0f; cloud.points[8].z = -3.0939693f;
cloud.points[9].x = 1.0f; cloud.points[9].y = 5.0f; cloud.points[9].z = -3.0999999f;
cloud.points[10].x = 0.96579796f; cloud.points[10].y = 5.0f; cloud.points[10].z = -3.0939693f;
cloud.points[11].x = 0.93572122f; cloud.points[11].y = 5.0f; cloud.points[11].z = -3.0766044f;
cloud.points[12].x = 0.91339743f; cloud.points[12].y = 5.0f; cloud.points[12].z = -3.05f;
cloud.points[13].x = 0.90151924f; cloud.points[13].y = 5.0f; cloud.points[13].z = -3.0173647f;
cloud.points[14].x = 0.90151924f; cloud.points[14].y = 5.0f; cloud.points[14].z = -2.9826353f;
cloud.points[15].x = 0.91339743f; cloud.points[15].y = 5.0f; cloud.points[15].z = -2.95f;
cloud.points[16].x = 0.93572122f; cloud.points[16].y = 5.0f; cloud.points[16].z = -2.9233956f;
cloud.points[17].x = 0.96579796f; cloud.points[17].y = 5.0; cloud.points[17].z = -2.9060307f;
cloud.points[18].x = 0.85000002f; cloud.points[18].y = 4.8499999f; cloud.points[18].z = -3.1500001f;
cloud.points[19].x = 1.15f; cloud.points[19].y = 5.1500001f; cloud.points[19].z = -2.8499999f;
// Create a shared 3d circle model pointer directly
SampleConsensusModelCircle3DPtr model (new SampleConsensusModelCircle3D<PointXYZ> (cloud.makeShared ()));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 3);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 18);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 7);
EXPECT_NEAR (coeff[0], 1, 1e-3);
EXPECT_NEAR (coeff[1], 5, 1e-3);
EXPECT_NEAR (coeff[2], -3, 1e-3);
EXPECT_NEAR (coeff[3],0.1, 1e-3);
EXPECT_NEAR (coeff[4], 0, 1e-3);
EXPECT_NEAR (coeff[5], -1, 1e-3);
EXPECT_NEAR (coeff[6], 0, 1e-3);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 7);
EXPECT_NEAR (coeff_refined[0], 1, 1e-3);
EXPECT_NEAR (coeff_refined[1], 5, 1e-3);
EXPECT_NEAR (coeff_refined[2], -3, 1e-3);
EXPECT_NEAR (coeff_refined[3],0.1, 1e-3);
EXPECT_NEAR (coeff_refined[4], 0, 1e-3);
EXPECT_NEAR (coeff_refined[5], -1, 1e-3);
EXPECT_NEAR (coeff_refined[6], 0, 1e-3);
}
TEST (RANSAC, SampleConsensusModelCylinder)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
PointCloud<Normal> normals;
cloud.points.resize (20); normals.points.resize (20);
cloud.points[0].x = -0.499902f; cloud.points[0].y = 2.199701f; cloud.points[0].z = 0.000008f;
cloud.points[1].x = -0.875397f; cloud.points[1].y = 2.030177f; cloud.points[1].z = 0.050104f;
cloud.points[2].x = -0.995875f; cloud.points[2].y = 1.635973f; cloud.points[2].z = 0.099846f;
cloud.points[3].x = -0.779523f; cloud.points[3].y = 1.285527f; cloud.points[3].z = 0.149961f;
cloud.points[4].x = -0.373285f; cloud.points[4].y = 1.216488f; cloud.points[4].z = 0.199959f;
cloud.points[5].x = -0.052893f; cloud.points[5].y = 1.475973f; cloud.points[5].z = 0.250101f;
cloud.points[6].x = -0.036558f; cloud.points[6].y = 1.887591f; cloud.points[6].z = 0.299839f;
cloud.points[7].x = -0.335048f; cloud.points[7].y = 2.171994f; cloud.points[7].z = 0.350001f;
cloud.points[8].x = -0.745456f; cloud.points[8].y = 2.135528f; cloud.points[8].z = 0.400072f;
cloud.points[9].x = -0.989282f; cloud.points[9].y = 1.803311f; cloud.points[9].z = 0.449983f;
cloud.points[10].x = -0.900651f; cloud.points[10].y = 1.400701f; cloud.points[10].z = 0.500126f;
cloud.points[11].x = -0.539658f; cloud.points[11].y = 1.201468f; cloud.points[11].z = 0.550079f;
cloud.points[12].x = -0.151875f; cloud.points[12].y = 1.340951f; cloud.points[12].z = 0.599983f;
cloud.points[13].x = -0.000724f; cloud.points[13].y = 1.724373f; cloud.points[13].z = 0.649882f;
cloud.points[14].x = -0.188573f; cloud.points[14].y = 2.090983f; cloud.points[14].z = 0.699854f;
cloud.points[15].x = -0.587925f; cloud.points[15].y = 2.192257f; cloud.points[15].z = 0.749956f;
cloud.points[16].x = -0.927724f; cloud.points[16].y = 1.958846f; cloud.points[16].z = 0.800008f;
cloud.points[17].x = -0.976888f; cloud.points[17].y = 1.549655f; cloud.points[17].z = 0.849970f;
cloud.points[18].x = -0.702003f; cloud.points[18].y = 1.242707f; cloud.points[18].z = 0.899954f;
cloud.points[19].x = -0.289916f; cloud.points[19].y = 1.246296f; cloud.points[19].z = 0.950075f;
normals.points[0].normal[0] = 0.000098f; normals.points[0].normal[1] = 1.000098f; normals.points[0].normal[2] = 0.000008f;
normals.points[1].normal[0] = -0.750891f; normals.points[1].normal[1] = 0.660413f; normals.points[1].normal[2] = 0.000104f;
normals.points[2].normal[0] = -0.991765f; normals.points[2].normal[1] = -0.127949f; normals.points[2].normal[2] = -0.000154f;
normals.points[3].normal[0] = -0.558918f; normals.points[3].normal[1] = -0.829439f; normals.points[3].normal[2] = -0.000039f;
normals.points[4].normal[0] = 0.253627f; normals.points[4].normal[1] = -0.967447f; normals.points[4].normal[2] = -0.000041f;
normals.points[5].normal[0] = 0.894105f; normals.points[5].normal[1] = -0.447965f; normals.points[5].normal[2] = 0.000101f;
normals.points[6].normal[0] = 0.926852f; normals.points[6].normal[1] = 0.375543f; normals.points[6].normal[2] = -0.000161f;
normals.points[7].normal[0] = 0.329948f; normals.points[7].normal[1] = 0.943941f; normals.points[7].normal[2] = 0.000001f;
normals.points[8].normal[0] = -0.490966f; normals.points[8].normal[1] = 0.871203f; normals.points[8].normal[2] = 0.000072f;
normals.points[9].normal[0] = -0.978507f; normals.points[9].normal[1] = 0.206425f; normals.points[9].normal[2] = -0.000017f;
normals.points[10].normal[0] = -0.801227f; normals.points[10].normal[1] = -0.598534f; normals.points[10].normal[2] = 0.000126f;
normals.points[11].normal[0] = -0.079447f; normals.points[11].normal[1] = -0.996697f; normals.points[11].normal[2] = 0.000079f;
normals.points[12].normal[0] = 0.696154f; normals.points[12].normal[1] = -0.717889f; normals.points[12].normal[2] = -0.000017f;
normals.points[13].normal[0] = 0.998685f; normals.points[13].normal[1] = 0.048502f; normals.points[13].normal[2] = -0.000118f;
normals.points[14].normal[0] = 0.622933f; normals.points[14].normal[1] = 0.782133f; normals.points[14].normal[2] = -0.000146f;
normals.points[15].normal[0] = -0.175948f; normals.points[15].normal[1] = 0.984480f; normals.points[15].normal[2] = -0.000044f;
normals.points[16].normal[0] = -0.855476f; normals.points[16].normal[1] = 0.517824f; normals.points[16].normal[2] = 0.000008f;
normals.points[17].normal[0] = -0.953769f; normals.points[17].normal[1] = -0.300571f; normals.points[17].normal[2] = -0.000030f;
normals.points[18].normal[0] = -0.404035f; normals.points[18].normal[1] = -0.914700f; normals.points[18].normal[2] = -0.000046f;
normals.points[19].normal[0] = 0.420154f; normals.points[19].normal[1] = -0.907445f; normals.points[19].normal[2] = 0.000075f;
// Create a shared cylinder model pointer directly
SampleConsensusModelCylinderPtr model (new SampleConsensusModelCylinder<PointXYZ, Normal> (cloud.makeShared ()));
model->setInputNormals (normals.makeShared ());
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// 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 ()), 20);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 7);
EXPECT_NEAR (coeff[0], -0.5, 1e-3);
EXPECT_NEAR (coeff[1], 1.7, 1e-3);
EXPECT_NEAR (coeff[6], 0.5, 1e-3);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 7);
EXPECT_NEAR (coeff_refined[6], 0.5, 1e-3);
}
TEST (RANSAC, SampleConsensusModelCone)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
PointCloud<Normal> normals;
cloud.points.resize (31); normals.points.resize (31);
cloud.points[0].x = -0.011247f; cloud.points[0].y = 0.200000f; cloud.points[0].z = 0.965384f;
cloud.points[1].x = 0.000000f; cloud.points[1].y = 0.200000f; cloud.points[1].z = 0.963603f;
cloud.points[2].x = 0.011247f; cloud.points[2].y = 0.200000f; cloud.points[2].z = 0.965384f;
cloud.points[3].x = -0.016045f; cloud.points[3].y = 0.175000f; cloud.points[3].z = 0.977916f;
cloud.points[4].x = -0.008435f; cloud.points[4].y = 0.175000f; cloud.points[4].z = 0.974038f;
cloud.points[5].x = 0.004218f; cloud.points[5].y = 0.175000f; cloud.points[5].z = 0.973370f;
cloud.points[6].x = 0.016045f; cloud.points[6].y = 0.175000f; cloud.points[6].z = 0.977916f;
cloud.points[7].x = -0.025420f; cloud.points[7].y = 0.200000f; cloud.points[7].z = 0.974580f;
cloud.points[8].x = 0.025420f; cloud.points[8].y = 0.200000f; cloud.points[8].z = 0.974580f;
cloud.points[9].x = -0.012710f; cloud.points[9].y = 0.150000f; cloud.points[9].z = 0.987290f;
cloud.points[10].x = -0.005624f; cloud.points[10].y = 0.150000f; cloud.points[10].z = 0.982692f;
cloud.points[11].x = 0.002812f; cloud.points[11].y = 0.150000f; cloud.points[11].z = 0.982247f;
cloud.points[12].x = 0.012710f; cloud.points[12].y = 0.150000f; cloud.points[12].z = 0.987290f;
cloud.points[13].x = -0.022084f; cloud.points[13].y = 0.175000f; cloud.points[13].z = 0.983955f;
cloud.points[14].x = 0.022084f; cloud.points[14].y = 0.175000f; cloud.points[14].z = 0.983955f;
cloud.points[15].x = -0.034616f; cloud.points[15].y = 0.200000f; cloud.points[15].z = 0.988753f;
cloud.points[16].x = 0.034616f; cloud.points[16].y = 0.200000f; cloud.points[16].z = 0.988753f;
cloud.points[17].x = -0.006044f; cloud.points[17].y = 0.125000f; cloud.points[17].z = 0.993956f;
cloud.points[18].x = 0.004835f; cloud.points[18].y = 0.125000f; cloud.points[18].z = 0.993345f;
cloud.points[19].x = -0.017308f; cloud.points[19].y = 0.150000f; cloud.points[19].z = 0.994376f;
cloud.points[20].x = 0.017308f; cloud.points[20].y = 0.150000f; cloud.points[20].z = 0.994376f;
cloud.points[21].x = -0.025962f; cloud.points[21].y = 0.175000f; cloud.points[21].z = 0.991565f;
cloud.points[22].x = 0.025962f; cloud.points[22].y = 0.175000f; cloud.points[22].z = 0.991565f;
cloud.points[23].x = -0.009099f; cloud.points[23].y = 0.125000f; cloud.points[23].z = 1.000000f;
cloud.points[24].x = 0.009099f; cloud.points[24].y = 0.125000f; cloud.points[24].z = 1.000000f;
cloud.points[25].x = -0.018199f; cloud.points[25].y = 0.150000f; cloud.points[25].z = 1.000000f;
cloud.points[26].x = 0.018199f; cloud.points[26].y = 0.150000f; cloud.points[26].z = 1.000000f;
cloud.points[27].x = -0.027298f; cloud.points[27].y = 0.175000f; cloud.points[27].z = 1.000000f;
cloud.points[28].x = 0.027298f; cloud.points[28].y = 0.175000f; cloud.points[28].z = 1.000000f;
cloud.points[29].x = -0.036397f; cloud.points[29].y = 0.200000f; cloud.points[29].z = 1.000000f;
cloud.points[30].x = 0.036397f; cloud.points[30].y = 0.200000f; cloud.points[30].z = 1.000000f;
normals.points[0].normal[0] = -0.290381f; normals.points[0].normal[1] = -0.342020f; normals.points[0].normal[2] = -0.893701f;
normals.points[1].normal[0] = 0.000000f; normals.points[1].normal[1] = -0.342020f; normals.points[1].normal[2] = -0.939693f;
normals.points[2].normal[0] = 0.290381f; normals.points[2].normal[1] = -0.342020f; normals.points[2].normal[2] = -0.893701f;
normals.points[3].normal[0] = -0.552338f; normals.points[3].normal[1] = -0.342020f; normals.points[3].normal[2] = -0.760227f;
normals.points[4].normal[0] = -0.290381f; normals.points[4].normal[1] = -0.342020f; normals.points[4].normal[2] = -0.893701f;
normals.points[5].normal[0] = 0.145191f; normals.points[5].normal[1] = -0.342020f; normals.points[5].normal[2] = -0.916697f;
normals.points[6].normal[0] = 0.552337f; normals.points[6].normal[1] = -0.342020f; normals.points[6].normal[2] = -0.760227f;
normals.points[7].normal[0] = -0.656282f; normals.points[7].normal[1] = -0.342020f; normals.points[7].normal[2] = -0.656283f;
normals.points[8].normal[0] = 0.656282f; normals.points[8].normal[1] = -0.342020f; normals.points[8].normal[2] = -0.656283f;
normals.points[9].normal[0] = -0.656283f; normals.points[9].normal[1] = -0.342020f; normals.points[9].normal[2] = -0.656282f;
normals.points[10].normal[0] = -0.290381f; normals.points[10].normal[1] = -0.342020f; normals.points[10].normal[2] = -0.893701f;
normals.points[11].normal[0] = 0.145191f; normals.points[11].normal[1] = -0.342020f; normals.points[11].normal[2] = -0.916697f;
normals.points[12].normal[0] = 0.656282f; normals.points[12].normal[1] = -0.342020f; normals.points[12].normal[2] = -0.656282f;
normals.points[13].normal[0] = -0.760228f; normals.points[13].normal[1] = -0.342020f; normals.points[13].normal[2] = -0.552337f;
normals.points[14].normal[0] = 0.760228f; normals.points[14].normal[1] = -0.342020f; normals.points[14].normal[2] = -0.552337f;
normals.points[15].normal[0] = -0.893701f; normals.points[15].normal[1] = -0.342020f; normals.points[15].normal[2] = -0.290380f;
normals.points[16].normal[0] = 0.893701f; normals.points[16].normal[1] = -0.342020f; normals.points[16].normal[2] = -0.290380f;
normals.points[17].normal[0] = -0.624162f; normals.points[17].normal[1] = -0.342020f; normals.points[17].normal[2] = -0.624162f;
normals.points[18].normal[0] = 0.499329f; normals.points[18].normal[1] = -0.342020f; normals.points[18].normal[2] = -0.687268f;
normals.points[19].normal[0] = -0.893701f; normals.points[19].normal[1] = -0.342020f; normals.points[19].normal[2] = -0.290380f;
normals.points[20].normal[0] = 0.893701f; normals.points[20].normal[1] = -0.342020f; normals.points[20].normal[2] = -0.290380f;
normals.points[21].normal[0] = -0.893701f; normals.points[21].normal[1] = -0.342020f; normals.points[21].normal[2] = -0.290381f;
normals.points[22].normal[0] = 0.893701f; normals.points[22].normal[1] = -0.342020f; normals.points[22].normal[2] = -0.290381f;
normals.points[23].normal[0] = -0.939693f; normals.points[23].normal[1] = -0.342020f; normals.points[23].normal[2] = 0.000000f;
normals.points[24].normal[0] = 0.939693f; normals.points[24].normal[1] = -0.342020f; normals.points[24].normal[2] = 0.000000f;
normals.points[25].normal[0] = -0.939693f; normals.points[25].normal[1] = -0.342020f; normals.points[25].normal[2] = 0.000000f;
normals.points[26].normal[0] = 0.939693f; normals.points[26].normal[1] = -0.342020f; normals.points[26].normal[2] = 0.000000f;
normals.points[27].normal[0] = -0.939693f; normals.points[27].normal[1] = -0.342020f; normals.points[27].normal[2] = 0.000000f;
normals.points[28].normal[0] = 0.939693f; normals.points[28].normal[1] = -0.342020f; normals.points[28].normal[2] = 0.000000f;
normals.points[29].normal[0] = -0.939693f; normals.points[29].normal[1] = -0.342020f; normals.points[29].normal[2] = 0.000000f;
normals.points[30].normal[0] = 0.939693f; normals.points[30].normal[1] = -0.342020f; normals.points[30].normal[2] = 0.000000f;
// Create a shared cylinder model pointer directly
SampleConsensusModelConePtr model (new SampleConsensusModelCone<PointXYZ, Normal> (cloud.makeShared ()));
model->setInputNormals (normals.makeShared ());
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 3);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 31);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 7);
EXPECT_NEAR (coeff[0], 0, 1e-2);
EXPECT_NEAR (coeff[1], 0.1, 1e-2);
EXPECT_NEAR (coeff[6], 0.349066, 1e-2);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 7);
EXPECT_NEAR (coeff_refined[6], 0.349066 , 1e-2);
}
template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
// Allocate enough space to hold the results
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudTarget input_corresp;
input_corresp.points.resize (indices_->size ());
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
// If the guessed transformation is non identity
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Resize the vector of distances between correspondences
std::vector<float> previous_correspondence_distances (indices_->size ());
correspondence_distances_.resize (indices_->size ());
while (!converged_) // repeat until convergence
{
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// And the previous set of distances
previous_correspondence_distances = correspondence_distances_;
int cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// Iterating over the entire index vector and find all correspondences
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
{
PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
source_indices[cnt] = (*indices_)[idx];
target_indices[cnt] = nn_indices[0];
cnt++;
}
// Save the nn_dists[0] to a global vector of distances
correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], static_cast<float> (dist_threshold));
}
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
// Resize to the actual number of valid correspondences
source_indices.resize (cnt); target_indices.resize (cnt);
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
sac.setMaxIterations (ransac_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
source_indices_good = source_indices;
target_indices_good = target_indices;
}
else
{
std::vector<int> inliers;
// Get the inliers
sac.getInliers (inliers);
source_indices_good.resize (inliers.size ());
target_indices_good.resize (inliers.size ());
boost::unordered_map<int, int> source_to_target;
for (unsigned int i = 0; i < source_indices.size(); ++i)
source_to_target[source_indices[i]] = target_indices[i];
// Copy just the inliers
std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
for (size_t i = 0; i < inliers.size (); ++i)
target_indices_good[i] = source_to_target[inliers[i]];
}
}
// Check whether we have enough correspondences
cnt = static_cast<int> (source_indices_good.size ());
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %zu points [100.0%%], RANSAC rejected: %zu [%f%%].\n",
getClassName ().c_str (),
cnt,
(static_cast<float> (cnt) * 100.0f) / static_cast<float> (indices_->size ()),
indices_->size (),
source_indices.size () - cnt,
static_cast<float> (source_indices.size () - cnt) * 100.0f / static_cast<float> (source_indices.size ()));
// Estimate the transform
//rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);
// Tranform the data
transformPointCloud (output, output, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
nr_iterations_++;
// Update the vizualization of icp convergence
if (update_visualizer_ != 0)
update_visualizer_(output, source_indices_good, *target_, target_indices_good );
// Various/Different convergence termination criteria
// 1. Number of iterations has reached the maximum user imposed number of iterations (via
// setMaximumIterations)
// 2. The epsilon (difference) between the previous transformation and the current estimated transformation
// is smaller than an user imposed value (via setTransformationEpsilon)
// 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via
// setEuclideanFitnessEpsilon)
if (nr_iterations_ >= max_iterations_ ||
(transformation_ - previous_transformation_).array ().abs ().sum () < transformation_epsilon_ ||
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
)
{
converged_ = true;
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
PCL_DEBUG ("(transformation_ - previous_transformation_).array ().abs ().sum () (%f) < transformation_epsilon_ (%f)\n",
(transformation_ - previous_transformation_).array ().abs ().sum (), transformation_epsilon_);
PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
euclidean_fitness_epsilon_);
}
}
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
int max_iterations = 1000, double threshold = 0.05, bool negative = false)
{
// Convert data to PointCloud<T>
PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
fromROSMsg (*input, *xyz);
// Estimate
TicToc tt;
print_highlight (stderr, "Computing ");
tt.tic ();
// Refine the plane indices
typedef SampleConsensusModelPlane<PointXYZ>::Ptr SampleConsensusModelPlanePtr;
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz));
RandomSampleConsensus<PointXYZ> sac (model, threshold);
sac.setMaxIterations (max_iterations);
bool res = sac.computeModel ();
vector<int> inliers;
sac.getInliers (inliers);
Eigen::VectorXf coefficients;
sac.getModelCoefficients (coefficients);
if (!res || inliers.empty ())
{
PCL_ERROR ("No planar model found. Relax thresholds and continue.\n");
return;
}
sac.refineModel (2, 50);
sac.getInliers (inliers);
sac.getModelCoefficients (coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%zu", inliers.size ()); print_info (" points]\n");
print_info ("Model coefficients: [");
print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n");
// Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first
if (negative)
{
// Remove the plane indices from the data
PointIndices::Ptr everything_but_the_plane (new PointIndices);
std::vector<int> indices_fullset (xyz->size ());
for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
indices_fullset[p_it] = p_it;
std::sort (inliers.begin (), inliers.end ());
set_difference (indices_fullset.begin (), indices_fullset.end (),
inliers.begin (), inliers.end (),
inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
// Extract largest cluster minus the plane
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setInputCloud (xyz);
ec.setIndices (everything_but_the_plane);
ec.extract (cluster_indices);
// Convert data back
copyPointCloud (*input, cluster_indices[0].indices, output);
}
else
{
// Convert data back
PointCloud<PointXYZ> output_inliers;
copyPointCloud (*input, inliers, output);
}
}
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);
}
TEST (SampleConsensusModelNormalParallelPlane, RANSAC)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
PointCloud<Normal> normals;
cloud.points.resize (10);
normals.resize (10);
for (unsigned idx = 0; idx < cloud.size (); ++idx)
{
cloud.points[idx].x = static_cast<float> ((rand () % 200) - 100);
cloud.points[idx].y = static_cast<float> ((rand () % 200) - 100);
cloud.points[idx].z = 0.0f;
normals.points[idx].normal_x = 0.0f;
normals.points[idx].normal_y = 0.0f;
normals.points[idx].normal_z = 1.0f;
}
// Create a shared plane model pointer directly
SampleConsensusModelNormalParallelPlanePtr model (new SampleConsensusModelNormalParallelPlane<PointXYZ, Normal> (cloud.makeShared ()));
model->setInputNormals (normals.makeShared ());
const float max_angle_rad = 0.01f;
const float angle_eps = 0.001f;
model->setEpsAngle (max_angle_rad);
// Test true axis
{
model->setAxis (Eigen::Vector3f (0, 0, 1));
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
sac.computeModel();
std::vector<int> inliers;
sac.getInliers (inliers);
ASSERT_EQ (cloud.size (), inliers.size ());
}
// test axis slightly in valid range
{
model->setAxis (Eigen::Vector3f (0, sin (max_angle_rad * (1 - angle_eps)), cos (max_angle_rad * (1 - angle_eps))));
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
sac.computeModel ();
std::vector<int> inliers;
sac.getInliers (inliers);
ASSERT_EQ (cloud.size (), inliers.size ());
}
// test axis slightly out of valid range
{
model->setAxis (Eigen::Vector3f (0, sin (max_angle_rad * (1 + angle_eps)), cos (max_angle_rad * (1 + angle_eps))));
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
sac.computeModel ();
std::vector<int> inliers;
sac.getInliers (inliers);
ASSERT_EQ (0, inliers.size ());
}
}
TEST (RANSAC, SampleConsensusModelNormalSphere)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
PointCloud<Normal> normals;
cloud.points.resize (27); normals.points.resize (27);
cloud.points[0].x = -0.014695f; cloud.points[0].y = 0.009549f; cloud.points[0].z = 0.954775f;
cloud.points[1].x = 0.014695f; cloud.points[1].y = 0.009549f; cloud.points[1].z = 0.954775f;
cloud.points[2].x = -0.014695f; cloud.points[2].y = 0.040451f; cloud.points[2].z = 0.954775f;
cloud.points[3].x = 0.014695f; cloud.points[3].y = 0.040451f; cloud.points[3].z = 0.954775f;
cloud.points[4].x = -0.009082f; cloud.points[4].y = -0.015451f; cloud.points[4].z = 0.972049f;
cloud.points[5].x = 0.009082f; cloud.points[5].y = -0.015451f; cloud.points[5].z = 0.972049f;
cloud.points[6].x = -0.038471f; cloud.points[6].y = 0.009549f; cloud.points[6].z = 0.972049f;
cloud.points[7].x = 0.038471f; cloud.points[7].y = 0.009549f; cloud.points[7].z = 0.972049f;
cloud.points[8].x = -0.038471f; cloud.points[8].y = 0.040451f; cloud.points[8].z = 0.972049f;
cloud.points[9].x = 0.038471f; cloud.points[9].y = 0.040451f; cloud.points[9].z = 0.972049f;
cloud.points[10].x = -0.009082f; cloud.points[10].y = 0.065451f; cloud.points[10].z = 0.972049f;
cloud.points[11].x = 0.009082f; cloud.points[11].y = 0.065451f; cloud.points[11].z = 0.972049f;
cloud.points[12].x = -0.023776f; cloud.points[12].y = -0.015451f; cloud.points[12].z = 0.982725f;
cloud.points[13].x = 0.023776f; cloud.points[13].y = -0.015451f; cloud.points[13].z = 0.982725f;
cloud.points[14].x = -0.023776f; cloud.points[14].y = 0.065451f; cloud.points[14].z = 0.982725f;
cloud.points[15].x = 0.023776f; cloud.points[15].y = 0.065451f; cloud.points[15].z = 0.982725f;
cloud.points[16].x = -0.000000f; cloud.points[16].y = -0.025000f; cloud.points[16].z = 1.000000f;
cloud.points[17].x = 0.000000f; cloud.points[17].y = -0.025000f; cloud.points[17].z = 1.000000f;
cloud.points[18].x = -0.029389f; cloud.points[18].y = -0.015451f; cloud.points[18].z = 1.000000f;
cloud.points[19].x = 0.029389f; cloud.points[19].y = -0.015451f; cloud.points[19].z = 1.000000f;
cloud.points[20].x = -0.047553f; cloud.points[20].y = 0.009549f; cloud.points[20].z = 1.000000f;
cloud.points[21].x = 0.047553f; cloud.points[21].y = 0.009549f; cloud.points[21].z = 1.000000f;
cloud.points[22].x = -0.047553f; cloud.points[22].y = 0.040451f; cloud.points[22].z = 1.000000f;
cloud.points[23].x = 0.047553f; cloud.points[23].y = 0.040451f; cloud.points[23].z = 1.000000f;
cloud.points[24].x = -0.029389f; cloud.points[24].y = 0.065451f; cloud.points[24].z = 1.000000f;
cloud.points[25].x = 0.029389f; cloud.points[25].y = 0.065451f; cloud.points[25].z = 1.000000f;
cloud.points[26].x = 0.000000f; cloud.points[26].y = 0.075000f; cloud.points[26].z = 1.000000f;
normals.points[0].normal[0] = -0.293893f; normals.points[0].normal[1] = -0.309017f; normals.points[0].normal[2] = -0.904509f;
normals.points[1].normal[0] = 0.293893f; normals.points[1].normal[1] = -0.309017f; normals.points[1].normal[2] = -0.904508f;
normals.points[2].normal[0] = -0.293893f; normals.points[2].normal[1] = 0.309017f; normals.points[2].normal[2] = -0.904509f;
normals.points[3].normal[0] = 0.293893f; normals.points[3].normal[1] = 0.309017f; normals.points[3].normal[2] = -0.904508f;
normals.points[4].normal[0] = -0.181636f; normals.points[4].normal[1] = -0.809017f; normals.points[4].normal[2] = -0.559017f;
normals.points[5].normal[0] = 0.181636f; normals.points[5].normal[1] = -0.809017f; normals.points[5].normal[2] = -0.559017f;
normals.points[6].normal[0] = -0.769421f; normals.points[6].normal[1] = -0.309017f; normals.points[6].normal[2] = -0.559017f;
normals.points[7].normal[0] = 0.769421f; normals.points[7].normal[1] = -0.309017f; normals.points[7].normal[2] = -0.559017f;
normals.points[8].normal[0] = -0.769421f; normals.points[8].normal[1] = 0.309017f; normals.points[8].normal[2] = -0.559017f;
normals.points[9].normal[0] = 0.769421f; normals.points[9].normal[1] = 0.309017f; normals.points[9].normal[2] = -0.559017f;
normals.points[10].normal[0] = -0.181636f; normals.points[10].normal[1] = 0.809017f; normals.points[10].normal[2] = -0.559017f;
normals.points[11].normal[0] = 0.181636f; normals.points[11].normal[1] = 0.809017f; normals.points[11].normal[2] = -0.559017f;
normals.points[12].normal[0] = -0.475528f; normals.points[12].normal[1] = -0.809017f; normals.points[12].normal[2] = -0.345491f;
normals.points[13].normal[0] = 0.475528f; normals.points[13].normal[1] = -0.809017f; normals.points[13].normal[2] = -0.345491f;
normals.points[14].normal[0] = -0.475528f; normals.points[14].normal[1] = 0.809017f; normals.points[14].normal[2] = -0.345491f;
normals.points[15].normal[0] = 0.475528f; normals.points[15].normal[1] = 0.809017f; normals.points[15].normal[2] = -0.345491f;
normals.points[16].normal[0] = -0.000000f; normals.points[16].normal[1] = -1.000000f; normals.points[16].normal[2] = 0.000000f;
normals.points[17].normal[0] = 0.000000f; normals.points[17].normal[1] = -1.000000f; normals.points[17].normal[2] = 0.000000f;
normals.points[18].normal[0] = -0.587785f; normals.points[18].normal[1] = -0.809017f; normals.points[18].normal[2] = 0.000000f;
normals.points[19].normal[0] = 0.587785f; normals.points[19].normal[1] = -0.809017f; normals.points[19].normal[2] = 0.000000f;
normals.points[20].normal[0] = -0.951057f; normals.points[20].normal[1] = -0.309017f; normals.points[20].normal[2] = 0.000000f;
normals.points[21].normal[0] = 0.951057f; normals.points[21].normal[1] = -0.309017f; normals.points[21].normal[2] = 0.000000f;
normals.points[22].normal[0] = -0.951057f; normals.points[22].normal[1] = 0.309017f; normals.points[22].normal[2] = 0.000000f;
normals.points[23].normal[0] = 0.951057f; normals.points[23].normal[1] = 0.309017f; normals.points[23].normal[2] = 0.000000f;
normals.points[24].normal[0] = -0.587785f; normals.points[24].normal[1] = 0.809017f; normals.points[24].normal[2] = 0.000000f;
normals.points[25].normal[0] = 0.587785f; normals.points[25].normal[1] = 0.809017f; normals.points[25].normal[2] = 0.000000f;
normals.points[26].normal[0] = 0.000000f; normals.points[26].normal[1] = 1.000000f; normals.points[26].normal[2] = 0.000000f;
// Create a shared sphere model pointer directly
SampleConsensusModelNormalSpherePtr model (new SampleConsensusModelNormalSphere<PointXYZ, Normal> (cloud.makeShared ()));
model->setInputNormals(normals.makeShared ());
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 4);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 27);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 4);
EXPECT_NEAR (coeff[0], 0.0, 1e-2);
EXPECT_NEAR (coeff[1], 0.025, 1e-2);
EXPECT_NEAR (coeff[2], 1.0, 1e-2);
EXPECT_NEAR (coeff[3], 0.05, 1e-2);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 4);
EXPECT_NEAR (coeff_refined[0], 0.0, 1e-2);
EXPECT_NEAR (coeff_refined[1], 0.025, 1e-2);
EXPECT_NEAR (coeff_refined[2], 1.0, 1e-2);
EXPECT_NEAR (coeff_refined[3], 0.05, 1e-2);
}