深藍學院激光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;
}