laser_scan_matcher 坐标变换代码分析

整个是激光回调函数中的部分 代码

getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);

这里是先用里程计算出，上一次回调和本次回调时间内，里程计的增量。

tf::Transform pr_ch;

createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);

把增量变换成tf类型的变换，相当于一个齐次坐标

目标是得到 laser坐标系下，从参考帧PF到本次回调这时间内的变换关系

f2b_：上一次回调时，base_link在odom下的坐标变换 是齐次坐标 即位姿

f2b_pf_: 关键帧那次回调，base_link在odom下的坐标变换 即位姿

此外，有这样的更新代码在后面 ：f2b_ = f2b_pf_ * corr_ch;

if (newKeyframeNeeded(corr_ch)) f2b_kf_ = f2b_;

关键帧是机器人移动了一段距离或旋转了一定角度达到阈值时得到激光帧作为关键帧。

所以f2b_pf_不是每次都更新的。

f2b_是回调一次如果匹配算法成功了就会更新的，公式就是f2b_ = f2b_pf_ * corr_ch;是从最近的这次关键帧做一个增量的变换得到。

下面分析下面的代码，其目的是得到pf->now时间内，laser的坐标变换

pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse());

(f2b_ * f2b_kf_.inverse()）会得到一个增量，即关键帧到上一次回调时间内，base在odom下的变换，再左乘一个pr_ch即上一次到本次回调时间内base的变换，就得到了pf到本次回调时间内base再odom系的坐标变换。

tf::Transform pr_ch_l;

pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ;

这个计算我是这么来看的，先看中间三项 f2b_.inverse() * pr_ch * f2b_，用坐标变换的左乘来分析，f2b_先左乘pr_ch,即将f2b_做了一个pr_ch的变换，这个变换是以固定坐标系来变换的，就是odom，然后再左乘f2b_inverse(), 又做了一个f2b_的反向变换，f2b_是上一次回调时，base_link在odom下的坐标变换，最终得到了，base_link在odom系下，pf->now时间内，base_link在odom下的坐标变换，因为f2b_的逆抵消掉了开始的f2b_,剩下的就是pr_ch的变换了，也就是 得到了

Tbasepf_basenow ,  公式变成了  laser_to_base_ * Tbasepf_basenow  * base_to_laser_，

就得到了，laser系下，pf->now的时间内，laser的变换。这个作为该次回调时 激光雷达的相对于PF时的激光雷达的位姿