zhuanlan.zhihu.com/p/338433941
SLAM 介绍
机器人想要屋子里进行导航。首先通过激光雷达，摄像头等传感器确定了周围环境的地图（地图构建），当机器人向前走了1米，这时候再将周围的环境建成地图，而且要将地图准确地放到之前建的地图的相应位置处（定位）。
定位，是通过将当前传感器感知到的环境信息与构建好的环境地图进行匹配，确定机器人在当前地图中的位置，只有地图准确了，定位才能够准确。
地图构建，通过将当前传感器感知到的环境信息构建成地图，这时的地图是要放到机器人当前位置处的，所以只有定位准确了，构建的地图才能够与真实环境相符合。
所以定位与地图构建，二者相互依赖，必须要同时进行求解才能构建好地图。
SLAM 则是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写。SLAM 是通过传感器获取环境信息然后进行定位和建图。在 ROS 2 中，提供了很多的 SLAM 功能包，比如 slam_toolbox，cartographer_ros 和 rtabmap_slam 等。
一些传统的SLAM算法如下所示．
- 二维激光SLAM：hector，gmapping，karto，cartographer等等．
- 三维激光SLAM：LOAM，Lego-LOAM，LIO-SAM，HDL graph SLAM，cartographer等等．
目前，主流SLAM的结构基本都分为前端里程计，后端优化，回环检测三个大模块．前端的目的是始终累加位姿，作为里程计使用；后端使用图的结构模型，优化整体位姿，减小前端里程计产生的累计误差；回环检测可以提供一个更强烈的图结构的约束，能够更好的减小累计误差．
几个比较关键的技术如下：
- 传感器感知通过各类传感器实现对环境的感知，比如通过激光雷达获取环境的深度信息。同时可以通过传感器融合来提高位姿估计的精度，比如融合轮式里程计、IMU、雷达、深度相机数据等。
- 视觉/激光里程计 基本原理是通过前后数据之间对比得出机器人位姿的变化。
- 回环检测判断机器人是否到达之前到过的位姿，可以解决位置估计误差问题，建图时可以纠正地图误差
SLAM 问题概率描述
首先，一个完整的SLAM问题是在给定传感器数据的情况下，同时进行机器人位姿和地图的估计的问题。然而，现实的情况是这样的，如果需要得到一个精确的位姿需要与地图进行匹配，如果需要得到一个好的地图需要有精确的位姿才能做到，显然这是一个相互矛盾的问题。
概率描述如下式（3.1）：显然是一个条件联合概率分布
用在移动机器人从开机到t时刻一系列传感器测量数据z1:t（这里当然是/scan）以及一系列控制数据u1:t(这里认为是/odom)的条件下，同时对地图m、机器人轨迹状态x1:t进行的估计 来描述SLAM问题。
FastSLAM 算法独辟蹊径，采用RBPF方法，将SLAM算法分解成两个问题。一个是机器人定位问题，另一个是已知机器人位姿进行地图构建的问题。
其中p(x1:t | u1:t, z1:t)表示估计机器人的轨迹，p(m|x1:t, z1:t) 表示在已知机器人轨迹和传感器观测数据情况下，进行地图构建的闭式计算。这样 SLAM 问题就分解成两个问题。其中已知机器人位姿的地图构建是个简单问题，所以机器人位姿的估计是一个重点问题。
每个粒子都携带这一时刻的位姿、权重、地图。这样每一个粒子都存储了一个机器人轨迹，以及一张环境地图。
SLAM 重要概念
三大任务：Scan to Scan Match ； Scan to Map Match ； 图优化理论（闭环或者回环检测） 。主要就是基于优化理论的
传感器
雷达
普通的单线激光雷达一般有一个发射器，一个接收器，发射器发出激光射线到前方的目标上，物品会将激光反射回来，然后激光雷达的接收器可以检测到反射的激光。
通过计算发送和反馈之间的时间间隔，乘上激光的速度，就可以计算出激光飞行的距离，该计算方法称为TOF（飞行时间法Time of flight，也称时差法）。
除了TOF之外还有其他方法进行测距，比如三角法，这里就不拓展了放一篇文章，大家自行阅读。
ROS2 中雷达 topic /scan 的数据类型是 sensor_msgs/msg/LaserScan , 使用 ros2 interface 查看具体结构如下：
ini
$ ros2 interface show sensor_msgs/msg/LaserScan
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data
std_msgs/Header header # timestamp in the header is the acquisition time of
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
float32[] ranges # range data [m]
# (Note: values < range_min or > range_max should be discarded)
float32[] intensities # intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty. 下面实现一个 Node 订阅 /scan topic ，并解析 LaserScan msg 的雷达数据 创建一个 ROS2 包
css
ros2 pkg create lesson1 --build-type ament_cmake --dependencies rclcpp sensor_msgs创建代码文件 laser_scan_node.cpp
arduino
#include "rclcpp/rclcpp.hpp"
#include "rosgraph_msgs/msg/clock.hpp" // Add the clock message
#include "sensor_msgs/msg/laser_scan.hpp"
using std::placeholders::_1;
using std::placeholders::_2;
/*
创建一个类节点，名字叫做 LaserScanNode,继承自Node.
*/
class LaserScanNode : public rclcpp::Node
{
private:
// 声明一个订阅者（成员变量）,用于订阅 topic /scan
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laserScan;
public:
LaserScanNode(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(), "Hello world: %s", name.c_str());
//QOS:
auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
laserScan = this->create_subscription<sensor_msgs::msg::LaserScan>("scan", sensor_qos, std::bind(&LaserScanNode::laserScanCallback, this, _1));
}
private:
// 收到话题数据的回调函数
void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
{
// 打印雷达元信息
RCLCPP_INFO_STREAM(this->get_logger(), "time stamp: " << scan_msg->header.stamp.sec <<
", frame_id: " << scan_msg->header.frame_id <<
", angle_min: " << scan_msg->angle_min <<
", angle_max: " << scan_msg->angle_max <<
", angle_increment: " << scan_msg->angle_increment <<
", time_increment: " << scan_msg->time_increment <<
", scan_time: " << scan_msg->scan_time <<
", range_min: " << scan_msg->range_min <<
", range_max: " << scan_msg->range_max <<
", range size: " << scan_msg->ranges.size() <<
", intensities size: " << scan_msg->intensities.size());
// 遍历 ranges ，解析扫描点的欧式坐标为
auto range_size = scan_msg->ranges.size();
for(int i = 0; i < range_size; i++) {
double range = scan_msg->ranges[i];
double angle = scan_msg->angle_min + scan_msg->angle_increment * i;
double x = range * cos(angle);
double y = range * sin(angle);
RCLCPP_INFO(this->get_logger(), "%d Range: %lf, angle: %lf, x: %lf, y: %lf", i, range, angle, x, y);
}
}
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
/* 产生一个 lesson1_laser_scan_node 节点 */
auto node = std::make_shared<LaserScanNode>("lesson1_laser_scan_node");
/* 运行节点，并检测退出信号 */
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}编译运行
arduino
colcon build --packages-select lesson1
ros2 run lesson1 laser_scan_node 启动雷达小车，再启动雷达数据解析 Node ，输出以下日志：
yaml
[INFO] [1754138926.671158368] [lesson1_laser_scan_node]: time stamp: 1754138926, frame_id: laser_link, angle_min: -3.14159, angle_max: 3.14159, angle_increment: 0.0149957, time_increment: 0.000348444, scan_time: 0.145998, range_min: 0.05, range_max: 64, range size: 420, intensities size: 420
...
[INFO] [1754138926.671314160] [lesson1_laser_scan_node]: Range 17: 0.000000, angle: -2.886666, x: -0.000000, y: -0.000000
[INFO] [1754138926.671319509] [lesson1_laser_scan_node]: Range 18: 0.600000, angle: -2.871671, x: -0.578275, y: -0.159994
[INFO] [1754138926.671327579] [lesson1_laser_scan_node]: Range 19: 0.611000, angle: -2.856675, x: -0.586367, y: -0.171739
...从这些信息中我们可以看出：
- 雷达的坐标系为 laser_link
- 雷达数据的最小角度与最大角度分别为 angle_min: -3.14159 与 angle_max: 3.14159，可见，这是一个水平视角为360度的雷达
- 雷达数据的最近最远距离分别为 range_min: 0.05m 与 range_max: 64m，可见，这个雷达的盲区为 5cm
- 雷达扫描一周将返回 range size: 420 个数据点，所以分辨率是 intensities size: 420
- 坐标转换，雷达数据中的 ranges 字段中储存的只有极坐标系 下的距离值，如果我们想知道每个数据点对应欧几里得坐标，还需要将极坐标进行转换，转换的方法就是 通过索引来获取 ranges 中的值，再通过索引算出这个值对应的角度
第i个数据点的距离值为 ranges[i]
第i个数据点的角度为 angle = angle_min + angle_increment * i
所以这个点对应的x坐标为 ranges[i] * cos(angle)
所以这个点对应的y坐标为 ranges[i] * sin(angle)
ini
// 遍历 ranges ，解析扫描点的欧式坐标为
auto range_size = scan_msg->ranges.size();
for(int i = 0; i < range_size; i++) {
double range = scan_msg->ranges[i];
double angle = scan_msg->angle_min + scan_msg->angle_increment * i;
double x = range * cos(angle);
double y = range * sin(angle);
RCLCPP_INFO(this->get_logger(), "%d Range: %lf, angle: %lf, x: %lf, y: %lf", i, range, angle, x, y);
}LIO-SAM中特征提取
使用PCL进行雷达的消息类型转换
- PCL Github: github.com/PointCloudL...
- PCL tutorial: github.com/adrian-soch...
pointclouds.org/documentati...
安装 ROS2 PCL 包
bash
sudo apt install ros-$ROS_DISTRO-pcl*创建一个 ROS2 包
css
ros2 pkg create lesson2 --build-type ament_cmake --dependencies rclcpp 创建代码文件 laser_scan_node.cpp
scss
cmake_minimum_required(VERSION 3.5)
project(ros2pcl_test)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io)
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(ros2pcl_test_sub
src/subscription_pcl.cpp
)
ament_target_dependencies(ros2pcl_test_sub
rclcpp
sensor_msgs
)
target_link_libraries(ros2pcl_test_sub ${PCL_LIBRARIES})
ament_package()里程计
轮速计里程计
通过电机旋转的圈数来计算机器人所走过的距离与角度，在ROS中称为Odometry，译为里程计．
轮速计则是一种里程计，就是安装在电机上的编码器，通过编码器获取的距离与角度，轮速计的频率一般不会太高，一般为20hz-50hz．
后来，随着SLAM的发展，里程计种类多了起来，如激光雷达里程计，视觉里程计等等，这些作用与轮速计差不多，都是通过各种方式，获取雷达或者相机这段时间内移动的距离与角度．
激光雷达里程计
用 PL-ICP 算法做雷达的帧间匹配，知道了每帧雷达数据间的坐标变换，累加起来就可以做成激光雷达里程计。
ICP算法进行相邻2帧雷达数据间坐标变换的计算
迭代最近点（Iterative Closest Point, 下简称ICP）算法是一种点云匹配算法。
其求解思路为：
- 首先对于一幅点云中的每个点，在另一幅点云中计算匹配点（最近点）
- 极小化匹配点间的匹配误差，计算位姿
- 然后将计算的位姿作用于点云
- 再重新计算匹配点
- 如此迭代，直到迭代次数达到阈值，或者极小化函数的变化量小于设定阈值
ICP算法有一些不足：
- ICP对初值比较敏感，初值给的不好，就需要花费更多的迭代次数进行匹配．
- 由于它是迭代很多次的，所以其花费的时间很长，这一点是非常致命的
- 精度与速度是矛盾的，ICP算法理论上可以实现很高的精度，但是要很多很多的迭代次数，以及很长的时间．所以，当限制了迭代次数的情况下，精度就不一定能保证了．
下面是关键代码，使用 PCL 库中的 IterativeClosestPoint 封装的 icp 算法
rust
// icp算法
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_;
/*
* 调用ICP进行帧间位姿的计算
*/
void ScanMatchICP::ScanMatchWithICP(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
{
// ICP 输入数据,输出数据的设置,还可以进行参数配置,这里使用默认参宿
icp_.setInputSource(last_pointcloud_);
icp_.setInputTarget(current_pointcloud_);
// 开始迭代计算
pcl::PointCloud<pcl::PointXYZ> unused_result;
icp_.align(unused_result);
// std::cout << "has converged:" << icp_.hasConverged() << " score: " << icp_.getFitnessScore() << std::endl;
// 如果迭代没有收敛,不进行输出
if (icp_.hasConverged() == false)
{
RCLCPP_INFO_STREAM(this->get_logger(), "not Converged" << std::endl);
}
else
{
// 收敛了之后, 获取坐标变换
Eigen::Affine3f transfrom;
transfrom = icp_.getFinalTransformation();
// 将Eigen::Affine3f转换成x, y, theta, 并打印出来
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw);
RCLCPP_INFO_STREAM(this->get_logger(), "transfrom: (" << x << ", " << y << ", " << yaw * 180 / M_PI << ")" << std::endl);
}
}PL-ICP(Point to Line ICP)
PL-ICP(Point to Line ICP) 使用点到线距离最小的方式进行ICP的计算，收敛速度快很多，同时精度也更高一些．
具体的pl-icp的介绍请看其论文，作者也开源了pl-icp的代码，作者将实现pl-icp的代码命名为csm( Canonical Scan Matcher).
论文与代码的详情请看作者的网站censi.science/software/cs....
CSM 包安装
用下面的命令安装 csm 包
swift
sudo apt-get install ros-$ROS_DISTRO-csm目前 csm 只发布了 ROS1 平台的包， 热心群众开发的 ROS2 平台的 csm 包, 源码地址
源码构建：
- clone 下载源码
bash
git clone https://github.com/AlexKaravaev/csm.git-
colcon 构建源码
colcon build
在 c_cpp_properties.json 文件中增加头文件路径
json
"includePath": [
"xxx/install/csm/include/**",
"/usr/include/eigen3"
]- 配置环境
bash
source install/setup.bash帧间匹配 - scan_tools-laser_scan_matcher
ros 中有人使用使用 csm 包进行了 ros下的实现，进行了扫描匹配与位姿累加的实现，但是没有发布 odometry 的 topic 与 tf．包的名字为 laser_scan_matcher，是 scan_tools 包集中的一个．
scan_tools 这个包里提供了很多操作二维激光雷达数据的功能,其包含的功能包名字如下所示，并对其功能进行了简要介绍．
- laser_ortho_projector: 将切斜的雷达数据投影到平面上．
- laser_scan_matcher: 基于pl-icp的扫描匹配的实现，并进行了位姿累加
- laser_scan_sparsifier: 对雷达数据进行稀疏处理
- laser_scan_splitter: 将一帧雷达数据分段，并发布出去
- ncd_parser: 读取New College Dataset，转换成ros的scan 与 odometry 发布出去
- polar_scan_matcher: 基于Polar Scan Matcher的扫描匹配器的ros实现
- scan_to_cloud_converter: 将 sensor_msgs/LaserScan 数据转成 sensor_msgs/PointCloud2 的数据格式．
帧间匹配首先初始化的工作记录第一帧雷达数据，第一帧时间戳。并且计算了雷达数据对应的每个角度值的cos与sin，避免重复计算，减少计算量．
每次匹配计算前，将雷达数据转换成csm需要的格式。再调用ScanMatchWithPLICP进行匹配计算．
- 计算了雷达数据对应的每个角度值的cos与sin
ini
/**
* 雷达数据间的角度是固定的，因此可以将对应角度的cos与sin值缓存下来，不用每次都计算
*/
void ScanMatchPLICP::CreateCache(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
{
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;
}- 将雷达的数据格式转成 csm 需要的格式
ini
/**
* 将雷达的数据格式转成 csm 需要的格式
*/
void ScanMatchPLICP::LaserScanToLDP(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg, LDP &ldp)
{
unsigned int n = scan_msg->ranges.size();
// 调用csm里的函数进行申请空间
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->estimate[0] = 0.0;
ldp->estimate[1] = 0.0;
ldp->estimate[2] = 0.0;
ldp->true_pose[0] = 0.0;
ldp->true_pose[1] = 0.0;
ldp->true_pose[2] = 0.0;
}- 使用PLICP进行帧间位姿的计算
c
/**
* 使用PLICP进行帧间位姿的计算
*/
void ScanMatchPLICP::ScanMatchWithPLICP(LDP &curr_ldp_scan, const rclcpp::Time &time)
{
// CSM is used in the following way:
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the laser frame since the last scan
// The computed correction is then propagated using the tf machinery
input_.laser_ref = prev_ldp_scan_;
input_.laser_sens = curr_ldp_scan;
// 位姿的预测值为0，就是不进行预测
input_.first_guess[0] = 0;
input_.first_guess[1] = 0;
input_.first_guess[2] = 0;
// 调用csm里的函数进行plicp计算帧间的匹配，输出结果保存在output里
sm_icp(&input_, &output_);
if (output_.valid)
{
std::cout << "transfrom: (" << output_.x[0] << ", " << output_.x[1] << ", "
<< output_.x[2] * 180 / M_PI << ")" << std::endl;
}
else
{
std::cout << "not Converged" << std::endl;
}
// 删除prev_ldp_scan_，用curr_ldp_scan进行替代
ld_free(prev_ldp_scan_);
prev_ldp_scan_ = curr_ldp_scan;
last_icp_time_ = time;
}激光雷达里程计计算
计算里程计之前需要先解释一下 ROS 中的 TF 坐标系转换，ROS 中有几个十分常用的坐标系，其简介如下：
- map: 地图坐标系，也被称为世界坐标系，是静止不动的
- odom: 里程计坐标系，相对于map来说一般情况下是静止的，有些情况下会变动（定位节点为了修正机器人的位姿从而改变了map->odom间的坐标变换）
- base_link: 代表机器人的旋转中心的坐标系，相对于odom来说base_link是运动的
- laser_link: 激光雷达的坐标系，相对于base_link来说是静止的，因为雷达装在机器人上，雷达不会自己飞起来
可以看到，上边的坐标系，是单方向依赖的， laser_link 依赖于 base_link，base_link 依赖于 odom 。也可以说成是一个坐标系指向下一个坐标系的，连起来的话也成了：
map -> odom -> base_link -> laser_link
在 ROS 中，将这种能够连接起来的坐标系称为 TF树，是一个由坐标系组成的树。可以通过 rqt 软件来可视化TF树，也可以通过 Rviz 的 tf 显示模块来可视化TF树。
ROS 中使用 TF 作为坐标系管理的库，后来，由于种种原因，TF库被弃用了。ROS 改用 TF2 作为新的管理坐标系的库。
-
tf2的wiki主页为：wiki.ros.org/tf2
-
tf2的官方教程为：wiki.ros.org/tf2/Tutori
所以在初始化阶段，使用 TF2 相关接口来获取机器人坐标系与雷达坐标系间的坐标变换。
- 获取机器人坐标系与雷达坐标系间的坐标变换
rust
/**
* 获取机器人坐标系与雷达坐标系间的坐标变换
*/
bool ScanMatchPLICP::GetBaseToLaserTf(const std::string &frame_id)
{
rclcpp::Time t = get_clock()->now();
geometry_msgs::msg::TransformStamped transformStamped;
// 获取tf并不是瞬间就能获取到的，要给1秒的缓冲时间让其找到tf
try
{
transformStamped = tfBuffer_.lookupTransform(base_frame_, frame_id,
t, rclcpp::Duration(1, 0));
}
catch (tf2::TransformException &ex)
{
RCLCPP_INFO(this->get_logger(), "%s", ex.what());
rclcpp::sleep_for(std::chrono::milliseconds(1000));
return false;
}
// 将获取的tf存到base_to_laser_中
tf2::fromMsg(transformStamped.transform, base_to_laser_);
laser_to_base_ = base_to_laser_.inverse();
return true;
}在帧间匹配的基础上， 增加了 基于匀速模型的位姿预测，位姿累加，发布TF以及odom话题，新建关键帧，从而完成里程计的计算和发布。
- 基于匀速模型的位姿预测
ini
/**
* 推测从上次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;
}- 发布TF以及odom话题
ini
/**
* 发布tf与odom话题
*/
void ScanMatchPLICP::PublishTFAndOdometry()
{
geometry_msgs::msg::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::msg::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 话题
odomPub->publish(odom_msg);
}- 新建关键帧
ini
/**
* 如果平移大于阈值，角度大于阈值，则创新新的关键帧
* @return 需要创建关键帧返回true, 否则返回false
*/
bool ScanMatchPLICP::NewKeyframeNeeded(const tf2::Transform &d)
{
scan_count_++;
if (fabs(tf2::getYaw(d.getRotation())) > kf_dist_angular_)
return true;
if (scan_count_ == kf_scan_count_)
{
scan_count_ = 0;
return true;
}
double x = d.getOrigin().getX();
double y = d.getOrigin().getY();
if (x * x + y * y > kf_dist_linear_sq_)
return true;
return false;
}- 里程计计算过程
ini
/**
* 使用PLICP进行帧间位姿的计算
*/
void ScanMatchPLICP::ScanMatchWithPLICP(LDP &curr_ldp_scan, const rclcpp::Time &time)
{
// CSM is used in the following way:
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the laser frame since the last scan
// The computed correction is then propagated using the tf machinery
prev_ldp_scan_->odometry[0] = 0.0;
prev_ldp_scan_->odometry[1] = 0.0;
prev_ldp_scan_->odometry[2] = 0.0;
prev_ldp_scan_->estimate[0] = 0.0;
prev_ldp_scan_->estimate[1] = 0.0;
prev_ldp_scan_->estimate[2] = 0.0;
prev_ldp_scan_->true_pose[0] = 0.0;
prev_ldp_scan_->true_pose[1] = 0.0;
prev_ldp_scan_->true_pose[2] = 0.0;
input_.laser_ref = prev_ldp_scan_;
input_.laser_sens = curr_ldp_scan;
// 匀速模型，速度乘以时间，得到预测的odom坐标系下的位姿变换
double dt = (time - last_icp_time_).seconds();
double pr_ch_x, pr_ch_y, pr_ch_a;
GetPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
tf2::Transform prediction_change;
CreateTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, prediction_change);
// account for the change since the last kf, in the fixed frame
// 将odom坐标系下的坐标变换 转换成 base_in_odom_keyframe_坐标系下的坐标变换
prediction_change = prediction_change * (base_in_odom_ * base_in_odom_keyframe_.inverse());
// the predicted change of the laser's position, in the laser frame
// 将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_;
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());
// If they are non-Null, free covariance gsl matrices to avoid leaking memory
if (output_.cov_x_m)
{
gsl_matrix_free(output_.cov_x_m);
output_.cov_x_m = 0;
}
if (output_.dx_dy1_m)
{
gsl_matrix_free(output_.dx_dy1_m);
output_.dx_dy1_m = 0;
}
if (output_.dx_dy2_m)
{
gsl_matrix_free(output_.dx_dy2_m);
output_.dx_dy2_m = 0;
}
start_time_ = std::chrono::steady_clock::now();
// 调用csm进行plicp计算
sm_icp(&input_, &output_);
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
// std::cout << "PLICP计算用时: " << time_used_.count() << " 秒。" << std::endl;
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;
latest_velocity_.linear.x = corr_ch.getOrigin().getX() / dt;
latest_velocity_.angular.z = tf2::getYaw(corr_ch.getRotation()) / dt;
}
else
{
RCLCPP_INFO(this->get_logger(), "not Converged");
}
// 发布tf与odom话题
PublishTFAndOdometry();
// 检查是否需要更新关键帧坐标
if (NewKeyframeNeeded(corr_ch))
{
// 更新关键帧坐标
ld_free(prev_ldp_scan_);
prev_ldp_scan_ = curr_ldp_scan;
base_in_odom_keyframe_ = base_in_odom_;
}
else
{
ld_free(curr_ldp_scan);
}
last_icp_time_ = time;
}待补充计算过程解释
栅格地图
地图数据格式
栅格地图就是用一个个栅格组成的网格来代表地图. 栅格里可以存储不同的数值, 代表这个栅格的不同含义.
ROS的栅格地图使用白色代表空闲，也就是可通过区域，其存储的值为 0；
黑色代表占用，也就是不可通过区域，其存储的值为 100；
灰色代表未知，就是说目前还不清楚这个栅格是否可以通过，其存储的值为 -1．
栅格地图由于其 占用与空闲的表示方法，在ROS中又被称为占用地图．
接下来看一下ROS中的栅格地图的消息类型，
bash
ros2 interface package nav_msgs
nav_msgs/srv/GetMap
nav_msgs/srv/GetPlan
nav_msgs/msg/MapMetaData
nav_msgs/srv/LoadMap
nav_msgs/msg/Odometry
nav_msgs/msg/Goals
nav_msgs/msg/OccupancyGrid
nav_msgs/srv/SetMap
nav_msgs/msg/GridCells
nav_msgs/msg/Path
bash
ros2 interface show nav_msgs/msg/OccupancyGrid
# This represents a 2-D grid map
std_msgs/Header header # 数据的消息头
builtin_interfaces/Time stamp # 数据的时间戳
int32 sec
uint32 nanosec
string frame_id # 地图的坐标系
# MetaData for the map
MapMetaData info # 地图的一些信息
builtin_interfaces/Time map_load_time # 加载地图的时间
int32 sec
uint32 nanosec
float32 resolution # 地图的分辨率，一个格子代表着多少米，一般为0.05，[m/cell]
uint32 width # 地图的宽度，像素的个数, [cells]
uint32 height # 地图的高度，像素的个数, [cells]
geometry_msgs/Pose origin # 地图左下角的格子对应的物理世界的坐标，[m, m, rad]
Point position
float64 x
float64 y
float64 z
Quaternion orientation
float64 x 0
float64 y 0
float64 z 0
float64 w 1
# The map data, in row-major order, starting with (0,0).
# Cell (1, 0) will be listed second, representing the next cell in the x direction.
# Cell (0, 1) will be at the index equal to info.width, followed by (1, 1).
# The values inside are application dependent, but frequently,
# 0 represents unoccupied, 1 represents definitely occupied, and
# -1 represents unknown.
int8[] data # 地图数据，优先累加行，从（0,0）开始。占用值的范围为[0,100]，未知为-1。可以看到，消息可以分为3个部分，消息头header，地图信息info，地图数据data．
地图信息info储存了地图相关的信息，包括加载地图的时间，地图的分辨率，地图的宽度与高度，以及地图左下角栅格对应的物理坐标．
地图本身是只有像素坐标的，其像素坐标系为左下角为(0, 0) 的坐标系．通过左下角栅格对应的物理坐标 origin 以及 分辨率，再通过 像素 * 分辨率 + origin , 将像素坐标转成物理世界的坐标，从而确定了整个地图的物理坐标．
地图数据data是一维的，我们在赋值之前要首先对这个一维数组进行初始化，数据的大小就是所有像素的个数．
遍历的时候要注意方向，这个数据是以行为主要递增方向的．也就是说遍历的时候要先遍历第一行的所有数据，然后再遍历第二行的所有数据．
- 初始化一个地图实例 nav_msgs::msg::OccupancyGrid map_;
ini
// 对map_进行初始化
map_.header.frame_id = "map";
// 地图的分辨率为0.05m,代表一个格子的距离是0.05m
map_.info.resolution = 0.05;
// 地图图片像素的大小, width为地图的宽度是多少个像素
map_.info.width = 30;
map_.info.height = 30;
// 如果要表示地图图片为多少米的话,就需要用实际长度除以分辨率,得到像素值
// map_.info.width = 100 / map_.info.resolution;
// map_.info.height = 100 / map_.info.resolution;
// 地图左下角的点对应的物理坐标
map_.info.origin.position.x = 0.0;
map_.info.origin.position.y = 0.0;
// 对数组进行初始化, 数组的大小为实际像素的个数
map_.data.resize(map_.info.width * map_.info.height);- 构造地图数据，并发送代码 /map topic
ini
// 构造map并进行发布
void OccupancyGrid::PublishMap()
{
start_time_ = std::chrono::steady_clock::now();
// 通过二维索引算出来的一维索引
uint32_t index = 0;
// 10种情况
uint32_t count = 10;
// 固定列, 优先对行进行遍历
for (uint32_t j = 0; j < map_.info.height; j++)
{
for (uint32_t i = 0; i < map_.info.width; i++)
{
// 二维坐标转成一维坐标
index = i + j * map_.info.width;
// std::cout << " index: " << index ;
// 0代表空闲, 100代表占用, -1代表未知, 默认值为0
// 为map赋予不同的值来体验效果, 从-1 到 254
if (index % count == 0)
map_.data[index] = -1;
else if (index % count == 1)
map_.data[index] = 0;
else if (index % count == 2)
map_.data[index] = 30;
else if (index % count == 3)
map_.data[index] = 60;
else if (index % count == 4)
map_.data[index] = 100;
else if (index % count == 5)
// map_.data[index] = 140;
map_.data[index] = 100;
else if (index % count == 6)
// map_.data[index] = 180;
map_.data[index] = 100;
else if (index % count == 7)
// map_.data[index] = 220;
map_.data[index] = 100;
else if (index % count == 8)
// map_.data[index] = 240;
map_.data[index] = 100;
else if (index % count == 9)
// map_.data[index] = 254;
map_.data[index] = 100;
}
}
// 设置这一帧地图数据的时间戳
map_.header.stamp = get_clock()->now();
// 发布map和map_metadata话题
mapPub->publish(map_);
mapPubMetadata->publish(map_.info);
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
std::cout << "发布一次地图用时: " << time_used_.count() << " 秒。\n" << std::endl;
}- rviz 查看
GMapping SLAM
GMapping 是 ROS 中 navigation 导航包集中推荐的二维建图算法包，是基于2D激光雷达使用RBPF（Rao-Blackwellized Particle Filters）粒子滤波算法实现的二维栅格地图构建。每个粒子都携带一张地图，将定位和建图过程分离，先进通过里程计数据获取粒子群的先验位姿，再通过雷达数据与地图的匹配程度对所有粒子进行打分，通过分数高的粒子群来近似机器人的真实位姿，再进行建图。
随着场景增大所需的粒子增加，因为每个粒子都携带一幅地图，因此在构建大地图时所需内存和计算量都会增加。因此不适合构建大场景地图。并且没有回环检测，因此在回环闭合时可能会造成地图错位，虽然增加粒子数目可以使地图闭合但是以增加计算量和内存为代价。
Gmapping可以实时构建室内地图，在构建小场景地图所需的计算量较小且精度较高。对激光雷达扫描频率要求较低、鲁棒性高
GMapping的具体实现是在open_gmapping包里，ROS 中的封装包slam_gmapping．
- gmapping在ROS中的wiki地址为 wiki.ros.org/gmapping
- open_gmapping Github
- slam_gmapping Github
安装 ROS2 package
ruby
$ sudo apt-get install ros-${ROS_DISTRO}-gmapping
GMapping 建图
使用 GMapping 中的地图数据格式，以及 GMapping 中的地图计算方式，我们可以实现了将一帧激光雷达数据构建成栅格地图。代码分可为2个部分：
- 第一部分为计算出地图的储存空间，并且计算出从雷达到激光点这条线在gmapping栅格地图中的坐标，以及雷达点的坐标．
ini
// lp为地图坐标系下的激光雷达坐标系的位姿
OrientedPoint lp(0, 0, 0.0);
// 将位姿lp转换成地图坐标系下的位置
IntPoint p0 = map.world2map(lp);
// 地图的有效区域(地图坐标系)
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
// 通过激光雷达的数据，找出地图的有效区域
for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
{
// 排除错误的激光点
double d = scan_msg->ranges[i];
if (d > max_range_ || d == 0.0 || !std::isfinite(d))
continue;
if (d > max_use_range_)
d = max_use_range_;
// p1为激光雷达的数据点在地图坐标系下的坐标
Point phit = lp;
phit.x += d * a_cos_[i];
phit.y += d * a_sin_[i];
IntPoint p1 = map.world2map(phit);
// 使用bresenham算法来计算 从激光位置到激光点 要经过的栅格的坐标
GridLineTraversalLine line;
GridLineTraversal::gridLine(p0, p1, &line);
// 将line保存起来以备后用
line_lists_.push_back(line);
// 计算活动区域的大小
for (int i = 0; i < line.num_points - 1; i++)
{
activeArea.insert(map.storage().patchIndexes(line.points[i]));
}
// 如果d<m_usableRange则需要把击中点也算进去 说明这个值是好的。
// 同时如果d==max_use_range_ 那么说明这个值只用来进行标记空闲区域 不用来进行标记障碍物
if (d < max_use_range_)
{
IntPoint cp = map.storage().patchIndexes(p1);
activeArea.insert(cp);
hit_lists_.push_back(phit);
}
}- 第二部分为将计算好的直线与点在gmapping地图中进行栅格值的更新．
arduino
// 在map上更新空闲点
for (auto line : line_lists_)
{
// 更新空闲位置
for (int k = 0; k < line.num_points - 1; k++)
{
// 未击中，就不记录击中的位置了，所以传入参数Point(0,0)
map.cell(line.points[k]).update(false, Point(0, 0));
}
}
// 在map上添加hit点
for (auto hit : hit_lists_)
{
IntPoint p1 = map.world2map(hit);
map.cell(p1).update(true, hit);
}
假设第一帧激光雷达数据为原点[0，0，0]，通过 PLICP 得到第二帧和第一帧相对偏移量[x1,y1,theta1],那么第二点位置坐标为[x1,y1,theta1],第三帧相对第二帧偏移[x2,y2,theta2],那么第三点坐标为[x1+x2,y1+y2,theta1+theta2],......，以此类推求得所有经过的点坐标轨迹，又有点对应的激光雷达扫描数据也是知道的，就使用 GMapping 的地图计算方式,生成该点的局部栅格地图。把所有的轨迹点的局部地图叠加在一起，就是建图的基本原理了。但这样累计误差会越来越大。虽然两帧之间匹配是很准确的，但是还是会有计算误差，积累起来到最后误差是可观的，所以 GMapping SLMA 使用粒子滤波算法
Hector SLAM
- wiki hector_mapping
- 官方文档 hector_slam
- 论文：Kohlbrecher S , Stryk O V , Meyer J , et al. A flexible and scalable SLAM system with full 3D motion estimation[C]// IEEE International Symposium on Safety. IEEE, 2011.
- github上有人对hector的源码进行了注释，其地址为 github.com/tu-darmstad...
- ROS2 hector_slam
Hector是2011年开源的二维激光SLAM的项目，非常创新地使用scan-to-map的匹配方式．
Hector SLAM是一种只需要激光雷达数据而不需要里程计数据的情况下，鲁棒性较好的2D SLAM方法和使用惯性传感系统的导航技术。Hector整体算法很直接，就是将激光点与已有的地图"对齐"，即扫描匹配。
Hector 利用高斯牛顿方法解决 scan-matching 问题，因此两次激光雷达采集数据时位姿变化不能太大，否则余项误差过大，造成建图失败------这也是为什么在hector论文中需要使用到扫描频率高达40Hz的激光雷达，如果使用10Hz的激光雷达，很容易建图失败（尤其在转向速度较大的时候）。因为它仅用到激光雷达信息，这样建图与定位的依据就不如多传感器结合的效果好。但Hector适合手持移动或者本身就没有里程计的机器人使用。
- 在大地图，低特征（distinctive landmarks）场景中，hector的建图误差高于gmapping，特别是在长廊问题中，误差更加明显。
- 优点：不需要使用里程计，所以使得空中无人机及地面小车在不平坦区域建图存在运用的可行性；使用多分辨率地图能避免局部最小值；3D空间的导航加入惯性测量系统（IMU），利用EKF滤波进行状态估计；
- 缺点：需要激光雷达的更新频率较高且测量噪声小，所以在构建地图过程中需要robot速度控制在比较低的情况下，建图效果才会比较理想，这也是它没有回环检测模块的一个后遗症；且在里程计数据比较精确的时候，无法有效利用里程计信息。
Hector 定位
假设一共3层多分辨率地图，首先使用 setFrom() 根据地图层数对原始激光数据坐标进行缩小，得到对应图层尺寸的激光数据，对每个原始数据都乘以 1/(2的n次方) , n 是 2, 1, 0．
php
/**
* 地图匹配，通过多分辨率地图求解当前激光帧的pose。
* @param beginEstimateWorld
* @param dataContainer
* @param covMatrix
* @return
*/
virtual Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld, const DataContainer &dataContainer, Eigen::Matrix3f &covMatrix)
{
size_t size = mapContainer.size();
Eigen::Vector3f tmp(beginEstimateWorld);
/// coarse to fine 的pose求精过程，i层的求解结果作为i-1层的求解初始值。
for (int index = size - 1; index >= 0; --index)
{
//std::cout << " m " << i;
if (index == 0)
{
tmp = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5)); /// 输入数据dataContainer 对应 mapContainer[0]
}
else
{
// 根据地图层数对原始激光数据坐标进行缩小，得到对应图层尺寸的激光数据
dataContainers[index - 1].setFrom(dataContainer, static_cast<float>(1.0 / pow(2.0, static_cast<double>(index))));
tmp = (mapContainer[index].matchData(tmp, dataContainers[index - 1], covMatrix, 3));
/// dataContainers[i]对应mapContainer[i+1]
}
}
return tmp;
}MapProcContainer::matchData 使用缩小后的雷达数据，与对应分辨率的地图进行扫描匹配，也就是在粗分辨率地图上先进行扫描匹配，得出的结果再传入更精细分辨率地图上进行扫描匹配，最终返回一个在 map 坐标系下的位姿.
php
/**
* 给定位姿初值，估计当前scan在当前图层中的位姿 ---- 位姿为世界系下的pose 、 dataContainer应与当前图层尺度匹配
* @param beginEstimateWorld 世界系下的位姿
* @param dataContainer 激光数据
* @param covMatrix scan-match的不确定性 -- 协方差矩阵
* @param maxIterations 最大的迭代次数
* @return
*/
Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld, const DataContainer &dataContainer, Eigen::Matrix3f &covMatrix, int maxIterations)
{
return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, maxIterations);
}ScanMatcher::matchData 实际进行位姿态估计的函数，也就是 scan to map 实际逻辑算法部分。调用了 estimateTransformationLogLh() 进行计算位姿，并且通过多次调用这个函数进行多次迭代求解，使结果更加准确。
estimateTransformationLogLh() 通过 getCompleteHessianDerivs() 计算出了协方差矩阵H , 以及 dTr向量。再通过高斯牛顿法最终推导出的求根公式： H的逆 乘以 dtr 求出 先验位姿与后验位姿间的增量. 也就是 X = H.inverse() * dTr --> H * X = dTr
最后再将这个增量加上之前的预测值，得到匹配之后的位姿．
php
/**
* 实际进行位姿估计的函数
* @param beginEstimateWorld 位姿初值
* @param gridMapUtil 网格地图工具,这里主要是用来做坐标变换
* @param dataContainer 激光数据
* @param covMatrix 协方差矩阵
* @param maxIterations 最大迭代次数
* @return
*/
Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld,
ConcreteOccGridMapUtil &gridMapUtil,
const DataContainer &dataContainer,
Eigen::Matrix3f &covMatrix, int maxIterations)
/**
* 高斯牛顿估计位姿
* @param estimate 位姿初始值
* @param gridMapUtil 网格地图相关计算工具
* @param dataPoints 激光数据
* @return 提示是否有解 －－－ 貌似没用上
*/
bool estimateTransformationLogLh(Eigen::Vector3f &estimate,
ConcreteOccGridMapUtil &gridMapUtil,
const DataContainer &dataPoints) OccGridMapUtil::getCompleteHessianDerivs 核心实现计算出了协方差矩阵H , 以及 dTr向量。
php
/**
* 使用当前pose投影dataPoints到地图，计算出 H 矩阵 b列向量， 理论部分详见Hector论文： 《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》.
* @param pose 地图系上的位姿
* @param dataPoints 已转换为地图尺度的激光点数据
* @param H 需要计算的 H矩阵
* @param dTr 需要计算的 g列向量
*/
void getCompleteHessianDerivs(const Eigen::Vector3f &pose,
const DataContainer &dataPoints,
Eigen::Matrix3f &H,
Eigen::Vector3f &dTr)Hector 建图
使用 Hector 中的地图数据格式，以及 Hector 中的地图计算方式，我们可以实现了将一帧激光雷达数据构建成栅格地图。代码分可为2个部分：
- 第一部分为计算出地图的储存空间，将雷达的数据类型转成hector需要的格式，将雷达的数据类型写成hector的地图，并且计算出从雷达到激光点这条线在栅格地图中的坐标，以及雷达点的坐标．将起始点坐标设置成了(800, 800), 这样就使得生成的地图始终是固定的了。进行雷达数据的遍历，求得每个雷达点对应的栅格地图的坐标，并且作为终点，在起点与终点间进行，使用 bresemham 画线，计算出从雷达到激光点这条线在栅格地图中的坐标，并写入 hector 地图。
ini
// 将ROS中的雷达数据格式转成hector中定义的格式
void HectorMapping::ROSLaserScanToDataContainer(const sensor_msgs::msg::LaserScan::SharedPtr scan,
hectorslam::DataContainer &dataContainer,
float resolution)
{
size_t size = scan->ranges.size();
float angle = scan->angle_min;
dataContainer.clear();
dataContainer.setOrigo(Eigen::Vector2f::Zero());
float maxRangeForContainer = scan->range_max - 0.1f;
for (size_t i = 0; i < size; ++i)
{
float dist = scan->ranges[i];
if ((dist > scan->range_min) && (dist < maxRangeForContainer))
{
// dist *= resolution; ///! 将实际物理尺度转换到地图尺度）
dataContainer.add(Eigen::Vector2f(cos(angle) * dist, sin(angle) * dist));
// std::cout << "x: " << cos(angle) * dist << " y: " << sin(angle) * dist << std::endl;
}
angle += scan->angle_increment;
}
}
// 将雷达数据写成hector的地图
void HectorMapping::ComputeMap()
{
Eigen::Vector3f robotPoseWorld(0, 0, 0);
hector_map_mutex_.lock();
hector_map_->updateByScanJustOnce(laserScan_container_, robotPoseWorld);
hector_map_mutex_.unlock();
}- 第二部分将 hector 地图转换成ros中的地图格式并发布出去
ini
// 将hector的地图转成ROS格式的地图并发布出去
void HectorMapping::PublishMap()
{
int sizeX = hector_map_->getSizeX();
int sizeY = hector_map_->getSizeY();
int size = sizeX * sizeY;
std::vector<int8_t> &data = map_.data;
//std::vector contents are guaranteed to be contiguous, use memset to set all to unknown to save time in loop
memset(&data[0], -1, sizeof(int8_t) * size);
ros_map_mutex_.lock();
for (int i = 0; i < size; ++i)
{
if (hector_map_->isFree(i))
{
data[i] = 0;
}
else if (hector_map_->isOccupied(i))
{
data[i] = 100;
}
}
ros_map_mutex_.unlock();
// 添加当前的时间戳
map_.header.stamp = this->now();
map_.header.frame_id = map_frame_;
// 发布map和map_metadata
map_publisher_->publish(map_);
map_publisher_metadata_->publish(map_.info);
}
Hector 示例
雷达畸变矫正
目前的单线激光雷达只有一个单点激光发射器，通过旋转反射镜或者旋转激光发射器一周，实现了360度范围的扫描．
而由于这一周的激光点数据不是同一时间得到的，就导致了雷达发生运动时，在一次一周的扫描周期内，雷达本身的位姿会变化，那么以扫描的起始位姿，处理一周激光点数据，则会有运动畸变。
从上面畸变的原因可知，只需要找到每个激光点对应时刻的激光雷达坐标系，相对于发射第一个点时刻的激光雷达坐标系，间的坐标变换，就可以将每个激光点都变换到发射第一个点时刻的激光雷达坐标系下，就完成了畸变的校正．
畸变校正的过程：
- 首先，假设雷达发射第一个点的时刻为 time_start，这时的雷达坐标系为 frame_start．
- 雷达其余点的时刻为 time_point，这时的雷达坐标系为 frame_point，其余点在 frame_point 坐标系下的坐标为 point．
- 只需要找到 frame_start 与 frame_point 间的坐标变换，就可以将 其余点的坐标 point 通过坐标变换 变换到 frame_start 坐标系下．
- 对所有点都进行上述操作后，得到的点的坐标，就是去畸变后的坐标了．
IMU 惯性测量单元
IMU全称Inertial Measurement Unit，惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器。
IMU一般包括3个功能，
- 3轴加速度计：测量 x y z 3个方向上的加速度，z轴的加速度也就是重力值的大小(IMU水平的情况下)
- 3轴陀螺仪(角度度计)：测量出IMU分别绕着 x y z 轴旋转的角速度
- 3轴磁力计：测出地球磁场的方向，用于确定IMU 在 x y z 轴的方向，但是受磁铁，金属等影响较大．
前两个功能的组合被称为6轴IMU，这3个功能的组合被称为9轴IMU．IMU的频率可以很高，10hz-400hz 不等．
IMU 计算与雷达数据时间同步的角度变化
由于是使用双端队列进行IMU数据的存储，所以，队列中IMU的时间是连续的．
同时由于IMU是100hz的，而雷达是10hz的，二者频率不同，时间戳也就对不上，所以需要做一下时间同步．
时间同步的思想就是，先将imu的双端队列中时间太早的数据删去．
之后找到IMU的时间与当前帧雷达的起始时间早的第一个IMU数据．
以这个IMU数据作为起点，使用之后的IMU数据进行积分，分别获得2个IMU数据之间的旋转，imu_rot_x_ 保存的是当前时刻相对于第一帧imu数据时刻，转动的角度值．
重复这个操作，直到IMU的时间戳比当前帧雷达数据旋转一周之后的时间要晚．也就是这些IMU数据的时间是包含了雷达数据转一圈的时间．
ini
// 根据点云中某点的时间戳赋予其 通过插值 得到的旋转量
void LidarUndistortion::ComputeRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
*rotXCur = 0;
*rotYCur = 0;
*rotZCur = 0;
// 找到在 pointTime 之后的imu数据
int imuPointerFront = 0;
while (imuPointerFront < current_imu_index_)
{
if (pointTime < imu_time_[imuPointerFront])
break;
++imuPointerFront;
}
// 如果上边的循环没进去或者到了最大执行次数，则只能近似的将当前的旋转 赋值过去
if (pointTime > imu_time_[imuPointerFront] || imuPointerFront == 0)
{
*rotXCur = imu_rot_x_[imuPointerFront];
*rotYCur = imu_rot_y_[imuPointerFront];
*rotZCur = imu_rot_z_[imuPointerFront];
}
else
{
int imuPointerBack = imuPointerFront - 1;
// 根据线性插值计算 pointTime 时刻的旋转
double ratioFront = (pointTime - imu_time_[imuPointerBack]) / (imu_time_[imuPointerFront] - imu_time_[imuPointerBack]);
double ratioBack = (imu_time_[imuPointerFront] - pointTime) / (imu_time_[imuPointerFront] - imu_time_[imuPointerBack]);
*rotXCur = imu_rot_x_[imuPointerFront] * ratioFront + imu_rot_x_[imuPointerBack] * ratioBack;
*rotYCur = imu_rot_y_[imuPointerFront] * ratioFront + imu_rot_y_[imuPointerBack] * ratioBack;
*rotZCur = imu_rot_z_[imuPointerFront] * ratioFront + imu_rot_z_[imuPointerBack] * ratioBack;
}
}里程计计算与雷达数据时间同步的平移量
前面有介绍到里程计的作用和种类，这里使用电机上的编码器提供的轮速计里程计。
轮速计的时间同步方式与IMU的基本一致，只是由于轮速计是固定坐标系下的位姿变换，所以不需要由我们自己对每一帧数据进行积分．
直接找到雷达数据开始前的一帧odom数据，与雷达旋转一圈之后的一帧odom数据，求这两个odom数据之间的坐标变换，即可得到雷达旋转一圈时间附近的总平移量．
雷达数据的畸变产生主要是由于旋转产生的，由平移产生的畸变比较少．而且轮速计的频率一般不会很高．
所以这里就直接用雷达数据前后的轮速计的坐标差当做这段的平移量了，没有对雷达旋转时间范围内的每个值进行处理．
ini
// 根据点云中某点的时间戳赋予其 通过插值 得到的平移量
void LidarUndistortion::ComputePosition(double pointTime, float *posXCur, float *posYCur, float *posZCur)
{
*posXCur = 0;
*posYCur = 0;
*posZCur = 0;
// 根据线性插值计算 pointTime 时刻的平移量
double ratioFront = (pointTime - start_odom_time_) / (end_odom_time_ - start_odom_time_);
*posXCur = odom_incre_x_ * ratioFront;
*posYCur = odom_incre_y_ * ratioFront;
*posZCur = odom_incre_z_ * ratioFront;
}