SLAM项目：从0开始，C++实现SLAM的后端优化，并在gazebo中可视化结果

SLAM的后端是整个SLAM中十分重要的部分，一般会承担整个SLAM过程中最大的计算量，所以一般后端的运算时间都会比较长，为了保证系统的实时性，往往采取多线程的方式为后端独立开启一个线程，同时在后端优化的过程中采取诸如只优化位姿，不优化路标等tricks控制整体的规模，这篇文章通过C++来为我们前面实现的SLAM增加一个后端，让前端的位姿在这里得到优化，从而建立出更准确的地图。

下面这几篇文章可以带你一步一步实现一套完整的SLAM系统（不调库，不抄开源框架），可以作为一个不错的简历加分项目

前端：

SLAM项目：从0开始复现2D激光里程计，并利用自己的雷达或者gazebo运行，详细解释原理及代码实现过程_Soumes的博客-CSDN博客

后端：

SLAM项目：从0开始，C++实现SLAM的后端优化，并在gazebo中可视化结果_Soumes的博客-CSDN博客

建图：

SLAM项目：从0开始复现一套完整的二维激光SLAM算法_Soumes的博客-CSDN博客

老规矩，在开始之前先展示效果：

 可以看到添加后端以后，在复杂环境下的建图也很稳，机器人快速运动的同时，也能够得到很精确的位姿，但是由于加入了后端，并且暂时还没有编写多线程，所以整体速度下降，为0.09s完成一次计算，我们的雷达是10hz的，0.09s意味着在一帧雷达数据的周期内依旧能够完成计算，所以不会出现数据堵塞和弃用，依旧保证了我们的实时性（当然比起之前的0.02秒已经慢了许多，可见后端确实承担很大的计算量）

下面简单介绍后端实现的原理

这次实现的后端是基于优化思想的，通过一个scan-to-map的精匹配获得一个精确的位姿，与前端scan-to-scan获得的初始位姿进行融合，从而得到一个优化后的位姿，总体流程可以分为三步。

第一步，建立局部地图

第一步通过获取雷达的数据建立一个局部地图（submap）这个地图的目的是用来作为scan-to-map里面的map，不同于我们全局发布的map，这个map的生命周期很短，在完成scan-to-map后就销毁了，少存点图能够节约资源，同时由于机器人在运动，局部的图也一直在变换，如果长期保存不利于我们下一次在新的位置的匹配（如果想要进一步优化，可以考虑把局部子图换成滑动窗口，这样可以在控制资源的情况下纳入更多的环境信息，但是相应的实现起来就更难）

代码如下：

#ifndef SUBMAP_BACKEND
#define SUBMAP_BACKEND
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>
#include <cmath>
struct Point_sub {
    int x;
    int y;
};


class submap_creater
{
public:
submap_creater();

std::vector<Point_sub> Bresenham_sub(int x0, int y0, int x1, int y1);
// 接受雷达数据 建立局部子图 返回子图

nav_msgs::OccupancyGrid CreateSubMap(const sensor_msgs::LaserScan::ConstPtr &scan_msg, double r_x, double r_y, double r_theta);
private:
//map param
double resolution_sub;
int map_width_sub;
int map_height_sub;
double map_origin_x_sub;
double map_origin_y_sub;
double occupied_threshold_sub;
double free_threshold_sub;
double angle_robot_sub;
double x_robot_sub;
double y_robot_sub;
int grid_x_sub;
int grid_y_sub;
int robot_location_x_sub;
int robot_location_y_sub;
double temp_angle_increment_sub;
double temp_angle_min_sub;
std::vector<std::vector<int>> grid_map_sub;
nav_msgs::OccupancyGrid map_msg_sub;
};

#endif 

.cc内容太多了，丢个.h在这里示意一下（反正也没什么人看=。=）通过核心函数CreateSubMap来建立子图，由于这个子图只是用来做匹配的，所以不需要发布这张地图，把map_msg_sub这个地图信息返回就可以了，在后续会获取这个信息作为scan-to-map的匹配依据。

第二步，实现scan-to-map匹配

在slam的历史长河中，涌现了无数种scan-to-map的匹配方式，从2011年的hector提出这种匹配方式之后，后续的如cartographer，loam，甚至是视觉方面的orb，openvins都采用scan-to-map的方式进行匹配，这种方式比起scan-to-scan有更高的精度，更大的运算量，在这里由于我们是用scan-to-map作为后端，需要的就是精度，所以不再刻意压缩规模，只剔除掉无效点，就直接进行匹配，输入一个雷达数据和robot pose 以及上一步得到的局部地图submap，得到一个非常精确的位姿

代码如下：

#ifndef MATCHER_BACKEND
#define MATCHER_BACKEND
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <nav_msgs/OccupancyGrid.h>
// 扫描匹配给出一个精位姿
class Backend {
public:
    
    std::vector<double> Backend_match(const sensor_msgs::LaserScan::ConstPtr& scan_msg, nav_msgs::OccupancyGrid map_msg);
    //Backend();
private:

    bool is_map_available;
 
};
#endif 

 通过核心函数Backend_match一发入魂，用0.07秒的运算，还你一个精确到...也还算精确的位姿估计，注意这里这些过程中创建了一些指针，如果要玩指针的话一定要注意管理好内存，做好指针的初始化和释放，还有不要没事就写=nullptr，不然会总是报内存相关的错误，排查起来非常的不方便，一个简单粗暴地方法就是把那些Constptr通通去掉，咱不玩指针，老老实实用变量。

第三步，融合前端scan-to-scan的位姿和后端scan-to-map的位姿

这一步十分关键，前后端都给出了一个位姿，要合理的融合这两个位姿才能够获得最好的位姿输出，如果权重设置的不好，就会导致数据的浪费和不准确，这里利用插值的方式对两个位姿进行融合，对于旋转，采取球面插值，权重为alpha，对于平移，采用线性差值，权重也为alpha，差值的目的就是在两个值之间找到一个值，这个值就是两者的融合结果，至于在哪里找到，就要看alpha了。alpha在这里作为权重，是按照时间戳进行定义的，我们获取前端给出的位姿时间戳timestamp1，后端给出的位姿时间戳timestamp2，并定义alpha为0到1之间的量，如果两个位姿的时间戳越接近，那么alpha就越往1靠，后端给出来的位姿对融合结果的影响越大（因为两者发布的时间近的话可以近似认为是对同一个场景计算得到的位姿，那自然要重用通过后端scan-to-map精匹配出来的）如果两个位姿的时间戳差的很多，那么alpha就接近0，更看重前端给的位姿，这是因为两者时间差得多，说明可能后端由于运算量大，导致位姿发布迟了。前端是很快的，所以肯定是用最新的信息算出来的，所以至少能肯定前端的pose就是最新的pose，但是后端很慢，不能确定此刻后端发布的pose是否还是最新环境中的pose，这种情况下，就更看重前端的pose，毕竟我们要的是最新的环境下的pose，保障实时性。

代码如下：

#ifndef POSE_FUSION_H
#define POSE_FUSION_H

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>
#include <ros/ros.h>
using namespace std;
class PoseFusion {
public:
    struct Pose {
        Eigen::Matrix3d rotation_matrix;
        Eigen::Vector3d translation_vector;
    };

    PoseFusion();

    vector<double> integratePoses(std::vector<double> pose1, std::vector<double> pose2, double time1, double time2);

    Pose double_2_pose(double x, double y, double theta);


private:
    double timestamp1_;
    double timestamp2_;
};

#endif // POSE_FUSION_H

核心函数integratePose完成对于位姿的融合，输出一个新的融合后的位姿，可以用这个位姿来建图，那就会比用前端给的pose建图更精确。

要注意：后端匹配的时候用的都是雷达数据scan，位于laser坐标系下，所以给出来的位姿也是在laser坐标系下的，要注意一下这里的坐标变换，要严格保证输入输出的所有数据都是处于同一个坐标系，不然的话就会全乱套，导致出来的位姿不知道在哪个坐标系，那建图就不知道建到哪里去了，坐标系变换永远是slam过程最重要的事情。

总结：

通过scan-to-map的精匹配融合前端的scan-to-scan粗匹配，对前端的位姿进行优化，实现了slam的后端，到这里我们自己写的slam系统就已经接近尾声了，最后一步是为整个系统加上一个回环检测，等我研究好搞定了以后再来更新。