图优化本身有成形的 开源的库 例如

  • g2o
  • ceres
  • gtsam

搞SLAM的话,最好可以理解其概念,然后再用好现成的工具.本篇主要介绍图优化的基本概念.以及在RVIZ中显示位姿图的代码

Pose Graph的概念

用一个图(Graph 图论)来表示SLAM问题
在这里插入图片描述

图中的节点来表示机器人的位姿 二维的话即为 (x,y,yaw)

两个节点之间的表示两个位姿的空间约束(相对位姿关系以及对应方差或线性矩阵)

边分为了两种边

  • 帧间边:连接的前后,时间上是连续的
  • 回环边:连接的前后,时间上是不连续的,但是直接也是两个位姿的空间约束

构建了回环边才会有误差出现,没有回环边是没有误差的

图优化的基本思想:
出现回环边,有了误差之后.构建图,并且找到一个最优的配置(各节点的位姿),让预测与观测的误差最小

一旦形成回环即可进行优化消除误差

里程积分的相对位姿视为预测值 图上的各个节点就是通过里程(激光里程计\轮速里程计)积分得到的
回环计算的相对位姿视为观测值 图上就是说通过 X2和X8的帧间匹配作为观测值

图优化要干的事:
构建图并调整各节点的位姿,让预测与观测的误差最小

图的构建

帧间边

里程测量 得到
在这里插入图片描述
相邻节点之间的相对位姿关系,可以由里程计、IMU、帧间匹配计算得到

回环边

通过回环检测得到
在这里插入图片描述
节点 i 和节点 j 在空间上相邻(观测到同样的数据),但是时间上不相邻(中间有其它节点)
用帧间匹配算法计算一个相对位姿 即为回环边的信息

一个简单的回环检测方法

在这里插入图片描述
针对当前节点,如图上的X8,标为红色

把其它节点为active (黄色)和 inactive(蓝色和绿色)两部分

找到当前节点周围一定范围内所有inactive节点,作为回环候选帧(绿色节点)
周围一定范围可以认为设定一个距离比如10m,或者根据位置不确定度了设置一个距离

当前节点和回环候选帧进行匹配,根据得分(匹配的相似度)判断是否形成回环

图优化简单例程

用一个最简单的例子,走一遍图优化的过程,加深整体理解.

Pose Graph建立(机器人移动情况):
在这里插入图片描述

  1. 一个机器人,初始点在X0处,位置为0.
  2. 然后机器人向前移动,通过编码器测得移动距离为1,节点为X1
  3. 机器人向后移动,通过编码器测得移动距离为0.8,节点为X2
  4. 通过闭环检测,X2与X0的偏差为0

位姿图(Pose Graph)就是这样的:
在这里插入图片描述

上面的例子,可以认为 编码器的测量存在误差,导致和闭环检测的结果,不一致.
那么到底机器人到了哪里,我们假如更相信闭环检测的结果,那么应该如何调整X0 X1 X2的位姿呢
就可以通过图优化的方式来进行求解.

图优化求解过程:
建立关系:
在这里插入图片描述
为了使总体的误差最小,使用最小二乘如下:
在这里插入图片描述
对上面的函数每个变量求偏导(雅克比矩阵),并使得偏导等于0在这里插入图片描述
这里既可以算出 x0 x1 和x2的值为:
在这里插入图片描述
即对整体进行了优化,使得误差项最小.

Pose Graph rivz显示

Code

下面的代码通过rviz将 建立的 Pose Graph 显示出来 ,方便进行 算法的调试

是通过 rviz的 MarkerArray 形式 来 展示 的. 写成了一个函数的形式,方便使用

/***
****函数名称: PublishGraphForVisulization 
****函数形参: pub 要发布的topic句柄 Vertexs 存的顶点向量  Edges 存的边的向量 color 颜色设置
****函数返回: 无
****函数功能: 在rviz中 查看 POSE graph  以MarkerArray的形式
****/
void PublishGraphForVisulization(ros::Publisher* pub,
                                 std::vector<Eigen::Vector3d>& Vertexs,
                                 std::vector<Edge>& Edges,
                                 int color = 0)
{

函数名称: PublishGraphForVisulization
函数形参: pub 要发布的topic句柄 Vertexs 存的顶点向量 Edges 存的边的向量 color 颜色设置
函数返回: 无
函数功能: 在rviz中 查看 POSE graph 以MarkerArray的形式

<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 分割线

    visualization_msgs::MarkerArray marray; //声明要发布的 MarkerArray

    //point--red
    visualization_msgs::Marker m; //声明一个maker  用来 画出 顶点
    //赋值顶点的 marker的 基本信息 初始化位置 为 0 
    m.header.frame_id = "map";
    m.header.stamp = ros::Time::now();
    m.id = 0;
    m.ns = "ls-slam";
    m.type = visualization_msgs::Marker::SPHERE;
    m.pose.position.x = 0.0;
    m.pose.position.y = 0.0;
    m.pose.position.z = 0.0;
    m.scale.x = 0.1;
    m.scale.y = 0.1;
    m.scale.z = 0.1;

    //根据传入的形参 设置 顶点的颜色
    if(color == 0)
    {
        m.color.r = 1.0;
        m.color.g = 0.0;
        m.color.b = 0.0;
    }
    else
    {
        m.color.r = 0.0;
        m.color.g = 1.0;
        m.color.b = 0.0;
    }

    m.color.a = 1.0;
    m.lifetime = ros::Duration(0);

声明要发布的 MarkerArray
声明一个maker 用来 画出 顶点
赋值顶点的 marker的 基本信息 初始化位置 为 0
根据传入的形参 设置 顶点的颜色

<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 分割线

    //linear--blue
    visualization_msgs::Marker edge;  //声明一个marker 用来画出边
    //赋值边的 marker的 基本信息 初始化位置 为 0 
    edge.header.frame_id = "map";
    edge.header.stamp = ros::Time::now();
    edge.action = visualization_msgs::Marker::ADD;
    edge.ns = "karto";
    edge.id = 0;
    edge.type = visualization_msgs::Marker::LINE_STRIP;
    edge.scale.x = 0.1;
    edge.scale.y = 0.1;
    edge.scale.z = 0.1;

    //根据传入的形参 设置 边的颜色
    if(color == 0)
    {
        edge.color.r = 0.0;
        edge.color.g = 0.0;
        edge.color.b = 1.0;
    }
    else
    {
        edge.color.r = 1.0;
        edge.color.g = 0.0;
        edge.color.b = 1.0;
    }
    edge.color.a = 1.0;

声明一个marker 用来画出边
赋值边的 marker的 基本信息 初始化位置 为 0
根据传入的形参 设置 边的颜色

<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 分割线

m.action = visualization_msgs::Marker::ADD;
    uint id = 0;

    //加入节点
    for (uint i=0; i<Vertexs.size(); i++)//遍历顶点 
    {
        //遍历每个顶点 将位置赋值
        m.id = id;
        m.pose.position.x = Vertexs[i](0);
        m.pose.position.y = Vertexs[i](1);
        marray.markers.push_back(visualization_msgs::Marker(m));//puse 该 maker
        id++;
    }

    //加入边
    for(int i = 0; i < Edges.size();i++)
    {
        //遍历每个边 将位置赋值
        Edge tmpEdge = Edges[i];
        edge.points.clear();

        geometry_msgs::Point p;
        p.x = Vertexs[tmpEdge.xi](0);
        p.y = Vertexs[tmpEdge.xi](1);
        edge.points.push_back(p);

        p.x = Vertexs[tmpEdge.xj](0);
        p.y = Vertexs[tmpEdge.xj](1);
        edge.points.push_back(p);
        edge.id = id;

        marray.markers.push_back(visualization_msgs::Marker(edge));
        id++;
    }

    pub->publish(marray);//发布 以MarkerArray
}

遍历每个顶点 将位置赋值
遍历每个边 将位置赋值
发布 以MarkerArray

函数完毕,调用方式如下
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 分割线

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pose_graph");//初始化节点

    ros::NodeHandle nodeHandle;//声明句柄

    // beforeGraph 声明一个publisher
    ros::Publisher beforeGraphPub;
    beforeGraphPub = nodeHandle.advertise<visualization_msgs::MarkerArray>("beforePoseGraph",1,true);

    // 设置 路径
    std::string VertexPath = "/home/jone/ros_slam_ws/src/data/test_v.dat";
    std::string EdgePath = "/home/jone/ros_slam_ws/src/data/test_e.dat";

    std::vector<Eigen::Vector3d> Vertexs;//声明顶点
    std::vector<Edge> Edges;//声明边

    ReadVertexInformation(VertexPath,Vertexs);//读取顶点
    ReadEdgesInformation(EdgePath,Edges);//读取边

    //调用 rivz poes graph 显示功能函数
    PublishGraphForVisulization(&beforeGraphPub,
                                Vertexs,
                                Edges);                            
    ros::spin();
    return 0;
}

Result

在这里插入图片描述

顶点的设置是这样的
在这里插入图片描述

边的设置是这样的
在这里插入图片描述
上面是一个用来测试的,例子

下面是实际工程中的数据的样子.
在这里插入图片描述
有了pose graph 的显示功能,可以很直观的 看到哪里建立了回环.