Точка облака ICP aligmnet

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 так, что расстояние между точками соответствия итеративно минимизируется. Флерон превращение этого будет использоваться

icp.align(Final); 

Для Algin в target_cloud и сохраняет его в pcl::PointCloud<pcl::PointXYZ> Final.

Чтобы объединить оба облака точек после ПМСА, вы должны преобразовать как в то же точку зрения, как этот

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;