1、TRICP點雲配準原理
2、TRICP在pcl中的實作
trimmed ICP在PCL的recognition子產品中，具體實作在pcl\recognition\ransac_based\的trimmed_icp.h檔案中。具體實作代碼為align函數：
inline void
align (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 correspondences
pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);
// Some variables for the closest point search
pcl::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 correspondences
for ( int i = 0 ; i < num_source_points ; ++i )
{
// Transform the i-th source point based on the current transform matrix
aux::transform (guess_and_result, source_points.points[i], transformed_source_point);
// Perform the closest point search
kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);
// Update the i-th correspondence
full_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 distance
std::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 estimation
for ( 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函數庫