c++ - Point cloud ICP aligmnet -

i have 2 point clouds want match. after alignment, input cloud, not merged 2 point clouds. problem ?

i have updated code , still doesnt align @

  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 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 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 segmentation object planar model ,  set 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 plane inliers , coefficients     seg.segment (*inliers_plane, *coefficients_plane);     std::cerr << "plane coefficients: " << *coefficients_plane << std::endl;      // extract planar inliers input cloud     extract.setinputcloud (cloud_filtered);     extract.setindices (inliers_plane);     extract.setnegative (false);        // write planar inliers disk     pcl::pointcloud<pcl::pointxyz>::ptr cloud_plane (new pcl::pointcloud<pcl::pointxyz> ());     extract.filter (*cloud_plane);     std::cerr << "pointcloud representing 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; 

icp uses input_cloud viewpoint reference. tries align target_cloud such distance between correspondences points iteratively minimized. finial transformation of used by

icp.align(final); 

to algin target_cloud , stores in pcl::pointcloud<pcl::pointxyz> final.

to merge both point clouds after icp, have transform both same viewpoint this

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);  // both point clouds transformed origin , can added pcl::pointcloud<pcl::pointxyz> output_cloud; output_cloud += transformed_input_cloud; output_cloud += transformed_target_cloud;