1、TRICP點云配準原理

?2、TRICP在pcl中的實現

trimmed ICP在PCL的recognition模塊中，具體實現在pcl\recognition\ransac_based\的trimmed_icp.h文件中。具體實現代碼為align函數：

inline voidalign (const PointCloud& source_points, int num_source_points_to_use, Matrix4& guess_and_result) const{int num_trimmed_source_points = num_source_points_to_use, num_source_points = static_cast<int> (source_points.size ());if ( num_trimmed_source_points >= num_source_points ){printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to ""the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set ""the number of source points to use to a lower value or use standard ICP.\n", __func__);num_trimmed_source_points = num_source_points;}// These are vectors containing source to target correspondencespcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);// Some variables for the closest point searchpcl::PointXYZ transformed_source_point;std::vector<int> target_index (1);std::vector<float> sqr_dist_to_target (1);float old_energy, energy = std::numeric_limits<float>::max ();// printf ("\nalign\n");do{// Update the correspondencesfor ( int i = 0 ; i < num_source_points ; ++i ){// Transform the i-th source point based on the current transform matrixaux::transform (guess_and_result, source_points.points[i], transformed_source_point);// Perform the closest point searchkdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);// Update the i-th correspondencefull_src_to_tgt[i].index_query = i;full_src_to_tgt[i].index_match = target_index[0];full_src_to_tgt[i].distance = sqr_dist_to_target[0];}// Sort in ascending order according to the squared distancestd::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences);old_energy = energy;energy = 0.0f;// Now, setup the trimmed correspondences used for the transform estimationfor ( int i = 0 ; i < num_trimmed_source_points ; ++i ){trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query;trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match;energy += full_src_to_tgt[i].distance;}this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result);// printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy);}while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress// printf ("\n");}

函數參數：

source_points:待配準點云 num_source_points_to_use：重疊點云數量 guess_and_result：變換矩陣初值

3、rmse值計算

tricp沒有相對應的rmse值對應的庫函數調用，下面是rmse值計算的程序。

class ComputeRmse { public:void ComupteRmse(PointT target_cloud, PointT after_registraed_cloud,double max_range){pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>core;core.setInputSource(after_registraed_cloud);core.setInputTarget(target_cloud);pcl::Correspondences all;core.determineReciprocalCorrespondences(all);float sum = 0.0, sum_x = 0.0, sum_y = 0.0, sum_z = 0.0, rmse, rmse_x, rmse_y, rmse_z;std::vector<float>Co;for (size_t j = 0; j < all.size(); j++) {sum += all[j].distance;Co.push_back(all[j].distance);sum_x += pow((target_cloud->points[all[j].index_match].x - after_registraed_cloud->points[all[j].index_query].x), 2);sum_y += pow((target_cloud->points[all[j].index_match].y - after_registraed_cloud->points[all[j].index_query].y), 2);sum_z += pow((target_cloud->points[all[j].index_match].z - after_registraed_cloud->points[all[j].index_query].z), 2);}rmse = sqrt(sum / all.size()); //均方根誤差rmse_x = sqrt(sum_x / all.size()); //X方向均方根誤差rmse_y = sqrt(sum_y / all.size()); //Y方向均方根誤差rmse_z = sqrt(sum_z / all.size()); //Z方向均方根誤差std::vector<float>::iterator max = max_element(Co.begin(), Co.end());//獲取最大距離的對應點std::vector<float>::iterator min = min_element(Co.begin(), Co.end());//獲取最小距離的對應點cout << "匹配點對個數" << all.size() << endl;cout << "距離最大值" << sqrt(*max) * 100 << "厘米" << endl;cout << "距離最小值" << sqrt(*min) * 100 << "厘米" << endl;cout << "均方根誤差" << rmse << "米" << endl;cout << "X均方根誤差" << rmse_x << "米" << endl;cout << "Y均方根誤差" << rmse_y << "米" << endl;cout << "Z均方根誤差" << rmse_z << "米" << endl;} };

?主要利用的是Class pcl::registration::CorrespondenceEstimation< PointSource, PointTarget, Scalar >，類CorrespondenceEstimation是確定目標和查詢點集(或特征)之間的對應關系的基類，輸入為目標和源點云，輸出為點對，即輸出兩組點云之間對應點集合。

#include <pcl/registration/correspondence_estimation.h> CorrespondenceEstimation () // 空構造函數 virtual ~CorrespondenceEstimation () // 空析構函數 virtual void determineCorrespondences (pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) // 確定輸入點云和目標點云的對應關系:輸入源點云和對應目標點云之間允許的最大距離max_distance,輸出找到的對應關系(查詢點索引、目標點索引和他們之間的距離)存儲在correspondences中。 virtual void determineReciprocalCorrespondences (pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) // 確定輸入點云和目標點云的對應關系:輸出找到的對應關系(查詢點索引、目標點索引和他們之間的距離存儲在correspondences中。該函數與上相同,但是查找的對應點是相互的。 virtual boost::shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > clone () const // 復制并強制轉換為CorrespondenceEstimationBase。 void setInputTarget (const PointCloudTargetConstPtr &cloud) // 設置指向目標點云的指針cloud。 void setPointRepresentation (const PointRepresentationConstPtr &point_representation) // 當對點進行比較的時,設置指向PointRepresentation 的 boost庫共享指針 point_representation,點的表示實現對點到n維特征向量的轉化,進而在對應點集搜索時使用kdtree進行搜索。

參考連接：

trimmed ICP及其在PCL代碼解析與使用_xinxiangwangzhi_的博客-CSDN博客

PCL函數庫摘要——點云配準_悠緣之空的博客-CSDN博客_pcl函數庫

總結

以上是生活随笔為你收集整理的TRICP点云配准的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。