深蓝学院激光slam学习——第六章（基于图优化的激光SLAM方法(Grid-based)）习题

这一章的习题主要围绕着位姿图优化进行

基础概念

位姿图优化的顶点和边

顶点 === Ti Tj
也就是 前端的结果，是scan matching的结果

边 === Tij 当做measurement

分为两类：
（1）里程计边，Tij是由里程计得到。
（2）回环边，Tij是由scan matching得到

具体公式的话看视频即可。

值得注意的是视频中有误的地方：首先是海森矩阵，视频中为：

实际上，中间为信息矩阵的逆：

其次，视频中的b写的是：

实际上是：

以slam十四讲那本书上为准。

习题里为了简单手写的高斯牛顿。
不如直接g2o。。。。。

附上解答：

main.cpp

#include <gaussian_newton.h>
#include <readfile.h>

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>


//for visual
void PublishGraphForVisulization(ros::Publisher* pub,
                                 std::vector<Eigen::Vector3d>& Vertexs,
                                 std::vector<Edge>& Edges,
                                 int color = 0)
{
    visualization_msgs::MarkerArray marray;

    //point--red
    visualization_msgs::Marker m;
    m.header.frame_id = "map";
    m.header.stamp = ros::Time::now();
    // m.lifetime = ros::Duration();
    // m.frame_locked = true;
    m.id = 0;
    // m.ns = "ls-slam";
    m.ns = "";
    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);

    //linear--blue
    visualization_msgs::Marker edge;
    edge.header.frame_id = "map";
    edge.header.stamp = ros::Time::now();
    edge.action = visualization_msgs::Marker::ADD;  // marker动作：创建或修改，与delete对应
    // edge.ns = "karto";
    edge.ns = "";
    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;

    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));
        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);
}




int main(int argc, char **argv)
{
    ros::init(argc, argv, "ls_slam");

    ros::NodeHandle nodeHandle;

    // beforeGraph
    ros::Publisher beforeGraphPub,afterGraphPub;
    beforeGraphPub = nodeHandle.advertise<visualization_msgs::MarkerArray>("beforePoseGraph",1,true);
    afterGraphPub  = nodeHandle.advertise<visualization_msgs::MarkerArray>("afterPoseGraph",1,true);


    std::string VertexPath = "/home/mjy/dev/lidar_space/src/ls_slam/data/test_quadrat-v.dat";
    std::string EdgePath = "/home/mjy/dev/lidar_space/src/ls_slam/data/test_quadrat-e.dat";

    // std::string VertexPath = "/home/mjy/dev/lidar_space/src/ls_slam/data/intel-v.dat";
    // std::string EdgePath = "/home/mjy/dev/lidar_space/src/ls_slam/data/intel-e.dat";

    std::vector<Eigen::Vector3d> Vertexs;
    std::vector<Edge> Edges;

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

    PublishGraphForVisulization(&beforeGraphPub,
                                Vertexs,
                                Edges);

    double initError = ComputeError(Vertexs,Edges);  // 用所有点和边计算一次初始error
    std::cout <<"initError:"<<initError<<std::endl;

    int maxIteration = 100;  // 迭代次数
    double epsilon = 1e-4;

    for(int i = 0; i < maxIteration;i++)
    {
        std::cout <<"Iterations:"<<i<<std::endl;
        Eigen::VectorXd dx = LinearizeAndSolve(Vertexs,Edges);

        //进行更新
        //TODO--Start
        for(int j=0; j < Vertexs.size(); j++)
        {

            Vertexs[j](0) += dx(3*j);
            Vertexs[j](1) += dx(3*j+1);
            Vertexs[j](2) += dx(3*j+2);

        }
        //TODO--End

        double maxError = -1;
        for(int k = 0; k < 3 * Vertexs.size();k++)
        {
            if(maxError < std::fabs(dx(k)))
            {
                maxError = std::fabs(dx(k));
            }
        }

        if(maxError < epsilon)
            break;
    }


    double finalError  = ComputeError(Vertexs,Edges);

    std::cout <<"FinalError:"<<finalError<<std::endl;

    PublishGraphForVisulization(&afterGraphPub,
                                Vertexs,
                                Edges,1);

    ros::spin();



    return 0;
}

2.gaussian_newton.cpp

#include "gaussian_newton.h"
#include <eigen3/Eigen/Jacobi>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Householder>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/LU>

#include <iostream>


//位姿-->转换矩阵
Eigen::Matrix3d PoseToTrans(Eigen::Vector3d x)
{
    Eigen::Matrix3d trans;
    trans << cos(x(2)),-sin(x(2)),x(0),
             sin(x(2)), cos(x(2)),x(1),
                     0,         0,    1;

    return trans;
}


//转换矩阵－－＞位姿
Eigen::Vector3d TransToPose(Eigen::Matrix3d trans)
{
    Eigen::Vector3d pose;
    pose(0) = trans(0,2);
    pose(1) = trans(1,2);
    pose(2) = atan2(trans(1,0),trans(0,0));

    return pose;
}

//计算整个pose-graph的误差
double ComputeError(std::vector<Eigen::Vector3d>& Vertexs,  // 一次计算，用掉所有点和边
                    std::vector<Edge>& Edges)
{
    double sumError = 0;
    for(int i = 0; i < Edges.size();i++)
    {
        Edge tmpEdge = Edges[i];
        Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
        Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
        Eigen::Vector3d z = tmpEdge.measurement;
        Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;

        Eigen::Matrix3d Xi = PoseToTrans(xi);  // 位姿到转换矩阵，这是因为计算误差是需要用T（转换矩阵）计算
        Eigen::Matrix3d Xj = PoseToTrans(xj);
        Eigen::Matrix3d Z  = PoseToTrans(z);

        Eigen::Matrix3d Ei = Z.inverse() *  Xi.inverse() * Xj;

        Eigen::Vector3d ei = TransToPose(Ei);  // 转换回来，获得真正的误差项（Xi.inverse() * Xj在Z.inverse()下的坐标）


        sumError += ei.transpose() * infoMatrix * ei;
    }
    return sumError;
}


/**
 * @brief CalcJacobianAndError
 *         计算jacobian矩阵和error
 * @param xi    fromIdx
 * @param xj    toIdx
 * @param z     观测值:xj相对于xi的坐标
 * @param ei    计算的误差
 * @param Ai    相对于xi的Jacobian矩阵
 * @param Bi    相对于xj的Jacobian矩阵
 */
void CalcJacobianAndError(Eigen::Vector3d xi,Eigen::Vector3d xj,Eigen::Vector3d z,
                          Eigen::Vector3d& ei,Eigen::Matrix3d& Ai,Eigen::Matrix3d& Bi)
{
    //TODO--Start

    Eigen::Matrix2d Rij;
    Eigen::Matrix2d Ri;
    Eigen::Matrix2d Rj;
    Eigen::Matrix2d Ri_theta;
    Eigen::Vector2d ti;
    Eigen::Vector2d tj;
    Eigen::Vector2d tij;

    Rij << cos(z(2)),-sin(z(2)),
           sin(z(2)), cos(z(2));

    Ri << cos(xi(2)),-sin(xi(2)),
          sin(xi(2)), cos(xi(2));

    Rj << cos(xj(2)),-sin(xj(2)),
          sin(xj(2)), cos(xj(2));

    Ri_theta << -sin(xi(2)),cos(xi(2)),
                -cos(xi(2)), -sin(xi(2));
    
    ti = Eigen::Vector2d(xi(0),xi(1));
    tj = Eigen::Vector2d(xj(0),xj(1));
    tij = Eigen::Vector2d(z(0),z(1));

    Ai.setZero();
    Bi.setZero();

    Ai(2,2) = -1;
    Ai.block<2,2>(0,0) = -Rij.transpose() * Ri.transpose();
    Ai.block<2,1>(0,2) = Rij.transpose() * Ri_theta * (tj-ti);

    Bi(2,2) = -1;
    Bi.block<2,2>(0,0) = Rij.transpose() * Ri.transpose();

    ei.head(2) = Rij.transpose()*(Ri.transpose()*(tj-ti)-tij);
    ei(2) = xj(2) - xi(2) - z(2);

    //TODO--end
}

/**
 * @brief LinearizeAndSolve
 *        高斯牛顿方法的一次迭代．
 * @param Vertexs   图中的所有节点
 * @param Edges     图中的所有边
 * @return          位姿的增量
 */
Eigen::VectorXd  LinearizeAndSolve(std::vector<Eigen::Vector3d>& Vertexs,
                                   std::vector<Edge>& Edges)
{
    //申请内存
    Eigen::MatrixXd H(Vertexs.size() * 3,Vertexs.size() * 3);
    Eigen::VectorXd b(Vertexs.size() * 3);

    H.setZero();
    b.setZero();

    //固定第一帧
    Eigen::Matrix3d I;
    I.setIdentity();
    H.block(0,0,3,3) += I;

    //构造H矩阵　＆ b向量
    for(int i = 0; i < Edges.size();i++)
    {
        //提取信息
        Edge tmpEdge = Edges[i];
        Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
        Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
        Eigen::Vector3d z = tmpEdge.measurement;
        Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;

        //计算误差和对应的Jacobian
        Eigen::Vector3d ei;
        Eigen::Matrix3d Ai;
        Eigen::Matrix3d Bi;
        CalcJacobianAndError(xi,xj,z,ei,Ai,Bi);


         //TODO--Start

        Eigen::MatrixXd Jij(3,Vertexs.size()*3);
        Jij.setZero();
        Jij.block<3,3>(0,tmpEdge.xi*3) = Ai;
        Jij.block<3,3>(0,tmpEdge.xj*3) = Bi;
        
        H += Jij.transpose() * infoMatrix.inverse() * Jij;

        // Eigen::MatrixXd ei_temp(Vertexs.size() * 3,1);
        // ei_temp.block<3,1>(3*tmpEdge.xi,0) = ei;
        // ei_temp.block<3,1>(3*tmpEdge.xj,0) = ei; 

        b += -Jij.transpose()* infoMatrix.inverse() *ei;
        // std::cout<< "debug!!!!!!!!!!!"<< std::endl;




        //TODO--End
    }

    //求解
    Eigen::VectorXd dx;

    //TODO--Start
    dx = H.ldlt().solve(b);
    // std::cout<< "本次迭代的变化量为：" << dx << std::endl;

    //TODO-End

    return dx;
}