从零开始搭二维激光SLAM --- Karto后端的位姿图相关的数据结构分析

从这篇文章开始, 学习如何使用g2o, ceres, gtsam三个优化库进行后端优化.

在开始使用g2o进行优化的代码讲解之前, 先分析一下karto的后端的位姿图相关的数据结构.

karto 在 Mapper.h 中定义了后端优化的接口 ScanSolver, 在实现自己的优化的代码时只有对这个接口进行继承并重写就可以了.

ScanSolver的定义如下

/**
   * Graph optimization algorithm
   */
class ScanSolver
{
public:
    /**
     * Vector of id-pose pairs
     */
    typedef std::vector<std::pair<kt_int32s, Pose2>> IdPoseVector;

public:
    /**
     * Solve!
     */
    virtual void Compute() = 0;

    /**
     * Adds a node to the solver
     */
    virtual void AddNode(Vertex<LocalizedRangeScan> * /*pVertex*/)
    {
    }

    /**
     * Adds a constraint to the solver
     */
    virtual void AddConstraint(Edge<LocalizedRangeScan> * /*pEdge*/)
    {
    }
}; // ScanSolver

1 节点的数据结构

首先看AddNode函数

virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex);

可知, 这个函数传入的是karto::Vertex<karto::LocalizedRangeScan> *pVertex 格式的节点类型.

karto::Vertex

接下来, 看一下karto中对Vertex的定义的代码. 如下所示

/**
   * Represents an object in a graph
   */
template <typename T>
class Vertex
{
    friend class Edge<T>;
    inline const std::vector<Edge<T> *> &GetEdges() const
    {
        return m_Edges;
    }

    inline T *GetObject() const
    {
        return m_pObject;
    }
    
    T *m_pObject;
    std::vector<Edge<T> *> m_Edges;
}; // Vertex<T>

可知, 节点里存了2个数据, 一个是模板类型T的 m_pObject, 一个是存储边的vector m_Edges.

karto::LocalizedRangeScan

而m_pObject的类型在上边的接口中已经指定了, 是 karto::LocalizedRangeScan,

LocalizedRangeScan里内容比较多, 只有2个函数在优化这有用到

通过 GetCorrectedPose 函数获取匹配后的位姿.
通过 GetUniqueId 函数获取位姿的id.

2 边(约束)的数据结构

边的数据结构如下

/**
   * Represents an edge in a graph
   */
template <typename T>
class Edge
{
public:
    Edge(Vertex<T> *pSource, Vertex<T> *pTarget)
        : m_pSource(pSource), m_pTarget(pTarget), m_pLabel(NULL)
    {
        m_pSource->AddEdge(this);
        m_pTarget->AddEdge(this);
    }
private:
    Vertex<T> *m_pSource;
    Vertex<T> *m_pTarget;
    EdgeLabel *m_pLabel;
}; // class Edge<T>

可以看到, Edge存储了3个数据, 2个Vertex节点 m_pSource与m_pTarget, 和这两个节点间的约束的描述 m_pLabel.

3 约束的描述 LinkInfo

3.1 LinkInfo的构造

通过如下 LinkScans 函数来新生成 LinkInfo 的对象.

而 LinkScans 是在AddEdges函数中进行调用, 通过如下代码可知, 传入 LinkScans函数的 rMean 的值就是pScan->GetSensorPose(), 也就是节点的位姿.

// 将前一帧雷达数据与当前雷达数据连接，添加边约束，添加1条边
void MapperGraph::LinkScans(LocalizedRangeScan *pFromScan, LocalizedRangeScan *pToScan,
                            const Pose2 &rMean, const Matrix3 &rCovariance)
{
    if (isNewEdge == true)
    {
        pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
        if (m_pMapper->m_pScanOptimizer != NULL)
        {
            m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
        }
    }
}

void MapperGraph::AddEdges(LocalizedRangeScan *pScan, const Matrix3 &rCovariance)
{
    LinkScans(pSensorManager->GetScan(rSensorName, previousScanNum), 
        pScan, pScan->GetSensorPose(), rCovariance);
}

3.2 节点的位姿

而节点的位姿不是一成不变的, 一共有如下4处更改了节点的位姿. 分别是

• 前端扫描匹配时, 将匹配的结果设置成节点的位姿
• 添加边时, 当前节点与多个chain进行扫描匹配可能会得到多个位姿, 这里根据权重求得这些位姿的均值
• 回环时根据回环的结果重新设置位姿
• 优化之后, 根据优化后的结果重新设置所有的节点的位姿

对应的代码感兴趣的可以自己去看, 我就不放出来了.

3.3 LinkInfo类的定义

void MapperGraph::AddEdges(LocalizedRangeScan *pScan, const Matrix3 &rCovariance)
{
    // 第一种 将前一帧雷达数据与当前雷达数据连接, 添加边约束, 添加1条边
    LinkScans(pSensorManager->GetScan(rSensorName, previousScanNum), 
        pScan, pScan->GetSensorPose(), rCovariance);
}

// 将前一帧雷达数据与当前雷达数据连接，添加边约束，添加1条边
void MapperGraph::LinkScans(LocalizedRangeScan *pFromScan, LocalizedRangeScan *pToScan,
                            const Pose2 &rMean, const Matrix3 &rCovariance)
{
    kt_bool isNewEdge = true;
    // 将source与target连接起来，或者说将pFromScan与pToScan连接起来
    Edge<LocalizedRangeScan> *pEdge = AddEdge(pFromScan, pToScan, isNewEdge);

    // only attach link information if the edge is new
    if (isNewEdge == true)
    {
        pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), 
          rMean, rCovariance));
        if (m_pMapper->m_pScanOptimizer != NULL)
        {
            m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
        }
    }
}

class LinkInfo : public EdgeLabel
{
public:
    LinkInfo(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
    {
        Update(rPose1, rPose2, rCovariance);
    }

public:
    void Update(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
    {
        m_Pose1 = rPose1;
        m_Pose2 = rPose2;

        // transform second pose into the coordinate system of the first pose
        Transform transform(rPose1, Pose2());
        m_PoseDifference = transform.TransformPose(rPose2);

        // transform covariance into reference of first pose
        Matrix3 rotationMatrix;
        rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());

        m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
    }

private:
    Pose2 m_Pose1;
    Pose2 m_Pose2;
    Pose2 m_PoseDifference;
    Matrix3 m_Covariance;
}; // LinkInfo

可见, LinkInfo类里保存了节点的2个位姿, 与这2个位姿间的坐标变换, 和这两个节点的协方差矩阵.

这里的 m_Pose1 是 pFromScan->GetSensorPose(), 也就是当前节点的前一个节点的位姿, 是形成边的第一个节点.

这里的 m_Pose2 是 rMean, 也就是当前节点扫描匹配后得到的位姿, 是形成边的第二个节点.

m_PoseDifference 是这两个位姿间的坐标变换.

在保存协方差矩阵的时候, 保存的是 R * 协方差 * R^T, 对协方差矩阵做了相似变换, 保持特征值不变, 让特征向量旋转了pose1的负角度.

有2个函数在优化这需要用到

通过 GetPoseDifference 获取2个位姿的坐标变换
通过 GetCovariance 获取2个位姿坐标变换的协方差

4 协方差怎么算的

具体的这部分代码解析可以看我之前的文章

karto探秘之open_karto 第三章 — 扫描匹配
https://blog.csdn.net/tiancailx/article/details/105264605#t8

4.1 位置协方差

位置协方差的计算是在 ScanMatcher::ComputePositionalCovariance() 这个函数中.

可以通过如下图片来说明, 图片来自于 https://blog.csdn.net/qq_24893115/article/details/52965410

所有的可能解的坐标为 (x, y)
先验 SearchPose 与 后验 BestPose 的坐标差 作为均值 dx 与 dy
通过 (x -dx)^2 * response 得到 XX 的累加值, 在除以 response的和 得到平均值, 作为 cov(x, x) 的值.
协方差矩阵的其余值也是同样的计算.

4.2 角度协方差

角度是在 ScanMatcher::ComputeAngularCovariance() 函数中进行计算

差不多的原理, 只不过是 角度与角度均值的差 做累加.

5 总结

这篇文章简要分析了karto中的位姿图中对节点以及边的定义, 并且说了边的描述 LinkInfo 中成员变量的含义, 以及协方差矩阵是如何计算的.

这篇文章只是铺垫, 下篇文章讲解如何使用g2o实现karto的后端优化的计算.