c++ - 点云 ICP 对齐

Question

我有两个要匹配的点云。但对齐后，我只得到输入云，而不是合并的两个点云。可能是什么问题？

我已经更新了我的代码，但它仍然根本不对齐

  pcl::PCDReader reader;
    pcl::PassThrough<pcl::PointXYZ> pass;
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
    pcl::PCDWriter writer;
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());

    // Datasets
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>),
            cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
    pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);

    reader.read ("match.pcd", *target_cloud);
    //Read in the cloud data
    reader.read ("pointClouds_000026.pcd", *input_cloud);
    std::cerr << "PointCloud has: " << input_cloud->points.size () << " data points." << std::endl;

    //    pass.setInputCloud (input_cloud);
    //    pass.setFilterFieldName ("z");
    //    pass.setFilterLimits (-600,1400);
    //    pass.filter (*cloud_filtered);


    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    // build the filter
    outrem.setInputCloud(input_cloud);
    outrem.setRadiusSearch(50);
    outrem.setMinNeighborsInRadius (5);
    // apply filter
    outrem.filter (*cloud_filtered);

    std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
    writer.write ("table_scene_mug_stereo_textured_filtered.pcd", *cloud_filtered, false);

    // Estimate point normals
    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud_filtered);
    ne.setKSearch (50);
    ne.compute (*cloud_normals);

    // Create the segmentation object for the planar model and  set all the parameters
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight (0.1);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (1000);
    seg.setDistanceThreshold (0.09);
    seg.setInputCloud (cloud_filtered);
    seg.setInputNormals (cloud_normals);
    seg.setEpsAngle(0.05320);
    Eigen::Vector3f YAxis(0,1,0);
    seg.setAxis(YAxis);
    // Obtain the plane inliers and coefficients
    seg.segment (*inliers_plane, *coefficients_plane);
    std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

    // Extract the planar inliers from the input cloud
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers_plane);
    extract.setNegative (false);



    // Write the planar inliers to disk
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
    extract.filter (*cloud_plane);
    std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
    writer.write ("cloudplane.pcd", *cloud_plane, false);

    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType (pcl::SACMODEL_PLANE);
    proj.setIndices (inliers_plane);
    proj.setInputCloud(cloud_filtered);
    proj.setModelCoefficients (coefficients_plane);
    proj.filter (*cloud_projected);
    std::cerr << "PointCloud after projection has: "
              << cloud_projected->points.size () << " data points." << std::endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ConvexHull<pcl::PointXYZ> chull;
    chull.setInputCloud (cloud_projected);
    chull.setComputeAreaVolume(true);
    chull.reconstruct (*cloud_hull);

    std::cerr <<"area of convex hull: "<< chull.getTotalArea()<<std::endl;
    std::cerr << "Convex hull has: " << cloud_hull->points.size () << " data points." << std::endl;

    writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    icp.setInputSource (cloud_plane);
    icp.setInputTarget (target_cloud);
    icp.setEuclideanFitnessEpsilon(0.000000000001);
    icp.setTransformationEpsilon(0.0000001);
    icp.setMaximumIterations(10000);
    icp.setMaxCorrespondenceDistance(300);
    writer.write("nonal_igned.pcd", *cloud_plane + *target_cloud, false);

    pcl::PointCloud<pcl::PointXYZ> final;

    icp.align (final);
    pcl::transformPointCloud(*cloud_plane, *cloud_plane, icp.getFinalTransformation());

    final = final + *target_cloud;

    writer.write("aligned.pcd", final, false);
    std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

Answer 1

ICP 使用 input_cloud 及其视点作为引用。它尝试对齐 target_cloud，使得对应点之间的距离迭代最小化。 this 的最终转换将由

icp.align(Final);

生成 target_cloud 并将其存储在 pcl::PointCloud<pcl::PointXYZ> Final 中.

要在 ICP 之后合并两个点云，您必须像这样将它们转换为相同的视点

pcl::PointCloud<pcl::PointXYZ> transformed_target_cloud;
Eigen::Matrix4f transformation_matrix = icp.getFinalTransformation();
pcl::transformPointCloud(Final, transformed_target_cloud, transformation_matrix);

pcl::PointCloud<pcl::PointXYZ> transformed_input_cloud;
Eigen::Isometry3f pose(Eigen::Isometry3f(Eigen::Translation3f(
                      input_cloud.sensor_origin_[0],
                      input_cloud.sensor_origin_[1], 
                      input_cloud.sensor_origin_[2])) * 
                      Eigen::Isometry3f(input_cloud.sensor_orientation_));
transformation_matrix = pose.matrix();
pcl::transformPointCloud(input_cloud, transformed_input_cloud, transformation_matrix);

// now both point clouds are transformed to the origin and can be added up
pcl::PointCloud<pcl::PointXYZ> output_cloud;
output_cloud += transformed_input_cloud;
output_cloud += transformed_target_cloud;