点云处理cpp

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接： https://blog.csdn.net/Discoverhfub/article/details/102755862

以时间戳保存点云为pcd文件

pcl::PointCloud<PointType>::Ptr fullcloud;
fullcloud->clear();
pcl::fromROSMsg(*fullcloudCloudMsg, *fullcloud);
long pose_time5 =  fullcloudCloudMsg->header.stamp.toNSec()/1000;
std::string str_count5_;
std::stringstream ss5_count;
ss5_count << pose_time5;
ss5_count >> str_count5_;
pcl::io::savePCDFileASCII("/home/ly/lego_pcd/segmented_cloud/" + str_count5_ + ".pcd", *fullcloud);  //pcl::io::savePCDFileBinaryCompressed("/home/ly/lego_pcd/segmented_cloud/" + str_count5_ + ".pcd", *fullcloud);  　　//这是保存为二进制，到时读取速度更快

icp匹配

                pcl::PointCloud<PointType>::Ptr outCloud_z(new pcl::PointCloud<PointType>());
                pcl::IterativeClosestPoint<PointType, PointType> icp;
                icp.setMaxCorrespondenceDistance (5.0);//10对应点对之间的最大距离;对配准影响较大
                icp.setMaximumIterations (25);
                icp.setTransformationEpsilon (1e-3);//设为1e-6或者更小,最小转换差异
                icp.setEuclideanFitnessEpsilon (1e-2);//0.001前后两次迭代的点对的欧式距离均值的最大容差
                icp.setInputSource (laserCloudTempcurr);//需要匹配的点云
                icp.setInputTarget (map_cloud_filtered2);//匹配的地图点云
                icp.align(*outCloud_z, transform_z.cast<float>());//transform_z是先将当前帧点云变换到前一时刻位置
                if (icp.hasConverged())
                {
                    Eigen::Matrix4d tran_z = icp.getFinalTransformation().cast<double>();
                }
                else
                {
                    std::cout << "registration is not converged!" << std::endl;
                }