2D 激光 SLAM-Cartographer 实战

源码

https://github.com/cartographer-project/cartographer

https://github.com/cartographer-project/cartographer_ros

课 程 下 的 注 释 版 代 码 ：

https://github.com/xiangli0608/cartographer_detailed_comments_ws

备用地址：

https://gitee.com/LauZanMo/cartographer

https://gitee.com/xiaojake/cartographer_ros

论文地址：https://ieeexplore.ieee.org/document/7487258

cartographer 整体可以分为三个部分：

一是 input sensor data，也就是读取单线激光雷达的 scan、轮速计的输入以及 IMU，

二是 Local SLAM 部分，也被称为前端。 该部分的主要任务是建立维护子地图（Submaps）,

但 问 题 是 该 部 分 建 图 误 差 会 随 着 时 间 累 积 。 该 部 分 相 关 的 参 数 定 义 在

/src/cartographer/configuration_files/trajectory_builder_2d.lua

和

/src/cartographer/configuration_files/trajectory_builder_3d.lua 中。

三是 Global SLAM 部分，也称为后端. 该部分的主要任务是进行 Loop Closure。闭环检测本

质上也是一个优化问题，文中将其表达成了 pixel-accurate match 的形式，采用 Branch-and

Bound Approach （BBA）的方法来解决。具体使用 Branch-and-Bound 方法，可以参见论

文

Real-Time Loop Closure in 2D LIDAR

SLAM(https://ieeexplore.ieee.org/document/7487258)。如果是 3D 情况下，该部分还负责根

据 IMU 数据找出重力的方向。

可以看出，从传感器出发，Laser 的数据经过两个滤波器后进行 Scan Matching，用来构建子

图 submap, 而新的 Laser Scan 进来后也会插入到已经维护着的子图的适当位置。如何决定

插入的最优位姿，就是通过 Ceres Scan Matching 来实现的。估计出来的最优位姿也会与里

程计和 IMU 数据融合，用来估计下一时刻的位姿。

总体而言，Local Slam 部分负责生成较好的子图，而 Global Slam 部分进行全局优化，将不

同的子图以最匹配的位姿连接在一起。

Cartographer 安装编译与 demo 运行

#下载 https://github.com/abseil/abseil-cpp

cd abseil-cpp

mkdir build && cd build

cmake ..

make

sudo make install

# 安装 abseil 的动态库：可以将之前的 build 文件夹删掉，重新建立 build 文

件夹

mkdir build && cd build

cmake .. -DBUILD_SHARED_LIBS=ON

make

sudo make install

#安装其他依赖

sudo apt-get update

sudo apt-get install -y python-wstool python-rosdep ninja-build

sudo apt-get install cmake

sudo apt-get install build-essential

sudo apt-get install libgoogle-glog-dev

sudo apt-get install libatlas-base-dev

# 创建 cartographer_ws 工作空间，并将以下三个链接都拷到 src 文件夹里：

# https://github.com/cartographer-project/cartographer

# https://github.com/cartographer-project/cartographer_ros

# https://github.com/ceres-solver/ceres-solver

#编译 ceres

cd ceres-solver

mkdir ceres-bin

cd ceres-bin

cmake ..

make

make test

sudo make install

#安装 protobuf3 https://github.com/protocolbuffers/protobuf/releases

cd protobuf

mkdir build

cd build

cmake -G Ninja \

-DCMAKE_POSITION_INDEPENDENT_CODE=ON \

cvlife.net -DCMAKE_BUILD_TYPE=Release \

-Dprotobuf_BUILD_TESTS=OFF \

../cmake

ninja

sudo ninja install

#构建 cartographer 工作空间

mkdir -p cartographer_ws/src

cd cartographer_ws/src

catkin_init_workspace

#cartographer 编译

cd cartographer_ws

rosdep update

rosdep install --from-paths src --ignore-src --

rosdistro=${ROS_DISTRO} -y

#Build and install.这一步会花费很长的时间，耐心等待

catkin_make_isolated --install --use-ninja

source install_isolated/setup.bash

#运行测试

wget -P ~/Downloads https://storage.googleapis.com/cartographer

public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag

roslaunch cartographer_ros demo_backpack_2d.launch

bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

Cartographer 保存轨迹

#保存轨迹

#思路：在使用 cartographer 建图的过程中，找到 cartographer 提供的

“/trajectory_query” 服务（可以用“rosrun rqt_service_caller

rqt_service_caller”命令），请求后会得到当前轨迹每一帧的位姿。在源码

中找到这部分内容，将其保存为 txt 文件即可。

#方法：源码 node.cc 中，修改 HandleTrajectoryQuery 函数：

bool Node::HandleTrajectoryQuery(

::cartographer_ros_msgs::TrajectoryQuery::Request& request,

::cartographer_ros_msgs::TrajectoryQuery::Response& response) {

cvlife.net absl::MutexLock lock(&mutex_);

response.status = TrajectoryStateToStatus(

request.trajectory_id,

{TrajectoryState::ACTIVE, TrajectoryState::FINISHED,

TrajectoryState::FROZEN} /* valid states */);

if (response.status.code != cartographer_ros_msgs::StatusCode::OK)

{

LOG(ERROR) << "Can't query trajectory from pose graph: "

<< response.status.message;

return true;

}

map_builder_bridge_.HandleTrajectoryQuery(request, response);

// 保存轨迹为 txt 文件

std::cout << "receiving" << std::endl;

std::string txt_filename = "/home/xxx/xxx.txt";

std::ofstream outf(txt_filename, std::ios::app);

for (int i = 0; i < response.trajectory.size(); i++){

int32_t sec = response.trajectory[i].header.stamp.sec;

int32_t nsec = response.trajectory[i].header.stamp.nsec;

double x = response.trajectory[i].pose.position.x;

double y = response.trajectory[i].pose.position.y;

double z = response.trajectory[i].pose.position.z;

outf << std::setprecision(20) << sec << "." << nsec << " " << x

<< " " << y << " " << z << std::endl;

}

return true;

}

rosrun rqt_service_caller rqt_service_caller

#选择/trajectory query 即可

#评估

evo_ape tum gt.txt result.txt -vap

#通常使用绝对轨迹误差的 RMSE 来衡量算法的定位精度

#保存 pgm 珊格地图

rosrun map_server map_saver

传感器实测方法及常见问题

参考 https://blog.csdn.net/weixin_44314245/article/details/110881565

https://blog.csdn.net/Darcy_MFFL/article/details/112433149

https://blog.csdn.net/weixin_42576673/article/details/107280930

Cartographer 工程化建议

rplidar 适配 cartographer 的方式：

1.修改 cartgrapher_ros/cartprapher_files 下的 revo_lds.lua 文件，把下面的两个配置选项替换

成“laser”

tracking_frame = "laser",

published_frame = "laser"

2.修改 cartographer_ros 下的 demo_revo_lds.launch 文件

<launch>

<param name="/use_sim_time" value="flase" />

<!-- 首先是 rplidar 节点：因此必须保证是安装了 rplidar_ros -->

<node pkg="rplidar_ros" type="rplidarNode" name="rplidarNode"

output="screen">

<param name="serial_port" type="string"

value="/dev/ttyUSB0"/>

<param name="serial_baudrate" type="int" value="265000"/>

<param name="frame_id" type="string" value="laser"/>

<param name="inverted" type="bool" value="false"/>

<param name="angle_compensate" type="bool" value="true"/>

</node>

<!-- 然后是 cartographer 节点：他在这里调用了 revo_lds.lua 文件，这个文

件我修改了 首先把 “map”改成了“/map” -->

<node name="cartographer_node" pkg="cartographer_ros"

type="cartographer_node" args="

-configuration_directory $(find

cartographer_ros)/configuration_files

-configuration_basename revo_lds.lua"

output="screen">

<remap from="scan" to="scan" />

</node>

<node name="rviz" pkg="rviz" type="rviz" required="true"

args="-d $(find

cartographer_ros)/configuration_files/demo_2d.rviz" />"

</launch>

3.进入工作空间重新编译

catkin_make_isolated --install --use-ninja

4.启动

roslaunch cartographer_ros demo_revo_lds.launch

注释版的

cartographer

源 码 地 址 ：

https://github.com/xiangli0608/cartographer_detailed_comments_ws