激光SLAM放弃指南4：PL-ICP的实现

参考链接：1.李太白lx 系列文章 从零开始搭二维激光SLAM link

计算运算时间差值

// step1 进行数据类型转换
std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
...
std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
std::cout << "\n转换数据格式用时: " << time_used.count() << " 秒。" << std::endl;

ICP的函数

#include <iostream>                 //标准输入输出头文件
#include <pcl/io/pcd_io.h>          //I/O操作头文件
#include <pcl/point_types.h>        //点类型定义头文件
#define BOOST_TYPEOF_EMULATION  //要加在#include <pcl/registration/icp.h>前
#include <pcl/registration/icp.h>   //ICP配准类相关头文件
#include <pcl/visualization/cloud_viewer.h>//点云查看窗口头文件
#include <pcl/visualization/pcl_visualizer.h>//可视化头文件

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  //创建ICP的实例类
icp.setInputSource(cloud_sources); // 源点云。指将要被旋转和平移的点云。
icp.setInputTarget(cloud_target);// 目标点云。从Source到Target匹配，将源点云变换到目标点云的相对位置。
icp.setMaxCorrespondenceDistance(100);   //设置对应点对之间的最大距离（此值对配准结果影响较大）如果两个点云距离大于此值，则被忽略（PCL默认距离单位是m），一般根据两个点云之间的距离估计，这个值对点云匹配的结果影响较大。
icp.setTransformationEpsilon(1e-10); //  设置两次变化矩阵之间的差值（一般设置为1e-10即可）
icp.setEuclideanFitnessEpsilon(0.001);   //设置收敛条件是均方误差和小于阈值， 停止迭代；
nline void setEuclideanFitnessEpsilon (doubleepsilon)      前后两次迭代方差的差值，可以用来终止迭代次数。
icp.setMaximumIterations(100);     //  最大迭代次数，icp是一个迭代的方法，最多迭代这些次
icc.align(final)；  // final 为配准后点云。
inline Matrix4 getFinalTransformation ()    获取最终的转换矩阵。

如果由于设置相关参数过于苛刻，而造成没有匹配成功法， icp.hasConverged() 会提示匹配失败，并且 icp.getFitnessScore() 为0，相关的转换矩阵为单位矩阵。

 if (icp_.hasConverged() == false)
    {
        std::cout << "not Converged" << std::endl;
    }

CSM的数据格式

struct sm_params {
// 第一个雷达数据
LDP laser_ref;
//第二个雷达数据
LDP laser_sens;
//位姿的预测值多少.设置为0 就是不预测
double first_guess[3]; 
//扫描之间的最大角位移（度）/
double max_angular_correction_deg;
//扫描之间的最大平移（米）
double max_linear_correction;
//最大迭代次数
int max_iterations;
//停止阀值
double epsilon_xy;
double epsilon_theta;
//通信有效的最大距离
double max_correspondence_dist;
// ？？？不懂这个是什么
int use_corr_tricks;
//如果错误低于阈值（0或1），则重新启动*
int restart;
//重新启动阈值
double restart_threshold_mean_error;
//重新启动位移
double restart_dt;
double restart_dtheta;
//异常值的最大百分比
double outliers_maxPerc;
// 异常值自适应顺序
double outliers_adaptive_order; /* 0.7 */
double outliers_adaptive_mult; /* 2 */

//不允许两个不同的通信共享一个点
int outliers_remove_doubles; 
 // 聚类阈值
double clustering_threshold;
//用于估计方向的相邻光线数
int orientation_neighbourhood;
//根据角度丢弃对应关系
int do_alpha_test;
double do_alpha_test_thresholdDeg;

// 可见性测试
int do_visibility_test;

//判断点到线的距离 如果为1，则使用PlICP；如果为0，则使用ICP
int use_point_to_line_distance;
// ？？？ 不懂
int use_ml_weights;
int use_sigma_weights;

//计算协方差
int do_compute_covariance;

//检查是否找到对应项，并给出正确答案
int debug_verify_tricks;

//传感器相对于机器人的姿态：用于计算给出了里程计的第一个估计值
double laser[3]; 

// 数据中有无效值
double sigma;

// 将此间隔之外的数据标记为无效（=不使用）
double min_reading, max_reading;

//特定于GPM的参数（未完成：-/）
double gpm_theta_bin_size_deg;
double gpm_extend_range_deg; 
int gpm_interval;
// 特定于HSM的参数（未完成：-/）
struct hsm_params hsm;
};

struct sm_result {
//如果是否有效
int valid;
//扫描匹配结果（x，y，θ）
double x[3];
//完成的迭代次数
int iterations;
//最后有效通信数
int nvalid;
//总通讯错误数
double error;

// 用于协方差计算的字段
#ifndef RUBY
gsl_matrix *cov_x_m;	
gsl_matrix *dx_dy1_m;
gsl_matrix *dx_dy2_m;
#endif
};

运行的时候报错

gsl: lu.c:266: ERROR: matrix is singular
Default GSL error handler invoked.

需要设置这个参数

input_.do_compute_covariance = 0;