学习SLAM和做项目的时候一直在看代码，调用代码，但是很少真正的写和SLAM有关的代码，所以逐渐成为了一个理论上的巨人，实践上的菜鸟，痛定思痛，决定通过一些小项目逐步实现一套SLAM系统，锻炼自己的动手能力，也给自己将来的求职简历添一笔。
目录
效果总览：
本篇文章做了什么：基于C++，在ROS环境下实现了一个简单的激光里程计，能够在RVIZ下实时显示位姿的变化和运动的轨迹，并且用自己的雷达跑通。
环境：UBUNTU20.04，C++，ROS
雷达：镭神M10P
效果展示：
图中黄色的连线即为运动轨迹，odom代表我们运动的起点，base_link代表我们手持的雷达，当我们手持雷达运动的时候base_link的姿态和轨迹都会变动，如下图所示：
整体的算法流程和比较核心的函数如下图所示：
接下来就按照这个流程图的顺序，结合代码看如何一步一步实现一个雷达里程计
划重点：在实现里程计时出现了多次的坐标变换，一定要弄清楚不同坐标系之间的关系，每个数据处于什么坐标系下，为什么要进行坐标变换，不然就会一团糟，每一次涉及坐标变换的时候，我都会专门提出来讲解，希望大家pay attention
坐标变换在SLAM中是一个很重要的环节，充分理解每个坐标之间的变换关系和为什么要进行坐标变换有利于我们快速搞懂代码，对于坐标变换的理解可以参考下面的博文，讲的比较详细且通俗易懂，这里就不多做介绍了
Step1：雷达数据的订阅
ros::NodeHandle node_handle_;
ros::Subscriber laser_scan_subscriber;
laser_scan_subscriber_ = node_handle_.subscribe(
"/scan", 1, &ScanMatchPLICP::ScanCallback, this);这一步算是ROS的常规操作了，首先注册节点句柄，随后创建一个订阅者订阅雷达话题
Step2: 雷达数据初始化（预处理）
CreateCache()
这个函数的目的是把雷达数据中的固定参数算出来并存储，有利于避免重复运算，常见的固定参数有每个扫描点对应的角度值，扫描的最大距离和最小距离，代码如下：
void ScanMatchPLICP::CreateCache(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
//这里计算sin和cos值的目的是因为在后续需要用到这些角度值把雷达的数据转换为
//icp算法需要的数据
a_cos_.clear();
a_sin_.clear();
double angle;
for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
{
angle = scan_msg->angle_min + i * scan_msg->angle_increment;
a_cos_.push_back(cos(angle));
a_sin_.push_back(sin(angle));
}
//保存扫描距离的最大和最小的目的是后续用这两个值来判断一个雷达数据是否有效
input_.min_reading = scan_msg->range_min;
input_.max_reading = scan_msg->range_max;
}
GetBaseToLaserTf()
这个函数的目的是发布base_link到laser_link的坐标变换，
bool ScanMatchPLICP::GetBaseToLaserTf(const std::string &frame_id)
{
ros::Time t = ros::Time::now();
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = frame_id;
transformStamped.child_frame_id = base_frame;
transformStamped.transform.translation.x = 0.0;
transformStamped.transform.translation.y = 0.0;
transformStamped.transform.translation.z = 0.0;
transformStamped.transform.rotation.x = 0.0;
transformStamped.transform.rotation.y = 0.0;
transformStamped.transform.rotation.z = 0.0;
transformStamped.transform.rotation.w = 1.0;
tf2::fromMsg(transformStamped.transform, base_to_laser_);
laser_to_base_ = base_to_laser_.inverse();
return true;
}
这段代码发布了一个从baser_link到laser_link的静态坐标转换，认为两个坐标系重合，因为我们直接手持雷达进行测试，并没有说把雷达装在机器人的某个位置，那么就认为雷达和机器人是一体的，那么坐标系自然也是重合的。
LaserScanToLDP()
这个函数的目的是把订阅得到的雷达数据转换为ICP算法所需要的数据格式，方便在后续喂给ICP算法进行帧间配准从而计算位姿。
void ScanMatchPLICP::LaserScanToLDP(const sensor_msgs::LaserScan::ConstPtr &scan_msg, LDP &ldp)
{
unsigned int n = scan_msg->ranges.size();
ldp = ld_alloc_new(n);
for (unsigned int i = 0; i < n; i++)
{
// calculate position in laser frame
double r = scan_msg->ranges[i];
if (r > scan_msg->range_min && r < scan_msg->range_max)
{
// 填充雷达数据
ldp->valid[i] = 1;
ldp->readings[i] = r;
}
else
{
ldp->valid[i] = 0;
ldp->readings[i] = -1; // for invalid range
}
ldp->theta[i] = scan_msg->angle_min + i * scan_msg->angle_increment;
ldp->cluster[i] = -1;
}
ldp->min_theta = ldp->theta[0];
ldp->max_theta = ldp->theta[n - 1];
ldp->odometry[0] = 0.0;
ldp->odometry[1] = 0.0;
ldp->odometry[2] = 0.0;
ldp->true_pose[0] = 0.0;
ldp->true_pose[1] = 0.0;
ldp->true_pose[2] = 0.0;
}
到这里，雷达数据预处理就进行完毕了，我们回顾一下做了什么：
- 把雷达数据中固定的参数进行了计算并储存
- 得到了base_link到laser_link之间的坐标变换关系
- 把雷达数据转换成了ICP算法所需要的数据，以备后续投喂算法
注意：GetBaseToLaserTf和CreateCache只在第一帧雷达数据进来的时候调用一次，往后都不会再调用，但是每一帧数据都会调用LaserScanToLDP把这一帧的数据转换为ICP所需要的数据
Step3： ICP算法进行位姿匹配
第三步看起来很简单，就只是跑了一个算法，但是里面的细节很多，首先我们需要利用匀速运动模型为算法提供一个初始的位姿预测，这一步可以为算法提供一个还不错的初值，大家都知道ICP这种迭代算法是很吃初值的，初值给的好，迭代就会又快又好。随后我们调用ICP算法进行帧间匹配计算位姿，并发布一个累计的里程计和tf变换，作为可视化内容。
注意，在这个步骤中我们将进行多次的坐标系变换，分别是在位姿初值预测和运动轨迹发布这两个地方。
位姿初值预测要进行坐标变换，是因为我们依据匀速运动模型来预测下一时刻可能运动到的位置，这个过程我们是站在世界坐标系(map)下观察机器人(base_link)的运动，但是最终使用这个初值进行迭代的雷达处于laser_link坐标系下，所以我们要把这个预测的位姿从map转换到laser_link去，即map-odom-base_link-laser_link。
发布运动轨迹时要进行变换，是因为基于雷达获取的运动数据是在laser_link下的，但是我们希望在世界坐标系下观察机器人的运动，所以我们需要把雷达的运动从laser_link转换到map中去，但是在ROS中，坐标变换是一个数结构，不能一步到位，所以我们进行了从laser_link到base_link再到odom最后到map的多次变换，这里比较复杂，但是搞懂了这个再看代码，就容易理解了。
GetPrediction()
顾名思义，通过匀速运动模型预测下一时刻的位姿，为ICP提供初值
void ScanMatchPLICP::GetPrediction(double &prediction_change_x,
double &prediction_change_y,
double &prediction_change_angle,
double dt)
{
// 速度小于 1e-6 , 则认为是静止的
prediction_change_x = latest_velocity_.linear.x < 1e-6 ? 0.0 : dt * latest_velocity_.linear.x;
prediction_change_y = latest_velocity_.linear.y < 1e-6 ? 0.0 : dt * latest_velocity_.linear.y;
prediction_change_angle = latest_velocity_.linear.z < 1e-6 ? 0.0 : dt * latest_velocity_.linear.z;
if (prediction_change_angle >= M_PI)
prediction_change_angle -= 2.0 * M_PI;
else if (prediction_change_angle < -M_PI)
prediction_change_angle += 2.0 * M_PI;
}
匀速运动模型是指在dt这个时间内我们假设机器人匀速运动，从而根据dt内的线速度和角速度*dt来计算下一时刻机器人的位置和角度，这里的latest_velocity_本质上是一个geometry_msgs::Twist类型的数据，是由上一帧雷达数据跑ICP得到的，
，
除以dt得到的，而雷达在这里作为里程计，所以这个位置预测是基于里程计数据计算得到的，处于odom坐标系，如果要喂给雷达ICP当做它的匹配初值，需要做从odom坐标系到laser_link的转换，如下：
double dt = (time - last_icp_time_).toSec();
double pr_ch_x, pr_ch_y, pr_ch_a;
GetPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
// predicition_change用来保存tf变换关系
tf2::Transform prediction_change;
CreateTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, prediction_change);
// 将odom坐标系下的坐标变换 转换成 base_in_odom_keyframe_坐标系下的坐标变换
prediction_change = prediction_change * (base_in_odom_ * base_in_odom_keyframe_.inverse());
// 将base_link坐标系下的坐标变换 转换成 雷达坐标系下的坐标变换
tf2::Transform prediction_change_lidar;
prediction_change_lidar = laser_to_base_ * base_in_odom_.inverse() * prediction_change * base_in_odom_ * base_to_laser_;
// 转到lser_link以后，才能够喂给icp，input_就是icp的输入参数，下面的代码是设定icp的初值的意思
input_.first_guess[0] = prediction_change_lidar.getOrigin().getX();
input_.first_guess[1] = prediction_change_lidar.getOrigin().getY();
input_.first_guess[2] = tf2::getYaw(prediction_change_lidar.getRotation());
CreateTfFromXYTheta()
这个函数的作用就是利用x，y，theta来赋值一个TF转换矩阵T，x，y，theta描述了两帧雷达之间的转换，TF中的转换矩阵T也可以用来描述两帧雷达之间的转换，但是在RVIZ中我们是通过发布TF来实现可视化的，所以这里把描述形式换一下，从xytheta改为转换矩阵T。
void ScanMatchPLICP::CreateTfFromXYTheta(double x, double y, double theta, tf2::Transform &t)
{
t.setOrigin(tf2::Vector3(x, y, 0.0));
tf2::Quaternion q;
q.setRPY(0.0, 0.0, theta);
t.setRotation(q);
}
sm_icp()
完成了这一切以后就只需要把数据丢给ICP算法即可，sm_icp一行代码搞定：
sm_icp(&input_, &output_);input_是传入的数据，之前在雷达数据预处理中的LaserScanToLDP函数，就把当前雷达的数据转换为了ICP的格式放在了input_里面，当然input_还包含了一些默认参数的配置，但是我们这里只做一个简单的实现，不去进行精确的调参，所以先略过，ICP算法会把输出的结果保存在output_里面，包含了匹配得到的位姿变换，，
，
等等。
最后就是进行里程数据和TF的发布和线速度角速度的计算，线速度和角速度就是用来为下一次ICP提供初值的，同时这里又涉及到了坐标变换，因为ICP计算得到的值是在laser_link坐标系下的，最后我们发布的里程计数据和TF是odom坐标系下的，所以要做一个从laser_link到odom的变换，如下：
tf2::Transform corr_ch;
if (output_.valid)
{
// 雷达坐标系下的坐标变换
tf2::Transform corr_ch_l;
CreateTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
// 将雷达坐标系下的坐标变换 转换成 base_link坐标系下的坐标变换
corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
// 更新 base_link 在 odom 坐标系下 的坐标
base_in_odom_ = base_in_odom_keyframe_ * corr_ch;
//计算线速度和角速度，这两个速度在下一帧数据到来的时候会用来计算一个预测的位置
//为下一次ICP提供初值
latest_velocity_.linear.x = corr_ch.getOrigin().getX() / dt;
latest_velocity_.angular.z = tf2::getYaw(corr_ch.getRotation()) / dt;
}
else
{
ROS_WARN("not Converged");
}
PublishTFAndOdometry()
最后把里程数据和TF发布出去，就可以在里程计运行的时候通过RVIZ实时查看了。
void ScanMatchPLICP::PublishTFAndOdometry()
{
geometry_msgs::TransformStamped tf_msg;
tf_msg.header.stamp = current_time_;
tf_msg.header.frame_id = odom_frame_;
tf_msg.child_frame_id = base_frame_;
tf_msg.transform = tf2::toMsg(base_in_odom_);
// 发布 odom 到 base_link 的 tf
tf_broadcaster_.sendTransform(tf_msg);
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = current_time_;
odom_msg.header.frame_id = odom_frame_;
odom_msg.child_frame_id = base_frame_;
tf2::toMsg(base_in_odom_, odom_msg.pose.pose);
odom_msg.twist.twist = latest_velocity_;
// 发布 odomemtry 话题
odom_publisher_.publish(odom_msg);
}
到这里，我们就完成了一个2D激光里程计，完结撒花之前，回顾一下这一步我们做了什么：
- 通过匀速运动模型预测下一时刻的位置，为ICP提供初值
- 进行坐标转换，将初值转换到laser_link下
- 调用ICP算法进行帧间匹配获得位姿变换
- 将获得的位姿变换结果从laser_link下转换到odom中
- 基于位姿变换发布TF和里程计
结语
将以上代码写在雷达的回调函数中，接上雷达，在ROS中把雷达节点开起来，再launch这个里程计，就可以在RVIZ中查看效果了，利用镭神M10P跑这个里程计，在旋转方面的精度非常高，但是在平移方面还有待提高，后续可能会研究一下如何解决这个问题，如果有手上没有雷达的小伙伴，可以利用gazebo做仿真，效果是一样的，这个后续也考虑出一篇博客写一下如何在gazebo中测试这个小破里程计，下一步是研究如何建图了，希望能够一步一步的积累知识和技能，最终实现一个完整的自己研发的slam系统~