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