RF2O-2D激光里程计算法源码解析与相关公式的详细推导

1.CMakeLists 起飞
2.main 函数
3.回调函数与数据初始化
4.计算激光里程计的主循环
- 4.1 创建图像金字塔 createImagePyramid
- 4.2 金字塔处理的八个步骤
- - 4.2.1 calculateCoord
  - 4.2.2 findNullPoints
  - 4.2.3 Compute derivatives
  - 4.2.4 computeNormals
  - 4.2.5 computeWeights
  - 4.2.6 solveSystemNonLinear
  - 4.2.7 filterLevelSolution
  - 4.2.8 performWarping
- 4.3 PoseUpdate

很早之前看过这篇论文，但是当时还是以多线激光雷达为主，最近又回到了2D激光里程计，所以有重新看了这部分代码，顺便记录一下！
之前的原理部分可参考：RF2O激光里程计算法原理。
本论文中缺少了一些理论介绍，在另一篇文章DI-FODO — 3D距离传感器的快速视觉里程计中～

设 R ( t ， α ) R(t，α) R(t，α)为范围扫描，其中 t t t是时间， α ∈ [ 0 ， N ) ⊂ R α \in [0，N)⊂R α∈[0，N)⊂R是扫描坐标，N是扫描的大小。任何点P相对于连接到传感器的局部参考系的位置由其极坐标 ( r ， θ ) (r，θ) (r，θ)给出，如果从激光雷达扫描点P，则 a a a如下所示，其中FOV为扫描仪的视野：

1.CMakeLists 起飞

先从 CMakeLists 开始看～

find_package(catkin REQUIRED COMPONENTS  roscpprospynav_msgssensor_msgsstd_msgstf
)
find_package(Boost REQUIRED COMPONENTS system)
find_package(cmake_modules REQUIRED)
find_package(Eigen REQUIRED)
find_package(MRPT REQUIRED base obs maps slam)

因为后面是打算去 ROS 的，所以比较关注工程的依赖，这里也就一个 tf ，看起来应该比较容易，还有一个陌生的库 MRPT ，在官网中是这样介绍的：

移动机器人编程工具包 (MRPT) 是一个广泛的、跨平台的开源 C++ 库，旨在帮助机器人研究人员设计和实施（主要）在同步定位和建图 (SLAM)、计算机视觉和运动规划（避障）。优点是代码的效率和可重用性。
这些库用于轻松管理 3D(6D) 几何的类、许多预定义变量（点和位姿、路标、地图）的概率密度函数 (pdf)、贝叶斯估计（卡尔曼滤波器、粒子滤波器）、图像处理、路径规划和障碍物躲避，所有类型的地图（点，栅格地图，路标，…）等的 3D 可视化。
高效地收集、操作和检查非常大的机器人数据集 (Rawlogs) 是 MRPT 的另一个目标，由多个类和应用程序支持。

具体在这里是怎么用的看了代码才能知道，全部代码都包括在一个 cpp 中，可以！

add_executable(rf2o_laser_odometry_node src/CLaserOdometry2D.cpp)

int main(int argc, char** argv)
{ros::init(argc, argv, "RF2O_LaserOdom");CLaserOdometry2D myLaserOdom;ROS_INFO("initialization complete...Looping");ros::Rate loop_rate(myLaserOdom.freq);while (ros::ok()) {ros::spinOnce();        //Check for new laser scansif( myLaserOdom.is_initialized() && myLaserOdom.scan_available() ) {            //Process odometry estimationmyLaserOdom.odometryCalculation();}else {ROS_WARN("Waiting for laser_scans....") ;}loop_rate.sleep();}return(0);
}

2.main 函数

首先就是新建了一个激光里程计对象，在该类的构造函数中，定义了激光数据的 topic 名称：/laser_scan，发布的 tf 坐标系关系 /base_link ，/odom ，以及发布的频率 freq ，都是一些基本的参数。然后就是定义了接收和发送 topic。

在 main 函数的主循环中，需要判断是否进行了初始化 Init() 和是否有新的激光数据，才会计算激光里程计。相当于两个线程在跑，一个通过消息回调接收数据，当有了新的数据，主循环中就会调用激光里程计计算位置信息。

3.回调函数与数据初始化

void CLaserOdometry2D::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan) {//Keep in memory the last received laser_scanlast_scan = *new_scan;//Initialize module on first scanif (first_laser_scan) {Init();first_laser_scan = false;}else {   //copy laser scan to internal variablefor (unsigned int i = 0; i<width; i++)range_wf(i) = new_scan->ranges[i];new_scan_available = true;}
}

在回调函数 LaserCallBack 中，也没做什么处理，会对一帧进行初始化 Init()，然后把后面的数据存到 range_wf 中，用于计算位置信息。

void CLaserOdometry2D::Init()
{// Got an initial scan laser, obtain its parametes 在第一帧激光数据，定义其参数ROS_INFO("Got first Laser Scan .... Configuring node");width = last_scan.ranges.size();    // Num of samples (size) of the scan laser 激光雷达的激光束数cols = width;                       // Max reolution. Should be similar to the width parameter fovh = fabs(last_scan.angle_max - last_scan.angle_min); // Horizontal Laser's FOV 激光雷达的水平视角ctf_levels = 5;                     // Coarse-to-Fine levels 设置一个由粗到细的 变换矩阵iter_irls = 5;                      //Num iterations to solve iterative reweighted least squares 最小二乘的迭代次数// Set laser pose on the robot (through tF) 通过 TF 设置激光雷达在小车上的位置// This allow estimation of the odometry with respect to the robot base reference system.// 得到的里程计信息是相对于机器人坐标系mrpt::poses::CPose3D LaserPoseOnTheRobot;tf::StampedTransform transform;try {// 得到激光坐标系 到 机器人坐标系的 转换关系tf_listener.lookupTransform("/base_link", last_scan.header.frame_id, ros::Time(0), transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();}//TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)const tf::Vector3 &t = transform.getOrigin(); // 平移LaserPoseOnTheRobot.x() = t[0];LaserPoseOnTheRobot.y() = t[1];LaserPoseOnTheRobot.z() = t[2];const tf::Matrix3x3 &basis = transform.getBasis(); // 旋转mrpt::math::CMatrixDouble33 R;for(int r = 0; r < 3; r++)for(int c = 0; c < 3; c++)R(r,c) = basis[r][c];LaserPoseOnTheRobot.setRotationMatrix(R);//Set the initial pose 设置激光雷达的初始位姿，在机器人坐标系下laser_pose = LaserPoseOnTheRobot;laser_oldpose = LaserPoseOnTheRobot;// Init module 初始化模型//-------------range_wf.setSize(1, width);//Resize vectors according to levels 初始化 由粗到精 的变化矩阵transformations.resize(ctf_levels);for (unsigned int i = 0; i < ctf_levels; i++)transformations[i].resize(3, 3);//Resize pyramid 调整金字塔大小unsigned int s, cols_i;const unsigned int pyr_levels = round(log2(round(float(width) / float(cols)))) + ctf_levels; // 金字塔层数，range.resize(pyr_levels);range_old.resize(pyr_levels);range_inter.resize(pyr_levels);xx.resize(pyr_levels);xx_inter.resize(pyr_levels);xx_old.resize(pyr_levels);yy.resize(pyr_levels);yy_inter.resize(pyr_levels);yy_old.resize(pyr_levels);range_warped.resize(pyr_levels);xx_warped.resize(pyr_levels);yy_warped.resize(pyr_levels);for (unsigned int i = 0; i < pyr_levels; i++) {s = pow(2.f, int(i));//每一层的尺度，1 - 0.5 - 0.25 - 0.125 - 0.0625cols_i = ceil(float(width) / float(s)); range[i].resize(1, cols_i);range_inter[i].resize(1, cols_i);range_old[i].resize(1, cols_i);range[i].assign(0.0f);range_old[i].assign(0.0f);xx[i].resize(1, cols_i);xx_inter[i].resize(1, cols_i);xx_old[i].resize(1, cols_i);xx[i].assign(0.0f);xx_old[i].assign(0.0f);yy[i].resize(1, cols_i);yy_inter[i].resize(1, cols_i);yy_old[i].resize(1, cols_i);yy[i].assign(0.f);yy_old[i].assign(0.f);if (cols_i <= cols) {range_warped[i].resize(1, cols_i);xx_warped[i].resize(1, cols_i);yy_warped[i].resize(1, cols_i);}}dt.resize(1, cols);dtita.resize(1, cols);normx.resize(1, cols);normy.resize(1, cols);norm_ang.resize(1, cols);weights.setSize(1, cols);null.setSize(1, cols);null.assign(0);cov_odo.assign(0.f);fps = 1.f;        //In Hznum_valid_range = 0; // 有效的激光距离数据个数// Compute gaussian mask 计算高斯掩码  [1/16，1/4，6/16，1/4，1/16] 用于计算加权平均值g_mask[0] = 1.f / 16.f; g_mask[1] = 0.25f; g_mask[2] = 6.f / 16.f; g_mask[3] = g_mask[1]; g_mask[4] = g_mask[0];kai_abs.assign(0.f);kai_loc_old.assign(0.f);module_initialized = true; // 模型初始化完成last_odom_time = ros::Time::now();
}

总的来说，在初始化的过程中，载入了激光数据的参数信息，并且初始化了一些容器。现在参数、计算需要的容器以及激光数据如同待宰羔羊，那么下面就要开始上手干活了。

4.计算激光里程计的主循环

下面就是 main 函数中的 odometryCalculation，开始计算激光里程计，在原有的代码中，注释也很明确。

4.1 创建图像金字塔 createImagePyramid

首先是创建图像金字塔 createImagePyramid，

加权均值计算，如果是边界则只计算一半：

void CLaserOdometry2D::createImagePyramid() {const float max_range_dif = 0.3f; // 最大的距离差值//Push the frames back 更新数据range_old.swap(range);xx_old.swap(xx);yy_old.swap(yy);// 金字塔的层数与里程计计算中使用的层数不匹配（因为我们有时希望以较低的分辨率完成）unsigned int pyr_levels = round(log2(round(float(width)/float(cols)))) + ctf_levels;//Generate levels 开始创建金字塔for (unsigned int i = 0; i<pyr_levels; i++) {unsigned int s = pow(2.f,int(i)); // 每次缩放 2 倍cols_i = ceil(float(width)/float(s));const unsigned int i_1 = i-1; // 上一层//First level -> Filter (not downsampling); if (i == 0) { // i = 0 第一层进行滤波for (unsigned int u = 0; u < cols_i; u++) { const float dcenter = range_wf(u); //Inner pixels  内部像素的处理， u从第三列开始到倒数第三列if ((u>1)&&(u<cols_i-2)) {        if (dcenter > 0.f) {    // 当前激光距离大于0,一般都满足float sum = 0.f; // 累加量，用于计算加权平均值float weight = 0.f; // 权重for (int l=-2; l<3; l++) { // 取附近五个点const float abs_dif = abs(range_wf(u+l)-dcenter); // 该点附近的五个点的距离，与当前点距离的绝对误差if (abs_dif < max_range_dif) { // 误差满足阈值const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif); // 认为越靠近该点的 权重应该最大，加权平均值weight += aux_w;sum += aux_w*range_wf(u+l);}}range[i](u) = sum/weight;}else // 否则当前激光距离设为0range[i](u) = 0.f;}//Boundary 下面对边界像素的处理else {if (dcenter > 0.f){     // 判断激光距离是否合法          float sum = 0.f;float weight = 0.f;for (int l=-2; l<3; l++) { // 计算加权平均值，只不过需要判断是否会越界const int indu = u+l;if ((indu>=0)&&(indu<cols_i)){const float abs_dif = abs(range_wf(indu)-dcenter);                                     if (abs_dif < max_range_dif){const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);weight += aux_w;sum += aux_w*range_wf(indu);}}}range[i](u) = sum/weight;}elserange[i](u) = 0.f;}}}//-----------------------------------------------------------------------------// 从第二层开始，做下采样，同样也是分为内部点，和边界点进行处理，原理与第一层相同，计算加权平均值// 不过值得注意的是，从第二层开始 使用的数据是上一层下采样后的数据，即range[i_1]，因此每次计算都会缩小2倍。// ......}
}

以上呢，就计算得到了每一层金字塔内所包含的点云位置，借用百度百科中的一张图，可以直观的看出图像金字塔的效果。不过这里是一个2D的金字塔，相当与降低了分辨率，用附近的点云距离，通过加权平均值算出一个”代表”。
计算出每层点的坐标：
a = N − 1 F O V θ + N − 1 2 = k a θ + N − 1 2 a = {N-1 \over FOV} \theta + {N-1 \over 2} = k_a \theta+{N-1 \over 2} a=FOVN−1θ+2N−1=kaθ+2N−1

//Calculate coordinates "xy" of the points
for (unsigned int u = 0; u < cols_i; u++) {if (range_1[i](u) > 0.f){const float tita = -0.5f*fovh + (float(u) + 0.5f)*fovh/float(cols_i);xx_1[i](u) = range_1[i](u)*cos(tita);yy_1[i](u) = range_1[i](u)*sin(tita);
}
else{xx_1[i](u) = 0.f;yy_1[i](u) = 0.f;
}

4.2 金字塔处理的八个步骤

然后对于每一层金字塔的处理分为八个步骤，也是核心部分，首先对整个流程介绍一下，后面会对各个函数进行详细的介绍。

void CLaserOdometry2D::odometryCalculation()
{m_clock.Tic();createImagePyramid();  // 计算金字塔//Coarse-to-fine scheme // 从金字塔的顶端开始开始for (unsigned int i=0; i<ctf_levels; i++) {//Previous computations  首先把旋转矩阵初始化transformations[i].setIdentity();level = i; // 记录目前计算进度，因为上一层的计算会当作下一层的初值，由粗到细的迭代unsigned int s = pow(2.f,int(ctf_levels-(i+1)));  // 从金字塔的最顶端开始，所以需要做一下处理，计算出最高层的缩放系数cols_i = ceil(float(cols)/float(s));  // 缩放后的数据长度image_level = ctf_levels - i + round(log2(round(float(width)/float(cols)))) - 1; // 当前需要处理的金字塔层数//1. Perform warping if (i == 0) {range_warped[image_level] = range[image_level];xx_warped[image_level] = xx[image_level];yy_warped[image_level] = yy[image_level];}elseperformWarping(); //2. Calculate inter coords  calculateCoord();//3. Find null points findNullPoints();//4. Compute derivatives calculaterangeDerivativesSurface();//5. Compute normals //computeNormals();//6. Compute weights computeWeights();//7. Solve odometry if (num_valid_range > 3) {solveSystemNonLinear();//solveSystemOneLevel();    //without robust-function}//8. Filter solution filterLevelSolution(); }m_runtime = 1000*m_clock.Tac();ROS_INFO("Time odometry (ms): %f", m_runtime);//Update posesPoseUpdate();new_scan_available = false;     //avoids the possibility to run twice on the same laser scan
}

4.2.1 calculateCoord

我们按照处理的顺序来，最高层并不会进行数据转换这部分，因此直接计算坐标。

void CLaserOdometry2D::calculateCoord()
{       for (unsigned int u = 0; u < cols_i; u++) // 遍历该层的所有数据信息{// 如果上一帧该层的数据 或者这一帧数据 为0就不处理if ((range_old[image_level](u) == 0.f) || (range_warped[image_level](u) == 0.f)){range_inter[image_level](u) = 0.f;xx_inter[image_level](u) = 0.f;yy_inter[image_level](u) = 0.f;}else // 否则 取两者均值{range_inter[image_level](u) = 0.5f*(range_old[image_level](u) + range_warped[image_level](u));xx_inter[image_level](u) = 0.5f*(xx_old[image_level](u) + xx_warped[image_level](u));yy_inter[image_level](u) = 0.5f*(yy_old[image_level](u) + yy_warped[image_level](u));}}
}

4.2.2 findNullPoints

找到一些距离为0的点，因为在之前金字塔缩放的时候，距离小与0的不合法激光数据都被置为0了。

void CLaserOdometry2D::findNullPoints()
{//Size of null matrix is set to its maximum size (constructor)// 记录空值的矩阵是 最大的数据大小，也就是和最底层的金字塔一样，不会访问越界num_valid_range = 0;for (unsigned int u = 1; u < cols_i-1; u++){if (range_inter[image_level](u) == 0.f)null(u) = 1;else{num_valid_range++; // 记录有效激光点的个数null(u) = 0;}}
}

4.2.3 Compute derivatives

我们的算法特别关注 range 梯度的计算。通常，使用固定的离散公式来近似扫描或图像梯度。在 range 数据的情况下，这种策略会导致对象边界处的梯度值非常高，这并不代表这些对象上的真实梯度。作为替代方案，我们使用关于环境几何形状的自适应公式。该公式使用连续观测（点）之间的 2D 距离对扫描中的前向和后向导数进行加权：
d ( a ) = ∣ ∣ ( x ( a ) − x ( a − 1 ) , y ( a ) − y ( a − 1 ) ) ∣ ∣ d(a) = ||(x(a) - x(a-1),y(a) - y(a-1))|| d(a)=∣∣(x(a)−x(a−1),y(a)−y(a−1))∣∣

// 计算相邻两点的 距离差值
for (unsigned int u = 0; u < cols_i-1; u++)
{const float dist = square(xx_inter[image_level](u+1) - xx_inter[image_level](u))+ square(yy_inter[image_level](u+1) - yy_inter[image_level](u));if (dist  > 0.f)rtita(u) = sqrt(dist);
}

R a − ( a ) = R ( a ) − R ( a − 1 ) , R a + = R ( a + 1 ) − R ( a ) R^-_a(a) = R(a) - R(a-1), R^+_a= R(a+1) -R(a) Ra−(a)=R(a)−R(a−1),Ra+=R(a+1)−R(a) R a ( a ) = d ( a + 1 ) R a − ( a ) + d ( a ) R a + ( a ) d ( a + 1 ) + d ( a ) R_a(a) = {d(a+1)R^-_a(a)+d(a)R^+_a(a) \over d(a+1)+d(a)} Ra(a)=d(a+1)+d(a)d(a+1)Ra−(a)+d(a)Ra+(a)


//Spatial derivatives
for (unsigned int u = 1; u < cols_i-1; u++)
dtita(u) = (rtita(u-1)*(range_inter[image_level](u+1)-range_inter[image_level](u)) + rtita(u)*    (range_inter[image_level](u) - range_inter[image_level](u-1)))/(rtita(u)+rtita(u-1));
// 第一对点 和 最后一对点 单独赋值
dtita(0) = dtita(1);
dtita(cols_i-1) = dtita(cols_i-2);

因此，最近的邻居总是对梯度计算贡献更多，而远点几乎不影响它。在两个邻居近似等距的情况下，所提出的公式等价于中心差分法。

 //Temporal derivative 对时间求导得到速度for (unsigned int u = 0; u < cols_i; u++)dt(u) = fps*(range_warped[image_level](u) - range_old[image_level](u));

4.2.4 computeNormals

void CLaserOdometry2D::computeNormals()
{normx.assign(0.f);normy.assign(0.f);norm_ang.assign(0.f);const float incr_tita = fovh/float(cols_i-1);  for (unsigned int u=0; u<cols_i; u++){if (null(u) == 0.f){const float tita = -0.5f*fovh + float(u)*incr_tita; const float alfa = -atan2(2.f*dtita(u), 2.f*range[image_level](u)*incr_tita);norm_ang(u) = tita + alfa;if (norm_ang(u) < -M_PI)norm_ang(u) += 2.f*M_PI;else if (norm_ang(u) < 0.f)norm_ang(u) += M_PI;else if (norm_ang(u) > M_PI)norm_ang(u) -= M_PI;normx(u) = cos(tita + alfa);normy(u) = sin(tita + alfa);}}
}

4.2.5 computeWeights

根据代码，我们把公式推导一下，其中设 k a = N − 1 F O V k_a = {N-1 \over FOV} ka=FOVN−1，则没束激光的角度可以求解得到：
a = N − 1 F O V θ + N − 1 2 = k a θ + N − 1 2 (1) a = {N-1 \over FOV} \theta + {N-1 \over 2} = k_a \theta+{N-1 \over 2} \tag{1} a=FOVN−1θ+2N−1=kaθ+2N−1(1)

const float kdtita = float(cols_i-1)/fovh;

刚性假设的不满足以及线性近似的偏差会导致扫描点对传感器运动施加的限制不准确，尽管 Cauchy M-estimator 可以减轻它们对整体运动估计的影响，但它并没有完全消除它。在求解运动之前很难检测到运动物体的存在，因此，在最小化过程中只能使用 Cauchy M-estimator 来降低它们的权重。另一方面，可以预先检测与线性近似的偏差，这有助于加速优化收敛并导致更准确的结果。

为此，我们提出了一种预加权策略来降低范围函数是非线性甚至不可微分的那些点的残差。我们称其为“预加权”，因为它是在解决最小化问题之前应用的。为了量化与线性化相关的误差，我们将泰勒级数扩展为二阶：
r ˙ = R t + R a a ˙ + R 2 o ( Δ t , a ˙ ) + O ( Δ t 2 , a ˙ ) \dot r = R_t + R_a\dot a +R_{2o}(\Delta t,\dot a) +O(\Delta t^2,\dot a) r˙=Rt+Raa˙+R2o(Δt,a˙)+O(Δt2,a˙) R 2 o ( Δ t , a ˙ ) = Δ t 2 ( R t t + R t a a ˙ + R a a a ˙ 2 ) (13) R_{2o} (\Delta t,\dot a)={\Delta t \over 2}(R_{tt}+R_{ta}\dot a +R_{aa}\dot a^2) \tag{13} R2o(Δt,a˙)=2Δt(Rtt+Rtaa˙+Raaa˙2)(13)

可以注意到，忽略高阶项， R 2 o ( Δ t , a ˙ 2 ) R_{2o}(Δt, \dot a^2) R2o(Δt,a˙2)的二阶导数可用于检测线性偏差。一种特殊情况是关于时间的二阶导数 ( R t t ) (R_{tt}) (Rtt)，它不能以粗到精的方案计算，因为扭曲的图像是不受时间影响的，因此，二阶时间导数的概念没有意义。此外，重要的是检测那些范围函数不仅非线性而且不可微的扫描区域。这些区域主要是观察到的不同对象的边缘，并且通常以非常高的一阶导数 ( R t , R α ) (R_t,R_α) (Rt,Rα)值为特征。为了惩罚这两种影响，非线性和不连续性，我们为每个扫描点定义以下预加权函数：
w ‾ = 1 ε + R α 2 + ∆ t 2 R t 2 + K d ( R α α 2 + ∆ t 2 R t α 2 ) \overline w = {1 \over \sqrt{\varepsilon+R_α^2+ ∆t^2R_t^2+K_d(R_{αα}^2+ ∆t^2R_{tα}^2)}} w=ε+Rα2+∆t2Rt2+Kd(Rαα2+∆t2Rtα2) 1

const float ini_dtita = range_old[image_level](u+1) - range_old[image_level](u-1);  // 与上一帧的该处数据的距离差
const float final_dtita = range_warped[image_level](u+1) - range_warped[image_level](u-1);const float dtitat = ini_dtita - final_dtita;
const float dtita2 = dtita(u+1) - dtita(u-1);const float w_der = kdt*square(dt(u)) + kdtita*square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2));

参数 K d K_d Kd标记一阶和二阶导数的相对重要性， ε \varepsilon ε是一个小常数，以避免奇异情况。

4.2.6 solveSystemNonLinear

在实践中，激光雷达运动不能仅通过三个独立的限制来估计，因为通常，（8）由于距离测量的噪声、线性近似（3）产生的误差或运动物体（非静态）的存在是不精确的环境）。因此，我们提出了一个密度公式，其中扫描的所有点都有助于运动估计。对于每个点，我们将几何残差 ρ ( ξ ) ρ(ξ) ρ(ξ) 定义为对给定扭曲 ξ ξ ξ 的距离流约束 (8) 的评估。
ρ ( ξ ) = R t + ( x s i n θ − y c o s θ − R a k a ) ω + ( c o s θ + R a k a s i n θ r ) v x + ( s i n θ − R a k a c o s θ r ) v y (9) ρ(ξ) = R_t +(xsin\theta - y cos\theta -R_ak_a)\omega + (cos\theta + {R_ak_asin\theta \over r})v_x +(sin\theta - {R_ak_acos\theta \over r})v_y \tag{9} ρ(ξ)=Rt+(xsinθ−ycosθ−Raka)ω+(cosθ+rRakasinθ)vx+(sinθ−rRakacosθ)vy(9)
k a = N − 1 F O V k_a = {N-1 \over FOV} ka=FOVN−1 ( x s i n θ − y c o s θ − R a k a ) ω + ( c o s θ + R a k a s i n θ r ) v x + ( s i n θ − R a k a c o s θ r ) v y = − R t (xsin\theta - y cos\theta -R_ak_a)\omega + (cos\theta + {R_ak_asin\theta \over r})v_x +(sin\theta - {R_ak_acos\theta \over r})v_y = -R_t (xsinθ−ycosθ−Raka)ω+(cosθ+rRakasinθ)vx+(sinθ−rRakacosθ)vy=−Rt ( c o s θ + R a k a s i n θ r s i n θ − R a k a c o s θ r x s i n θ − y c o s θ − R a k a ) ( v x v y w z ) = − R t \begin{pmatrix} cos\theta + {R_ak_asin\theta \over r} \\ sin\theta - {R_ak_acos\theta \over r} \\ xsin\theta - y cos\theta -R_ak_a \\ \end{pmatrix}\begin{pmatrix} v_x & v_y & w_z \\ \end{pmatrix} = -R_t ⎝⎛cosθ+rRakasinθsinθ−rRakacosθxsinθ−ycosθ−Raka⎠⎞(vxvywz)=−Rt A T x = B A^Tx=B ATx=B

for (unsigned int u = 1; u < cols_i-1; u++)if (null(u) == 0){// Precomputed expressionsconst float tw = weights(u);const float tita = -0.5*fovh + u/kdtita;//Fill the matrix AA(cont, 0) = tw*(cos(tita) + dtita(u)*kdtita*sin(tita)/range_inter[image_level](u));A(cont, 1) = tw*(sin(tita) - dtita(u)*kdtita*cos(tita)/range_inter[image_level](u));A(cont, 2) = tw*(-yy[image_level](u)*cos(tita) + xx[image_level](u)*sin(tita) - dtita(u)*kdtita);B(cont,0) = tw*(-dt(u));cont++;}
//Solve the linear system of equations using a minimum least squares method
MatrixXf AtA, AtB;
AtA.multiply_AtA(A);
AtB.multiply_AtB(A,B);
Var = AtA.ldlt().solve(AtB);
//Covariance matrix calculation     Cov Order -> vx,vy,wz
MatrixXf res(num_valid_range,1);
res = A*Var - B;

为了较好地处理离群点（比如环境中的运动物体），这里采用更加鲁棒的 Cauchy M-estimator 核函数代替2范数。该 F 函数是 Cauchy M-estimator ， k k k 是一个可调参数。
F ( p ) = k 2 2 l n ( 1 + 1 k 2 ) (11) F(p) = {k^2\over 2}ln(1+{1\over k}^2) \tag{11} F(p)=2k2ln(1+k12)(11)

float aver_dt = 0.f, aver_res = 0.f; unsigned int ind = 0;
for (unsigned int u = 1; u < cols_i-1; u++)if (null(u) == 0){aver_dt += fabsf(dt(u));aver_res += fabsf(res(ind++));}
aver_dt /= cont; aver_res /= cont;
const float k = 10.f/aver_dt; //200
//float energy = 0.f;
//for (unsigned int i=0; i<res.rows(); i++)
//  energy += log(1.f + square(k*res(i)));
//printf("\n\nEnergy(0) = %f", energy);

与更常见的 L2 或 L1 范数选择相反，此函数降低了具有非常高残差的点的相关性，并代表了一种处理异常值的有效且自动的方法。优化问题通过迭代重加权最小二乘法 (IRLS) 解决，其中与 Cauchy M-estimator 相关的权重为：
w ( p ) = 1 1 + ( p k ) 2 (12) w(p) = {1\over 1+({p \over k})^2} \tag{12} w(p)=1+(kp)21(12)

const float res_weight = sqrt(1.f/(1.f + square(k*res(cont))));
//Fill the matrix Aw
Aw(cont,0) = res_weight*A(cont,0);
Aw(cont,1) = res_weight*A(cont,1);
Aw(cont,2) = res_weight*A(cont,2);
Bw(cont) = res_weight*B(cont);

为了获得准确的估计，通过最小化 robust 函数 F 中的所有几何残差来计算传感器运动：
ξ M = a r g m i n ξ ∑ i = 1 N F ( p i ( ξ ) ) (10) ξ_M = \underset{ξ}{argmin} \sum_{i=1}^{N}F(p_i(ξ)) \tag{10} ξM=ξargmini=1∑NF(pi(ξ))(10)
使用 IRLS(迭代加权最小二乘)，系统通过重新计算残差和随后的权重来迭代求解，直到收敛。

//Solve the linear system of equations using a minimum least squares method
AtA.multiply_AtA(Aw);
AtB.multiply_AtB(Aw,Bw);
Var = AtA.ldlt().solve(AtB);
res = A*Var - B;

如果 M M M 矩阵是正定的，则场景的点提供了足够的几何约束来估计运动的 3 DOF。相反，如果 M M M 没有满秩，则无法估计一些线速度或角速度项，求解器为这些变量提供的解是没有意义的。可以通过分析与最小二乘解相关的协方差矩阵 Σ ξ ∈ R 3 × 3 Σ_ξ ∈ \mathbb R^{3×3} Σξ∈R3×3 来检测这种猜想
Σ ξ = 1 N − 3 ∑ i = 1 N r i 2 ( ( A w ) T A w ) − 1 Σ_ξ = {1 \over N −3}∑^N_{i=1}r^2_i((A^w )^T A^w )^{−1} Σξ=N−31i=1∑Nri2((Aw)TAw)−1
其中 r i r_i ri 是最小二乘解的残差。

cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
kai_loc_level = Var;

4.2.7 filterLevelSolution

最常见的情况是激光雷达只观测墙壁。在这种情况下，平行于墙壁的运动是不确定的，因此求解器将为其提供任意解决方案（不仅是我们的方法，而且任何基于纯几何学的方法）。为了缓解这个问题，我们在速度 ξ ξ ξ 的特征空间中应用了一个低通滤波器，其工作原理如下所述。
首先，分析IRLS解的协方差矩阵 Σ ∈ R 3 × 3 Σ∈\mathbb R^{3×3} Σ∈R3×3的特征值，以检测哪些运动（或运动组合）未确定，哪些运动完全受约束。在特征向量空间中，将速度 ξ M t ξ^t_M ξMt 与前一时间区间 ξ t − 1 ξ^{t-1} ξt−1 的速度加权，得到新的滤波速度：
[ ( 1 + k l ) I + k e E ] ξ t = ξ M t + ( k l I + k e E ) ξ t − 1 (20) [(1 +k_l)I+k_eE]ξ^t=ξ^t_M+ (k_lI+k_eE)ξ^{t−1} \tag{20} [(1+kl)I+keE]ξt=ξMt+(klI+keE)ξt−1(20)

其中 E E E 是一个对角矩阵，包含特征值 k l k_l kl 和 k e k_e ke， k k k 是滤波器的参数。具体来说， k l k_l kl 在求解器的解和先前的估计之间施加恒定的权重，同时定义特征值 k e k_e ke 如何影响最终估计。
k l = 0.05 e − ( l − 1 ) , k e = 15 × 1 0 3 e − ( l − 1 ) (21) k_l= 0.05e^{−(l−1)}, k_e= 15×10^3e^{−(l−1)}\tag{21} kl=0.05e−(l−1),ke=15×103e−(l−1)(21)

其中 l l l 是金字塔级别，范围从（最粗）到所考虑的级别数。

//   计算特征值和特征向量
SelfAdjointEigenSolver<MatrixXf> eigensolver(cov_odo);//首先，我们必须在“特征向量”中描述新的线速度和角速度
Matrix<float,3,3> Bii;
Matrix<float,3,1> kai_b;
Bii = eigensolver.eigenvectors();  // 求解特征向量
kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level); // 使用eigen库求解Bii*x=kai_loc_level//其次，我们也必须在“特征向量”中描述旧的线速度和角速度
CMatrixFloat31 kai_loc_sub;//重要提示：我们必须从以前的层数中减去这个结果
Matrix3f acu_trans;
acu_trans.setIdentity();
// 得到该层真实的变化，需要累加上之前的变化
for (unsigned int i=0; i<level; i++)acu_trans = transformations[i]*acu_trans;kai_loc_sub(0) = -fps*acu_trans(0,2);
kai_loc_sub(1) = -fps*acu_trans(1,2);
if (acu_trans(0,0) > 1.f)kai_loc_sub(2) = 0.f;
elsekai_loc_sub(2) = -fps*acos(acu_trans(0,0))*sign(acu_trans(1,0));
kai_loc_sub += kai_loc_old;Matrix<float,3,1> kai_b_old;
kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub);//Filter speed
const float cf = 15e3f*expf(-int(level)), df = 0.05f*expf(-int(level)); //  cf:ke df:kl

速度估计必须在金字塔的每一层进行过滤，因为每一层都可能缺乏几何特征。然而，最后的速度估计不能直接用于过滤每个级别的解，因为所有先前的级别已经估计了一些应该从中减去的运动。过滤器在金字塔的每一层执行的顺序步骤如下。

计算总速度估计 ξ i t , a c u ξ^{t,acu}_i ξit,acu 累积到当前层 i i i。
从最后的速度估计 ξ t − 1 ξ^{t−1} ξt−1 中减去 ξ i t , a c u ξ^{t,acu}_i ξit,acu得到 ξ i t , s u b ξ^{t,sub}_i ξit,sub 。
计算协方差矩阵 Σ ξ , i t Σ^t_{ξ ,i} Σξ,it 。
计算特征值 D i t D^t_i Dit 和特征向量 E i t E^t_i Eit 。
在基 E i t E^t_i Eit 中表示最小二乘解 ξ i t , s ξ^{t,s}_i ξit,s 和 ξ i t , s u b ξ^{t,sub}_i ξit,sub 。
以 ξ i , E t , s ξ^{t,s}_{i,E} ξi,Et,s 和 ξ i , E t , s u b ξ^{t,sub}_{i,E} ξi,Et,sub 作为输入应用(20)，得到 ξ i , E t ξ^t_{i,E} ξi,Et。
将 ξ i , E t ξ^t_{i,E} ξi,Et变换为相机的坐标系
ξ t = ξ M t + ( k l I + k e E ) ξ t − 1 [ ( 1 + k l ) I + k e E ] (20) ξ^t={ξ^t_M+ (k_lI+k_eE)ξ^{t−1} \over [(1 +k_l)I+k_eE]} \tag{20} ξt=[(1+kl)I+keE]ξMt+(klI+keE)ξt−1(20)

Matrix<float,3,1> kai_b_fil;
for (unsigned int i=0; i<3; i++)
{kai_b_fil(i,0) = (kai_b(i,0) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i,0))/(1.f + cf*eigensolver.eigenvalues()(i,0) + df);//kai_b_fil_f(i,0) = (1.f*kai_b(i,0) + 0.f*kai_b_old_f(i,0))/(1.0f + 0.f);
}//将滤波后的速度转换为局部参考系并计算转换
Matrix<float,3,1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil);

4.2.8 performWarping

线性化适用于连续扫描之间的小位移或具有恒定范围梯度（在激光雷达的情况下，会出现在非常不寻常的几何形状中：阿基米德螺旋）。为了克服这个限制，我们以粗到细的方案估计运动，其中较粗的水平提供粗略估计，随后在更精细的水平上得到改进。

令 R 0 R_0 R0、 R 1 R_1 R1 为连续两次激光扫描。最初，通过对原始扫描 R 0 R_0 R0、 R 1 R_1 R1 连续下采样（通常为 2）来创建两个高斯金字塔。通常，高斯掩码应用于对 RGB 或灰度图像进行下采样，但在 range 数据的情况下，标准高斯滤波器不是最佳选择，因为它会在过滤后的扫描中产生伪影。作为替代方案，我们采用双边滤波器，它不会混合可能属于场景不同对象的远距离点。一旦金字塔建成，速度估计问题就从最粗到最细的层次迭代求解。在每次转换到更精细的级别时，两个输入扫描中的一个必须根据前一个级别中估计的总体速度相对于另一个进行扭曲 ( ξ p ) (ξ_p) (ξp) 。

这种变形过程总是分为两个步骤，在我们的公式中，应用于第二次扫描 R 1 R_1 R1。首先，使用与扭转相关的刚体运动 ( ξ p ) (ξ_p) (ξp) 对每个在 R 1 R_1 R1 观测到的点 P P P 进行空间变换：
( x w y w 1 ) = e ξ ^ p ( x y 1 ) , e ξ ^ p = ( 0 − ω p v x , p ω o 0 v y , p 0 0 0 ) (16) \begin{pmatrix} x^w \\ y^w \\ 1 \\ \end{pmatrix}= e^{\hat ξ_p} \begin{pmatrix} x \\ y \\ 1 \\ \end{pmatrix},e^{\hat ξ_p} = \begin{pmatrix} 0 & -\omega_p & v_{x,p} \\ \omega_o & 0 & v_{y,p} \\ 0 & 0 & 0 \\ \end{pmatrix}\tag{16} ⎝⎛xwyw1⎠⎞=eξ^p⎝⎛xy1⎠⎞,eξ^p=⎝⎛0ωo0−ωp00vx,pvy,p0⎠⎞(16)

其次，必须将变换后的点重新投影到 R 1 R_1 R1 上以构建扭曲的扫描 R 1 w R^w_1 R1w，以便：
R 1 w ( a w ) = ( x w ) 2 + ( y w ) 2 (17) R^w_1(a^w) = \sqrt{(x^w)^2+(y^w)^2} \tag{17} R1w(aw)=(xw)2+(yw)2 (17) a w = k a a r c t a n ( y w x w ) + N − 1 2 = N − 1 F O V arctan ⁡ ( y w x w ) + N − 1 2 (18) a^w = k_a arctan({y^w\over x^w}) +{N-1 \over 2} = {N-1 \over FOV} \arctan({y^w\over x^w}) +{N-1 \over 2} \tag{18} aw=kaarctan(xwyw)+2N−1=FOVN−1arctan(xwyw)+2N−1(18)

//Transform point to the warped reference frame
const float x_w = acu_trans(0,0)*xx[image_level](j) + acu_trans(0,1)*yy[image_level](j) + acu_trans(0,2);
const float y_w = acu_trans(1,0)*xx[image_level](j) + acu_trans(1,1)*yy[image_level](j) + acu_trans(1,2);
const float tita_w = atan2(y_w, x_w);
const float range_w = sqrt(x_w*x_w + y_w*y_w);//Calculate warping
const float uwarp = kdtita*(tita_w + 0.5*fovh);

几个点可能被扭曲到相同的坐标 α w α^w αw，在这种情况下：

最接近的点被保留（其他点将被遮挡）
求加权平均值（例子中的代码）

//扭曲的像素（一般不是整数）对所有周围的像素都有贡献
if (( uwarp >= 0.f)&&( uwarp < cols_lim)){const int uwarp_l = uwarp;const int uwarp_r = uwarp_l + 1;const float delta_r = float(uwarp_r) - uwarp;const float delta_l = uwarp - float(uwarp_l);//1. 如果离整数很接近，那么就直接累加到该位置上if (abs(round(uwarp) - uwarp) < 0.05f){range_warped[image_level](round(uwarp)) += range_w;wacu(round(uwarp)) += 1.f; // 权重为1}else{// 2.否则加权平均到附近的两个像素上const float w_r = square(delta_l);range_warped[image_level](uwarp_r) += w_r*range_w;wacu(uwarp_r) += w_r; // 右边的权重为，距离右边整数的距离const float w_l = square(delta_r);range_warped[image_level](uwarp_l) += w_l*range_w;wacu(uwarp_l) += w_l;// 左边的权重为，距离左边整数的距离}
}

如果 ξ p ξ_p ξp 收敛到真实速度，那么扭曲扫描 R 1 w R^w_1 R1w 将比原始 R 1 R_1 R1 更接近第一个扫描 R 0 R_0 R0，这允许我们以更精细的分辨率应用（2）中的线性近似。

4.3 PoseUpdate

void CLaserOdometry2D::PoseUpdate()
{//First, compute the overall transformation//---------------------------------------------------Matrix3f acu_trans;acu_trans.setIdentity();for (unsigned int i=1; i<=ctf_levels; i++)acu_trans = transformations[i-1]*acu_trans;//             Compute kai_loc and kai_abs//--------------------------------------------------------kai_loc(0) = fps*acu_trans(0,2);kai_loc(1) = fps*acu_trans(1,2);if (acu_trans(0,0) > 1.f)kai_loc(2) = 0.f;elsekai_loc(2) = fps*acos(acu_trans(0,0))*sign(acu_trans(1,0));//cout << endl << "Arc cos (incr tita): " << kai_loc(2);float phi = laser_pose.yaw();kai_abs(0) = kai_loc(0)*cos(phi) - kai_loc(1)*sin(phi);kai_abs(1) = kai_loc(0)*sin(phi) + kai_loc(1)*cos(phi);kai_abs(2) = kai_loc(2);//                     Update poses//-------------------------------------------------------laser_oldpose = laser_pose;math::CMatrixDouble33 aux_acu = acu_trans;poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);laser_pose = laser_pose + pose_aux_2D;//               Compute kai_loc_old//-------------------------------------------------------phi = laser_pose.yaw();kai_loc_old(0) = kai_abs(0)*cos(phi) + kai_abs(1)*sin(phi);kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi);kai_loc_old(2) = kai_abs(2);ROS_INFO("LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());// GET ROBOT POSE from LASER POSE//------------------------------  mrpt::poses::CPose3D LaserPoseOnTheRobot_inv;tf::StampedTransform transform;try{tf_listener.lookupTransform(last_scan.header.frame_id, "/base_link", ros::Time(0), transform);}catch (tf::TransformException &ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();}//TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)const tf::Vector3 &t = transform.getOrigin();LaserPoseOnTheRobot_inv.x() = t[0];LaserPoseOnTheRobot_inv.y() = t[1];LaserPoseOnTheRobot_inv.z() = t[2];const tf::Matrix3x3 &basis = transform.getBasis();mrpt::math::CMatrixDouble33 R;for(int r = 0; r < 3; r++)for(int c = 0; c < 3; c++)R(r,c) = basis[r][c];LaserPoseOnTheRobot_inv.setRotationMatrix(R);//Compose Transformationsrobot_pose = laser_pose + LaserPoseOnTheRobot_inv;ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());// Estimate linear/angular speeds (mandatory for base_local_planner)// last_scan -> the last scan received// last_odom_time -> The time of the previous scan lasser used to estimate the pose//-------------------------------------------------------------------------------------double time_inc_sec = (last_scan.header.stamp - last_odom_time).toSec();last_odom_time = last_scan.header.stamp;double lin_speed = acu_trans(0,2) / time_inc_sec;//double lin_speed = sqrt( square(robot_oldpose.x()-robot_pose.x()) + square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec;double ang_inc = robot_pose.yaw() - robot_oldpose.yaw();if (ang_inc > 3.14159)ang_inc -= 2*3.14159;if (ang_inc < -3.14159)ang_inc += 2*3.14159;double ang_speed = ang_inc/time_inc_sec;robot_oldpose = robot_pose;
}

有问题欢迎大佬交流，码字不易，转载请注明出处！以后还会更新相关SLAM博文～

代码地址：
RF2O：
Github: Estimation of 2D odometry based on planar laser scans. Useful for mobile robots with innacurate base odometry.

SRF是RF2O的延续：
Github: Robust Planar Odometry Based on Symmetric Range Flow and Multi-Scan Alignment