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函数库