5：C++搭配PCL點雲配準之PFH特徵
阿新 • • 發佈：2021-06-29
程式碼：
1 #pragma warning(disable:4996) 2 #include <pcl/registration/ia_ransac.h>//取樣一致性 3 #include <pcl/point_types.h> 4 #include <pcl/point_cloud.h> 5 #include <pcl/features/normal_3d.h> 6 #include <pcl/features/fpfh.h> 7 #include <pcl/features/pfh.h> 8 #include <pcl/search/kdtree.h> 9#include <pcl/io/pcd_io.h> 10 #include <pcl/filters/voxel_grid.h>// 11 #include <pcl/filters/filter.h>// 12 #include <pcl/registration/icp.h>//icp配準 13 #include <pcl/registration/gicp.h> 14 #include <pcl/visualization/pcl_visualizer.h>//視覺化 15 #include <time.h>//時間 16 1718 using pcl::NormalEstimation; 19 using pcl::search::KdTree; 20 typedef pcl::PointXYZ PointT; 21 typedef pcl::PointCloud<PointT> PointCloud; 22 23 //點雲視覺化 24 void visualize_pcd2(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_src1, PointCloud::Ptr pcd_tgt1) 25 {26 27 //建立初始化目標 28 pcl::visualization::PCLVisualizer viewer("registration Viewer"); 29 int v1(0); 30 int v2(1); 31 viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1); 32 viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2); 33 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0); 34 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0); 35 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h1(pcd_src1, 0, 255, 0); 36 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h1(pcd_tgt1, 255, 0, 0); 37 viewer.setBackgroundColor(255, 255, 255); 38 viewer.addPointCloud(pcd_src, src_h, "source cloud", v1); 39 viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud", v1); 40 viewer.addPointCloud(pcd_src1, src_h1, "source cloud1", v2); 41 viewer.addPointCloud(pcd_tgt1, tgt_h1, "tgt cloud1", v2); 42 43 //viewer.addCoordinateSystem(0.05); 44 while (!viewer.wasStopped()) 45 { 46 viewer.spinOnce(100); 47 boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 48 } 49 } 50 51 //由旋轉平移矩陣計算旋轉角度 52 void matrix2angle(Eigen::Matrix4f &result_trans, Eigen::Vector3f &result_angle) 53 { 54 double ax, ay, az; 55 if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1) 56 { 57 az = 0; 58 double dlta; 59 dlta = atan2(result_trans(0, 1), result_trans(0, 2)); 60 if (result_trans(2, 0) == -1) 61 { 62 ay = M_PI / 2; 63 ax = az + dlta; 64 } 65 else 66 { 67 ay = -M_PI / 2; 68 ax = -az + dlta; 69 } 70 } 71 else 72 { 73 ay = -asin(result_trans(2, 0)); 74 ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay)); 75 az = atan2(result_trans(1, 0) / cos(ay), result_trans(0, 0) / cos(ay)); 76 } 77 result_angle << ax, ay, az; 78 79 cout << "x軸旋轉角度：" << ax << endl; 80 cout << "y軸旋轉角度：" << ay << endl; 81 cout << "z軸旋轉角度：" << az << endl; 82 } 83 84 int main(int argc, char** argv) 85 { 86 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudadd(new pcl::PointCloud<pcl::PointXYZ>); 87 //載入點雲檔案 88 PointCloud::Ptr cloud_src_o(new PointCloud);//原點雲，待配準 89 pcl::io::loadPCDFile("ear.pcd", *cloud_src_o); 90 PointCloud::Ptr cloud_tgt_o(new PointCloud);//目標點雲 91 pcl::io::loadPCDFile("earzhuan.pcd", *cloud_tgt_o); 92 93 clock_t start = clock(); 94 95 //去除NAN點 96 std::vector<int> indices_src; //儲存去除的點的索引 97 pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src); 98 std::cout << "remove *cloud_src_o nan" << endl; 99 100 std::vector<int> indices_tgt; 101 pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt); 102 std::cout << "remove *cloud_tgt_o nan" << endl; 103 104 //下采樣濾波 105 pcl::VoxelGrid<pcl::PointXYZ> voxel_grid; 106 voxel_grid.setLeafSize(2,2,2); 107 voxel_grid.setInputCloud(cloud_src_o); 108 PointCloud::Ptr cloud_src(new PointCloud); 109 voxel_grid.filter(*cloud_src); 110 std::cout << "down size *cloud_src_o from " << cloud_src_o->size() << "to" << cloud_src->size() << endl; 111 //::io::savePCDFileASCII("rabbit_src_down.pcd", *cloud_src); 112 113 114 pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2; 115 voxel_grid_2.setLeafSize(2, 2, 2); 116 voxel_grid_2.setInputCloud(cloud_tgt_o); 117 PointCloud::Ptr cloud_tgt(new PointCloud); 118 voxel_grid_2.filter(*cloud_tgt); 119 std::cout << "down size *cloud_tgt_o.pcd from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl; 120 //pcl::io::savePCDFileASCII("bunny_tgt_down.pcd", *cloud_tgt); 121 122 //計算表面法線 123 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src; 124 ne_src.setInputCloud(cloud_src); 125 pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>()); 126 ne_src.setSearchMethod(tree_src); 127 pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>); 128 ne_src.setRadiusSearch(4); 129 ne_src.compute(*cloud_src_normals); 130 131 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt; 132 ne_tgt.setInputCloud(cloud_tgt); 133 pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>()); 134 ne_tgt.setSearchMethod(tree_tgt); 135 pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>); 136 //ne_tgt.setKSearch(20); 137 ne_tgt.setRadiusSearch(4); 138 ne_tgt.compute(*cloud_tgt_normals); 139 140 //計算PFH 141 pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh_src; 142 pfh_src.setInputCloud(cloud_src); 143 pfh_src.setInputNormals(cloud_src_normals); 144 pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>); 145 pfh_src.setSearchMethod(tree_src_fpfh); 146 pcl::PointCloud<pcl::PFHSignature125>::Ptr pfhs_src (new pcl::PointCloud<pcl::PFHSignature125>()); 147 pfh_src.setRadiusSearch(5); 148 pfh_src.compute(*pfhs_src); 149 std::cout << "compute *cloud_src pfh" << endl; 150 151 pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh_tgt; 152 pfh_src.setInputCloud(cloud_tgt); 153 pfh_src.setInputNormals(cloud_tgt_normals); 154 pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>); 155 pfh_src.setSearchMethod(tree_tgt_fpfh); 156 pcl::PointCloud<pcl::PFHSignature125>::Ptr pfhs_tgt(new pcl::PointCloud<pcl::PFHSignature125>()); 157 pfh_src.setRadiusSearch(5); 158 pfh_src.compute(*pfhs_tgt); 159 std::cout << "compute *cloud_tgt pfh" << endl; 160 161 162 //SAC配準 163 pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::PFHSignature125> scia; 164 scia.setInputSource(cloud_src); 165 scia.setInputTarget(cloud_tgt); 166 scia.setSourceFeatures(pfhs_src); 167 scia.setTargetFeatures(pfhs_tgt); 168 //scia.setMinSampleDistance(1); 169 //scia.setNumberOfSamples(2); 170 //scia.setCorrespondenceRandomness(20); 171 PointCloud::Ptr sac_result(new PointCloud); 172 scia.align(*sac_result); 173 std::cout << "sac has converged:" << scia.hasConverged() << " score: " << scia.getFitnessScore() << endl; 174 Eigen::Matrix4f sac_trans; 175 sac_trans = scia.getFinalTransformation(); 176 std::cout << sac_trans << endl; 177 //pcl::io::savePCDFileASCII("bunny_transformed_sac.pcd", *sac_result); 178 clock_t sac_time = clock(); 179 180 //icp配準 181 PointCloud::Ptr icp_result(new PointCloud); 182 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; 183 icp.setInputSource(cloud_src); 184 icp.setInputTarget(cloud_tgt_o); 185 //Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored) 186 icp.setMaxCorrespondenceDistance(4); 187 // 最大迭代次數 188 icp.setMaximumIterations(100); 189 // 兩次變化矩陣之間的差值 190 icp.setTransformationEpsilon(1e-10); 191 // 均方誤差 192 icp.setEuclideanFitnessEpsilon(0.01); 193 icp.align(*icp_result, sac_trans); 194 195 clock_t end = clock(); 196 cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl; 197 cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl; 198 cout << "icp time: " << (double)(end - sac_time) / (double)CLOCKS_PER_SEC << " s" << endl; 199 200 std::cout << "ICP has converged:" << icp.hasConverged() 201 << " score: " << icp.getFitnessScore() << std::endl; 202 Eigen::Matrix4f icp_trans; 203 icp_trans = icp.getFinalTransformation(); 204 //cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl; 205 std::cout << icp_trans << endl; 206 //使用建立的變換對未過濾的輸入點雲進行變換 207 pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans); 208 //儲存轉換的輸入點雲 209 //pcl::io::savePCDFileASCII("bunny_transformed_sac_ndt.pcd", *icp_result); 210 211 //計算誤差 212 Eigen::Vector3f ANGLE_origin; 213 Eigen::Vector3f TRANS_origin; 214 ANGLE_origin << 0, 0, M_PI / 4; 215 TRANS_origin << 0, 0.3, 0.2; 216 double a_error_x, a_error_y, a_error_z; 217 double t_error_x, t_error_y, t_error_z; 218 Eigen::Vector3f ANGLE_result; 219 matrix2angle(icp_trans, ANGLE_result); 220 a_error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0)); 221 a_error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1)); 222 a_error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2)); 223 cout << "點雲實際旋轉角度:\n" << ANGLE_origin << endl; 224 cout << "x軸旋轉誤差 : " << a_error_x << " y軸旋轉誤差 : " << a_error_y << " z軸旋轉誤差 : " << a_error_z << endl; 225 226 cout << "點雲實際平移距離:\n" << TRANS_origin << endl; 227 t_error_x = fabs(icp_trans(0, 3)) - fabs(TRANS_origin(0)); 228 t_error_y = fabs(icp_trans(1, 3)) - fabs(TRANS_origin(1)); 229 t_error_z = fabs(icp_trans(2, 3)) - fabs(TRANS_origin(2)); 230 cout << "計算得到的平移距離" << endl << "x軸平移" << icp_trans(0, 3) << endl << "y軸平移" << icp_trans(1, 3) << endl << "z軸平移" << icp_trans(2, 3) << endl; 231 cout << "x軸平移誤差 : " << t_error_x << " y軸平移誤差 : " << t_error_y << " z軸平移誤差 : " << t_error_z << endl; 232 //視覺化 233 234 visualize_pcd2(cloud_src_o, cloud_tgt_o, icp_result, cloud_tgt_o); 235 236 return (0); 237 }