代码：

  1 #include <pcl/point_types.h>
  2 #include <pcl/point_cloud.h>
  3 #include <pcl/features/normal_3d.h>
  4 #include <pcl/features/3dsc.h>
  5 #include <pcl/search/kdtree.h>
  6 #include <pcl/io/pcd_io.h>
  7 #include <pcl/filters/random_sample.h>//采取固定数量的点云
  8 #include <pcl/registration/ia_ransac.h>//采样一致性
  9 #include <pcl/registration/icp.h>//icp配准
 10 #include <boost/thread/thread.hpp>
 11 #include <pcl/visualization/pcl_visualizer.h>//可视化
 12 #include <time.h>//时间
 13 
 14 using pcl::NormalEstimation;
 15 using pcl::search::KdTree;
 16 typedef pcl::PointXYZ PointT;
 17 typedef pcl::PointCloud<PointT> PointCloud;
 18 
 19 //点云可视化
 20 void visualize_pcd2(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_src1, PointCloud::Ptr pcd_tgt1)
 21 {
 22 
 23     //创建初始化目标
 24     pcl::visualization::PCLVisualizer viewer("registration Viewer");
 25     int v1(0);
 26     int v2(1);
 27     viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
 28     viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
 29     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
 30     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
 31     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h1(pcd_src1, 0, 255, 0);
 32     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h1(pcd_tgt1, 255, 0, 0);
 33     viewer.setBackgroundColor(255, 255, 255);
 34     viewer.addPointCloud(pcd_src, src_h, "source cloud", v1);
 35     viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud", v1);
 36     viewer.addPointCloud(pcd_src1, src_h1, "source cloud1", v2);
 37     viewer.addPointCloud(pcd_tgt1, tgt_h1, "tgt cloud1", v2);
 38 
 39     //viewer.addCoordinateSystem(0.05);
 40     while (!viewer.wasStopped())
 41     {
 42         viewer.spinOnce(100);
 43         boost::this_thread::sleep(boost::posix_time::microseconds(100000));
 44     }
 45 }
 46 //由旋转平移矩阵计算旋转角度
 47 void matrix2angle(Eigen::Matrix4f &result_trans, Eigen::Vector3f &result_angle)
 48 {
 49     double ax, ay, az;
 50     if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1)
 51     {
 52         az = 0;
 53         double dlta;
 54         dlta = atan2(result_trans(0, 1), result_trans(0, 2));
 55         if (result_trans(2, 0) == -1)
 56         {
 57             ay = M_PI / 2;
 58             ax = az + dlta;
 59         }
 60         else
 61         {
 62             ay = -M_PI / 2;
 63             ax = -az + dlta;
 64         }
 65     }
 66     else
 67     {
 68         ay = -asin(result_trans(2, 0));
 69         ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));
 70         az = atan2(result_trans(1, 0) / cos(ay), result_trans(0, 0) / cos(ay));
 71     }
 72     result_angle << ax, ay, az;
 73 
 74     cout << "x轴旋转角度：" << ax << endl;
 75     cout << "y轴旋转角度：" << ay << endl;
 76     cout << "z轴旋转角度：" << az << endl;
 77 }
 78 
 79 
 80 int main(int argc, char** argv)
 81 {
 82     //加载点云文件
 83     PointCloud::Ptr cloud_src_o(new PointCloud);//原点云，待配准
 84     pcl::io::loadPCDFile("ear.pcd", *cloud_src_o);
 85     PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
 86     pcl::io::loadPCDFile("earzhuan05.pcd", *cloud_tgt_o);
 87 
 88     clock_t start = clock();
 89 
 90     //去除NAN点
 91     std::vector<int> indices_src; //保存去除的点的索引
 92     pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);
 93     std::cout << "remove *cloud_src_o nan" << endl;
 94 
 95     std::vector<int> indices_tgt;
 96     pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);
 97     std::cout << "remove *cloud_tgt_o nan" << endl;
 98 
 99     //采样固定的点云数量
100     pcl::RandomSample<PointT> rs_src;
101     rs_src.setInputCloud(cloud_src_o);
102     rs_src.setSample(550);
103     PointCloud::Ptr cloud_src(new PointCloud);
104     rs_src.filter(*cloud_src);
105     std::cout << "down size *cloud_src_o from " << cloud_src_o->size() << "to" << cloud_src->size() << endl;
106 
107     pcl::RandomSample<PointT> rs_tgt;
108     rs_tgt.setInputCloud(cloud_tgt_o);
109     rs_tgt.setSample(550);
110     PointCloud::Ptr cloud_tgt(new PointCloud);
111     rs_tgt.filter(*cloud_tgt);
112     std::cout << "down size *cloud_tgt_o from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl;
113 
114     //计算表面法线
115     pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;
116     ne_src.setInputCloud(cloud_src);
117     pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
118     ne_src.setSearchMethod(tree_src);
119     pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
120     ne_src.setRadiusSearch(4);
121     ne_src.compute(*cloud_src_normals);
122 
123     pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;
124     ne_tgt.setInputCloud(cloud_tgt);
125     pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
126     ne_tgt.setSearchMethod(tree_tgt);
127     pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
128     //ne_tgt.setKSearch(20);
129     ne_tgt.setRadiusSearch(4);
130     ne_tgt.compute(*cloud_tgt_normals);
131 
132     //计算3dsc
133     pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> sp_tgt;
134     sp_tgt.setInputCloud(cloud_tgt);
135     sp_tgt.setInputNormals(cloud_tgt_normals);
136     //kdTree加速
137     pcl::search::KdTree<PointT>::Ptr tree_tgt_sp(new pcl::search::KdTree<PointT>);
138     sp_tgt.setSearchMethod(tree_tgt_sp);
139     pcl::PointCloud<pcl::ShapeContext1980>::Ptr sps_tgt(new pcl::PointCloud<pcl::ShapeContext1980>());
140     sp_tgt.setRadiusSearch(4);
141     sp_tgt.compute(*sps_tgt);
142 
143     cout << "compute *cloud_tgt_sps" << endl;
144 
145     pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> sp_src;
146     sp_src.setInputCloud(cloud_src);
147     sp_src.setInputNormals(cloud_src_normals);
148     //kdTree加速
149     pcl::search::KdTree<PointT>::Ptr tree_src_sp(new pcl::search::KdTree<PointT>);
150     sp_src.setSearchMethod(tree_src_sp);
151     pcl::PointCloud<pcl::ShapeContext1980>::Ptr sps_src(new pcl::PointCloud<pcl::ShapeContext1980>());
152     sp_src.setRadiusSearch(4);
153     sp_src.compute(*sps_src);
154 
155     cout << "compute *cloud_tgt_sps" << endl;
156 
157 
158 
159     //SAC配准
160     pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::ShapeContext1980> scia;
161     scia.setInputSource(cloud_src);
162     scia.setInputTarget(cloud_tgt);
163     scia.setSourceFeatures(sps_src);
164     scia.setTargetFeatures(sps_tgt);
165     //scia.setMinSampleDistance(1);
166     //scia.setNumberOfSamples(2);
167     //scia.setCorrespondenceRandomness(20);
168     PointCloud::Ptr sac_result(new PointCloud);
169     scia.align(*sac_result);
170     std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore() << endl;
171     Eigen::Matrix4f sac_trans;
172     sac_trans = scia.getFinalTransformation();
173     std::cout << sac_trans << endl;
174     //pcl::io::savePCDFileASCII("bunny_transformed_sac.pcd", *sac_result);
175     clock_t sac_time = clock();
176 
177     //icp配准
178     PointCloud::Ptr icp_result(new PointCloud);
179     pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
180     icp.setInputSource(cloud_src);
181     icp.setInputTarget(cloud_tgt_o);
182     //Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
183     icp.setMaxCorrespondenceDistance(8);
184     // 最大迭代次数
185     icp.setMaximumIterations(100);
186     // 两次变化矩阵之间的差值
187     icp.setTransformationEpsilon(1e-10);
188     // 均方误差
189     icp.setEuclideanFitnessEpsilon(0.01);
190     icp.align(*icp_result, sac_trans);
191 
192     clock_t end = clock();
193     cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
194 
195     cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl;
196     cout << "icp time: " << (double)(end - sac_time) / (double)CLOCKS_PER_SEC << " s" << endl;
197 
198     std::cout << "ICP has converged:" << icp.hasConverged()
199         << " score: " << icp.getFitnessScore() << std::endl;
200     Eigen::Matrix4f icp_trans;
201     icp_trans = icp.getFinalTransformation();
202     //cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl;
203     std::cout << icp_trans << endl;
204     //使用创建的变换对未过滤的输入点云进行变换
205     pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);
206     //保存转换的输入点云
207     //pcl::io::savePCDFileASCII("_transformed_sac_ndt.pcd", *icp_result);
208 
209     //计算误差
210     Eigen::Vector3f ANGLE_origin;
211     Eigen::Vector3f TRANS_origin;
212     ANGLE_origin << 0, 0, M_PI / 4;
213     TRANS_origin << 0, 0.3, 0.2;
214     double a_error_x, a_error_y, a_error_z;
215     double t_error_x, t_error_y, t_error_z;
216     Eigen::Vector3f ANGLE_result;
217     matrix2angle(icp_trans, ANGLE_result);
218     a_error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0));
219     a_error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1));
220     a_error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2));
221     cout << "点云实际旋转角度:\n" << ANGLE_origin << endl;
222     cout << "x轴旋转误差 : " << a_error_x << "  y轴旋转误差 : " << a_error_y << "  z轴旋转误差 : " << a_error_z << endl;
223 
224     cout << "点云实际平移距离:\n" << TRANS_origin << endl;
225     t_error_x = fabs(icp_trans(0, 3)) - fabs(TRANS_origin(0));
226     t_error_y = fabs(icp_trans(1, 3)) - fabs(TRANS_origin(1));
227     t_error_z = fabs(icp_trans(2, 3)) - fabs(TRANS_origin(2));
228     cout << "计算得到的平移距离" << endl << "x轴平移" << icp_trans(0, 3) << endl << "y轴平移" << icp_trans(1, 3) << endl << "z轴平移" << icp_trans(2, 3) << endl;
229     cout << "x轴平移误差 : " << t_error_x << "  y轴平移误差 : " << t_error_y << "  z轴平移误差 : " << t_error_z << endl;
230 
231     //可视化
232     visualize_pcd2(cloud_src_o, cloud_tgt_o, icp_result, cloud_tgt_o);
233     return (0);
234 }