粗配准+icp精配准搭配3DSC 实现配准流程

文章目录

前言
一、制作source和target点云数据
二、完整代码
三、配准效果

前言

借鉴
参考一

一、制作source和target点云数据

制作翻转点云数据可以参照这篇文章这里不再赘述

二、完整代码

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/3dsc.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/random_sample.h>//采取固定数量的点云
#include <pcl/registration/ia_ransac.h>//采样一致性
#include <pcl/registration/icp.h>//icp配准
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <time.h>//时间using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;//点云可视化
void visualize_pcd2(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_src1, PointCloud::Ptr pcd_tgt1)
{//创建初始化目标pcl::visualization::PCLVisualizer viewer("registration Viewer");int v1(0);int v2(1);viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h1(pcd_src1, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h1(pcd_tgt1, 255, 0, 0);viewer.setBackgroundColor(255, 255, 255);viewer.addPointCloud(pcd_src, src_h, "source cloud", v1);viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud", v1);viewer.addPointCloud(pcd_src1, src_h1, "source cloud1", v2);viewer.addPointCloud(pcd_tgt1, tgt_h1, "tgt cloud1", v2);//viewer.addCoordinateSystem(0.05);while (!viewer.wasStopped()){viewer.spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}
}
//由旋转平移矩阵计算旋转角度
void matrix2angle(Eigen::Matrix4f& result_trans, Eigen::Vector3f& result_angle)
{double ax, ay, az;if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1){az = 0;double dlta;dlta = atan2(result_trans(0, 1), result_trans(0, 2));if (result_trans(2, 0) == -1){ay = M_PI / 2;ax = az + dlta;}else{ay = -M_PI / 2;ax = -az + dlta;}}else{ay = -asin(result_trans(2, 0));ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));az = atan2(result_trans(1, 0) / cos(ay), result_trans(0, 0) / cos(ay));}result_angle << ax, ay, az;cout << "x轴旋转角度：" << ax << endl;cout << "y轴旋转角度：" << ay << endl;cout << "z轴旋转角度：" << az << endl;
}int main(int argc, char** argv)
{//加载点云文件PointCloud::Ptr cloud_src_o(new PointCloud);//原点云，待配准pcl::io::loadPCDFile("rabbit.pcd", *cloud_src_o);PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云pcl::io::loadPCDFile("transformed_rabbit.pcd", *cloud_tgt_o);clock_t start = clock();//去除NAN点std::vector<int> indices_src; //保存去除的点的索引pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);std::cout << "remove *cloud_src_o nan" << endl;std::vector<int> indices_tgt;pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);std::cout << "remove *cloud_tgt_o nan" << endl;//采样固定的点云数量pcl::RandomSample<PointT> rs_src;rs_src.setInputCloud(cloud_src_o);/*rs_src.setSample(550);*/rs_src.setSample(130);PointCloud::Ptr cloud_src(new PointCloud);rs_src.filter(*cloud_src);std::cout << "down size *cloud_src_o from " << cloud_src_o->size() << "to" << cloud_src->size() << endl;pcl::RandomSample<PointT> rs_tgt;rs_tgt.setInputCloud(cloud_tgt_o);/*rs_tgt.setSample(550);*/rs_tgt.setSample(130);PointCloud::Ptr cloud_tgt(new PointCloud);rs_tgt.filter(*cloud_tgt);std::cout << "down size *cloud_tgt_o from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl;//计算表面法线pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;ne_src.setInputCloud(cloud_src);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());ne_src.setSearchMethod(tree_src);pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);ne_src.setRadiusSearch(4);ne_src.compute(*cloud_src_normals);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;ne_tgt.setInputCloud(cloud_tgt);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());ne_tgt.setSearchMethod(tree_tgt);pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);//ne_tgt.setKSearch(20);ne_tgt.setRadiusSearch(4);ne_tgt.compute(*cloud_tgt_normals);//计算3dscpcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> sp_tgt;sp_tgt.setInputCloud(cloud_tgt);sp_tgt.setInputNormals(cloud_tgt_normals);//kdTree加速pcl::search::KdTree<PointT>::Ptr tree_tgt_sp(new pcl::search::KdTree<PointT>);sp_tgt.setSearchMethod(tree_tgt_sp);pcl::PointCloud<pcl::ShapeContext1980>::Ptr sps_tgt(new pcl::PointCloud<pcl::ShapeContext1980>());sp_tgt.setRadiusSearch(4);sp_tgt.compute(*sps_tgt);cout << "compute *cloud_tgt_sps" << endl;pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> sp_src;sp_src.setInputCloud(cloud_src);sp_src.setInputNormals(cloud_src_normals);//kdTree加速pcl::search::KdTree<PointT>::Ptr tree_src_sp(new pcl::search::KdTree<PointT>);sp_src.setSearchMethod(tree_src_sp);pcl::PointCloud<pcl::ShapeContext1980>::Ptr sps_src(new pcl::PointCloud<pcl::ShapeContext1980>());sp_src.setRadiusSearch(4);sp_src.compute(*sps_src);cout << "compute *cloud_tgt_sps" << endl;//SAC配准pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::ShapeContext1980> scia;scia.setInputSource(cloud_src);scia.setInputTarget(cloud_tgt);scia.setSourceFeatures(sps_src);scia.setTargetFeatures(sps_tgt);//scia.setMinSampleDistance(1);//scia.setNumberOfSamples(2);//scia.setCorrespondenceRandomness(20);PointCloud::Ptr sac_result(new PointCloud);scia.align(*sac_result);std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore() << endl;Eigen::Matrix4f sac_trans;sac_trans = scia.getFinalTransformation();std::cout << sac_trans << endl;pcl::io::savePCDFileASCII("bunny_transformed_sac.pcd", *sac_result);clock_t sac_time = clock();//icp配准PointCloud::Ptr icp_result(new PointCloud);pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputSource(cloud_src);icp.setInputTarget(cloud_tgt_o);//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)icp.setMaxCorrespondenceDistance(8);// 最大迭代次数/*icp.setMaximumIterations(100);*/icp.setMaximumIterations(100);// 两次变化矩阵之间的差值icp.setTransformationEpsilon(1e-10);// 均方误差icp.setEuclideanFitnessEpsilon(0.01);icp.align(*icp_result, sac_trans);clock_t end = clock();cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl;cout << "icp time: " << (double)(end - sac_time) / (double)CLOCKS_PER_SEC << " s" << endl;std::cout << "ICP has converged:" << icp.hasConverged()<< " score: " << icp.getFitnessScore() << std::endl;Eigen::Matrix4f icp_trans;icp_trans = icp.getFinalTransformation();//cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl;std::cout << icp_trans << endl;//使用创建的变换对未过滤的输入点云进行变换pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);//保存转换的输入点云pcl::io::savePCDFileASCII("_transformed_sac_ndt.pcd", *icp_result);//计算误差Eigen::Vector3f ANGLE_origin;Eigen::Vector3f TRANS_origin;ANGLE_origin << 0, 0, M_PI / 4;TRANS_origin << 0, 0.3, 0.2;double a_error_x, a_error_y, a_error_z;double t_error_x, t_error_y, t_error_z;Eigen::Vector3f ANGLE_result;matrix2angle(icp_trans, ANGLE_result);a_error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0));a_error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1));a_error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2));cout << "点云实际旋转角度:\n" << ANGLE_origin << endl;cout << "x轴旋转误差 : " << a_error_x << "  y轴旋转误差 : " << a_error_y << "  z轴旋转误差 : " << a_error_z << endl;cout << "点云实际平移距离:\n" << TRANS_origin << endl;t_error_x = fabs(icp_trans(0, 3)) - fabs(TRANS_origin(0));t_error_y = fabs(icp_trans(1, 3)) - fabs(TRANS_origin(1));t_error_z = fabs(icp_trans(2, 3)) - fabs(TRANS_origin(2));cout << "计算得到的平移距离" << endl << "x轴平移" << icp_trans(0, 3) << endl << "y轴平移" << icp_trans(1, 3) << endl << "z轴平移" << icp_trans(2, 3) << endl;cout << "x轴平移误差 : " << t_error_x << "  y轴平移误差 : " << t_error_y << "  z轴平移误差 : " << t_error_z << endl;//可视化//visualize_pcd2(cloud_src_o, cloud_tgt_o, icp_result, cloud_tgt_o);boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ICP"));pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(icp_result, 255, 0, 0);		// 目标点云viewer->addPointCloud<pcl::PointXYZ>(icp_result, target_color, "target cloud");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>result_color(cloud_tgt_o, 0, 255, 0);		// 配准结果点云viewer->addPointCloud<pcl::PointXYZ>(cloud_tgt_o, result_color, "result cloud");while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return (0);
}