7：C++搭配PCL点云配准之3DSC特征
代码：
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 }
