RF2O激光里程计算法源码解析与相关公式推导
Posted 秃头队长
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了RF2O激光里程计算法源码解析与相关公式推导相关的知识，希望对你有一定的参考价值。
目录
很早之前看过这篇论文，但是当时还是以多线激光雷达为主，最近又回到了2D激光里程计，所以有重新看了这部分代码，顺便记录一下！
之前的原理部分可参考：RF2O激光里程计算法原理
1.CMakeLists 起飞
先从 CMakeLists 开始看 ～
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
nav_msgs
sensor_msgs
std_msgs
tf
)
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 scans
if( myLaserOdom.is_initialized() && myLaserOdom.scan_available() )
//Process odometry estimation
myLaserOdom.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_scan
last_scan = *new_scan;
//Initialize module on first scan
if (first_laser_scan)
Init();
first_laser_scan = false;
else
//copy laser scan to internal variable
for (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.0625
cols_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 Hz
num_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 // 否则当前激光距离设为0
range[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;
else
range[i](u) = 0.f;
//-----------------------------------------------------------------------------
// 从第二层开始，做下采样，同样也是分为内部点，和边界点进行处理，原理与第一层相同，计算加权平均值
// 不过值得注意的是，从第二层开始 使用的数据是上一层下采样后的数据，即range[i_1]，因此每次计算都会缩小2倍。
else
for (unsigned int u = 0; u < cols_i; u++)
const int u2 = 2*u; // 比上次缩小二倍
const float dcenter = range[i_1](u2);
//Inner pixels
if ((u>0)&&(u<cols_i-1))
if (dcenter > 0.f)
float sum = 0.f;
float weight = 0.f;
for (int l=-2; l<3; l++)
const float abs_dif = abs(range[i_1](u2+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[i_1](u2+l);
range[i](u) = sum/weight;
else
range[i](u) = 0.f;
//Boundary
else
if (dcenter > 0.f)
float sum = 0.f;
float weight = 0.f;
const unsigned int cols_i2 = range[i_1].cols();
for (int l=-2; l<3; l++)
const int indu = u2+l;
if ((indu视觉激光融合——VLOAM / LIMO算法解析