第10讲后端2

最新推荐文章于 2024-04-27 12:17:01 发布

YMWM_

最新推荐文章于 2024-04-27 12:17:01 发布

阅读量1k

点赞数 2

分类专栏：《视觉SLAM十四讲》代码研读文章标签： slam 计算机视觉

本文链接：https://blog.csdn.net/YMWM_/article/details/117391506

版权

《视觉SLAM十四讲》代码研读专栏收录该内容

17 篇文章 38 订阅

订阅专栏

1 用g2o_viewer进行优化

打开终端，输入g2o_viewer sphere.g2o，得下图，
在这里插入图片描述点击左下角Optimize按钮，执行优化，优化结果为，

2 利用g2o默认的顶点和边进行优化

cpp文件内容为，

#include <iostream>
#include <fstream>
#include <string>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

using namespace std;

/***
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它
 * 尽管可以直接通过load函数来读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数
 */

int main()
{
    string path = "../sphere.g2o";
    ifstream fin(path);
    if(!fin)
    {
        cout << "无法找到文件" << path << "！" << endl;
        return 1;
    }

    //设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;  //图模型
    optimizer.setAlgorithm(solver);  //设置求解器
    optimizer.setVerbose(true);  //打开调试输出

    int vertexCnt = 0, edgeCnt = 0;  //顶点和边的数量
    while(!fin.eof())
    {
        string name;
        fin >> name;
        if(name == "VERTEX_SE3:QUAT"){
            //SE3顶点
            g2o::VertexSE3* v = new g2o::VertexSE3();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            if(index == 0)
                v->setFixed(true);
        } else if(name == "EDGE_SE3:QUAT"){
            //SE3-SE3边
            g2o::EdgeSE3* e = new g2o::EdgeSE3();
            int idx1, idx2;  //关联的两个顶点
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
        }
        if(!fin.good()) break;
    }

    cout << "读入" << vertexCnt << "个顶点和" << edgeCnt << "条边！" << endl;
    cout << "开始执行优化！" << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "保存优化结果！" << endl;
    optimizer.save("result.g2o");

    return 0;
}

CMakeLists.txt文件内容为，

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGE "-std=c++11 -O2")

include_directories("/usr/include/eigen3")

find_package(g2o REQUIRED)
include_directories(${G2O_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main g2o_core g2o_stuff g2o_types_slam3d ${CHOLMOD_LIBRARIES})

程序运行结果为，

读入2500个顶点和9799条边！
开始执行优化！
iteration= 0	 chi2= 1023011093.967641	 time= 0.49926	 cumTime= 0.49926	 edges= 9799	 schur= 0	 lambda= 805.622433	 levenbergIter= 1
iteration= 1	 chi2= 385118688.233187	 time= 0.450708	 cumTime= 0.949968	 edges= 9799	 schur= 0	 lambda= 537.081622	 levenbergIter= 1
iteration= 2	 chi2= 166223726.693657	 time= 0.447777	 cumTime= 1.39775	 edges= 9799	 schur= 0	 lambda= 358.054415	 levenbergIter= 1
iteration= 3	 chi2= 86610874.269316	 time= 0.445089	 cumTime= 1.84283	 edges= 9799	 schur= 0	 lambda= 238.702943	 levenbergIter= 1
iteration= 4	 chi2= 40582782.710190	 time= 0.445358	 cumTime= 2.28819	 edges= 9799	 schur= 0	 lambda= 159.135295	 levenbergIter= 1
iteration= 5	 chi2= 15055383.753041	 time= 0.444982	 cumTime= 2.73317	 edges= 9799	 schur= 0	 lambda= 101.425210	 levenbergIter= 1
iteration= 6	 chi2= 6715193.487655	 time= 0.452123	 cumTime= 3.1853	 edges= 9799	 schur= 0	 lambda= 37.664667	 levenbergIter= 1
iteration= 7	 chi2= 2171949.168382	 time= 0.44889	 cumTime= 3.63419	 edges= 9799	 schur= 0	 lambda= 12.554889	 levenbergIter= 1
iteration= 8	 chi2= 740566.827049	 time= 0.447842	 cumTime= 4.08203	 edges= 9799	 schur= 0	 lambda= 4.184963	 levenbergIter= 1
iteration= 9	 chi2= 313641.802464	 time= 0.4435	 cumTime= 4.52553	 edges= 9799	 schur= 0	 lambda= 2.583432	 levenbergIter= 1
iteration= 10	 chi2= 82659.743578	 time= 0.443948	 cumTime= 4.96948	 edges= 9799	 schur= 0	 lambda= 0.861144	 levenbergIter= 1
iteration= 11	 chi2= 58220.369189	 time= 0.443721	 cumTime= 5.4132	 edges= 9799	 schur= 0	 lambda= 0.287048	 levenbergIter= 1
iteration= 12	 chi2= 52214.188561	 time= 0.4424	 cumTime= 5.8556	 edges= 9799	 schur= 0	 lambda= 0.095683	 levenbergIter= 1
iteration= 13	 chi2= 50948.580336	 time= 0.444892	 cumTime= 6.30049	 edges= 9799	 schur= 0	 lambda= 0.031894	 levenbergIter= 1
iteration= 14	 chi2= 50587.776729	 time= 0.446354	 cumTime= 6.74685	 edges= 9799	 schur= 0	 lambda= 0.016436	 levenbergIter= 1
iteration= 15	 chi2= 50233.038802	 time= 0.44271	 cumTime= 7.18956	 edges= 9799	 schur= 0	 lambda= 0.010957	 levenbergIter= 1
iteration= 16	 chi2= 49995.082839	 time= 0.44841	 cumTime= 7.63797	 edges= 9799	 schur= 0	 lambda= 0.007305	 levenbergIter= 1
iteration= 17	 chi2= 48876.738967	 time= 0.880772	 cumTime= 8.51874	 edges= 9799	 schur= 0	 lambda= 0.009298	 levenbergIter= 2
iteration= 18	 chi2= 48806.625522	 time= 0.443205	 cumTime= 8.96194	 edges= 9799	 schur= 0	 lambda= 0.006199	 levenbergIter= 1
iteration= 19	 chi2= 47790.891373	 time= 0.880511	 cumTime= 9.84245	 edges= 9799	 schur= 0	 lambda= 0.008265	 levenbergIter= 2
iteration= 20	 chi2= 47713.626582	 time= 0.443605	 cumTime= 10.2861	 edges= 9799	 schur= 0	 lambda= 0.005510	 levenbergIter= 1
iteration= 21	 chi2= 46869.323689	 time= 0.880758	 cumTime= 11.1668	 edges= 9799	 schur= 0	 lambda= 0.007347	 levenbergIter= 2
iteration= 22	 chi2= 46802.585509	 time= 0.444129	 cumTime= 11.6109	 edges= 9799	 schur= 0	 lambda= 0.004898	 levenbergIter= 1
iteration= 23	 chi2= 46128.758041	 time= 0.880064	 cumTime= 12.491	 edges= 9799	 schur= 0	 lambda= 0.006489	 levenbergIter= 2
iteration= 24	 chi2= 46069.133541	 time= 0.445264	 cumTime= 12.9363	 edges= 9799	 schur= 0	 lambda= 0.004326	 levenbergIter= 1
iteration= 25	 chi2= 45553.862164	 time= 0.879013	 cumTime= 13.8153	 edges= 9799	 schur= 0	 lambda= 0.005595	 levenbergIter= 2
iteration= 26	 chi2= 45511.762616	 time= 0.445974	 cumTime= 14.2613	 edges= 9799	 schur= 0	 lambda= 0.003730	 levenbergIter= 1
iteration= 27	 chi2= 45122.762999	 time= 0.881711	 cumTime= 15.143	 edges= 9799	 schur= 0	 lambda= 0.004690	 levenbergIter= 2
iteration= 28	 chi2= 45095.174397	 time= 0.443623	 cumTime= 15.5866	 edges= 9799	 schur= 0	 lambda= 0.003127	 levenbergIter= 1
iteration= 29	 chi2= 44811.248505	 time= 0.878859	 cumTime= 16.4655	 edges= 9799	 schur= 0	 lambda= 0.003785	 levenbergIter= 2
保存优化结果！

利用g2o_viewer打开优化后的位姿图result.g2o如下，
在这里插入图片描述

3 利用自己定义的顶点和边进行优化

cpp文件内容为，

#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

#include <sophus/se3.hpp>

using namespace std;
using namespace Eigen;
using Sophus::SE3d;  //在SE3d子类中引用基类Sophus的成员
using Sophus::SO3d;  //在SO3d子类中引用基类Sophus的成员

/**
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 本节使用李代数表达位姿，节点和边的方式为自定义
 */

typedef Matrix<double, 6, 6> Matrix6d;

//给定误差求J_R^{-1}的近似
Matrix6d JRInv(const SE3d& e){
    Matrix6d J;
    J.block(0, 0, 3, 3) = SO3d::hat(e.so3().log());
    J.block(0, 3, 3, 3) = SO3d::hat(e.translation());
    J.block(3, 0, 3, 3) = Matrix3d::Zero(3, 3);
    J.block(3, 3, 3, 3) = SO3d::hat(e.so3().log());
    J = J * 0.5 + Matrix6d::Identity();
    //J = Matrix6d::Identity();  //用单位阵来近似雅可比矩阵
    return J;
}

//李代数顶点
typedef Matrix<double, 6, 1> Vector6d;

class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual bool read(istream& is) override{
        double data[7];
        for(int i = 0; i < 7; i++)
            is >> data[i];
        setEstimate(SE3d(Quaterniond(data[6], data[3], data[4], data[5]), Vector3d(data[0], data[1], data[2])));
        return true;  //这句话很重要，缺少会报错！
        //报错的信息如下
        //Sophus ensure failed in function 'void Sophus::SO3Base<Derived>::normalize() [with Derived = Sophus::SO3<double>]', file '/usr/local/include/sophus/so3.hpp', line 300.
Quaternion (   0.706662 4.32706e-17    0.707551 -4.3325e-17) should not be close to zero!
    }

    virtual bool write(ostream& os) const override{
        os << id() << " ";
        Quaterniond q = _estimate.unit_quaternion();
        os << _estimate.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl;
        return true;
    }

    virtual void setToOriginImpl() override{
        _estimate = SE3d();
    }

    //左乘更新
    virtual void oplusImpl(const double* update) override{
        Vector6d upd;
        upd << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = SE3d::exp(upd) * _estimate;
    }
};

//两个李代数节点之边
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual bool read(istream& is) override{
        double data[7];
        for(int i = 0; i < 7; i++)
            is >> data[i];
        Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
        setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2])));
        for(int i = 0; i < information().rows() && is.good(); i++)
            for(int j = i; j < information().cols() && is.good(); j++){
                is >> information()(i, j);
                if(i != j)
                    information()(j, i) = information()(i, j);
            }
        return true;
    }

    virtual bool write(ostream& os) const override{
        VertexSE3LieAlgebra* v1 = static_cast<VertexSE3LieAlgebra*> (_vertices[0]);
        VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*> (_vertices[1]);
        os << v1->id() << " " << v2->id() << " ";
        SE3d m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();
        os << m.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";

        //信息矩阵
        for(int i = 0; i < information().rows(); i++)
            for(int j = i; j < information().cols(); j++)
                os << information()(i, j) << " ";
        os << endl;
        return true;
    }

    //误差与书中推导的一致
    virtual void computeError() override{
        SE3d v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        _error = (_measurement.inverse() * v1.inverse() * v2).log();
    }

    //雅可比计算
    virtual void linearizeOplus() override{
        SE3d v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3d::exp(_error));
        //尝试把J近似为I？
        _jacobianOplusXi = -J * v2.inverse().Adj();
        _jacobianOplusXj = J * v2.inverse().Adj();
    }
};

int main()
{
    string path = "../sphere.g2o";
    ifstream fin(path);
    if(!fin){
        cout << "找不到文件" << path << "！" << endl;
        return 1;
    }

    //设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
            );
    g2o::SparseOptimizer optimizer;  //设置图模型
    optimizer.setAlgorithm(solver);  //设置求解器
    optimizer.setVerbose(true);  //打开调试输出

    int vertexCnt = 0, edgeCnt = 0;  //顶点和边的数量

    vector<VertexSE3LieAlgebra*> vectices;
    vector<EdgeSE3LieAlgebra*> edges;
    while(!fin.eof()){
        string name;
        fin >> name;
        if(name == "VERTEX_SE3:QUAT"){
            //顶点
            VertexSE3LieAlgebra* v = new VertexSE3LieAlgebra();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if(index == 0)
                v->setFixed(true);
        }else if(name == "EDGE_SE3:QUAT"){
            //SE3-SE3边
            EdgeSE3LieAlgebra* e = new EdgeSE3LieAlgebra();
            int idx1, idx2;  //关联的两个顶点
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if(!fin.good()) break;
    }

    cout << "读入" << vertexCnt << "个顶点和" << edgeCnt << "条边!" << endl;
    cout << "开始优化！" << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "保存优化结果！" << endl;
    //因为用了自定义顶点且没有向g2o注册，这里的保存自己来实现
    //伪装成SE3顶点和边，让g2o_viewer可以认出
    ofstream fout("result_lie.g2o");
    for(VertexSE3LieAlgebra* v : vectices){
        fout << "VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for(EdgeSE3LieAlgebra* e : edges){
        fout << "EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();

    return 0;
}

需要注意的一点是，高翔博士在github上提供的代码中，关于顶点类VertexSE3LieAlgebra的读入函数virtual bool read(istream& is)缺少返回值，会报如下错误，

Sophus ensure failed in function 'void Sophus::SO3Base<Derived>::normalize() [with Derived = Sophus::SO3<double>]', file '/usr/local/include/sophus/so3.hpp', line 300.
Quaternion (   0.706662 4.32706e-17    0.707551 -4.3325e-17) should not be close to zero!

在函数virtual bool read(istream& is)中加入return true;即可避免！
CMakeLists.txt文件内容为，

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")

include_directories("/usr/include/eigen3")

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

find_package(g2o REQUIRED)
include_directories(${G2O_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main g2o_core g2o_stuff ${CHOLMOD_LIBRARIES} ${Sophus_LIBRARIES})

程序运行结果为，

读入2500个顶点和9799条边!
开始优化！
iteration= 0	 chi2= 626657736.014956	 time= 0.483827	 cumTime= 0.483827	 edges= 9799	 schur= 0	 lambda= 6706.585223	 levenbergIter= 1
iteration= 1	 chi2= 233236853.521435	 time= 0.426288	 cumTime= 0.910115	 edges= 9799	 schur= 0	 lambda= 2235.528408	 levenbergIter= 1
iteration= 2	 chi2= 142629876.750105	 time= 0.426396	 cumTime= 1.33651	 edges= 9799	 schur= 0	 lambda= 745.176136	 levenbergIter= 1
iteration= 3	 chi2= 84218288.615591	 time= 0.425538	 cumTime= 1.76205	 edges= 9799	 schur= 0	 lambda= 248.392045	 levenbergIter= 1
iteration= 4	 chi2= 42150277.050051	 time= 0.425198	 cumTime= 2.18725	 edges= 9799	 schur= 0	 lambda= 82.797348	 levenbergIter= 1
iteration= 5	 chi2= 16727578.672824	 time= 0.421611	 cumTime= 2.60886	 edges= 9799	 schur= 0	 lambda= 27.599116	 levenbergIter= 1
iteration= 6	 chi2= 6190811.332756	 time= 0.423815	 cumTime= 3.03267	 edges= 9799	 schur= 0	 lambda= 9.199705	 levenbergIter= 1
iteration= 7	 chi2= 2200248.525946	 time= 0.426517	 cumTime= 3.45919	 edges= 9799	 schur= 0	 lambda= 3.066568	 levenbergIter= 1
iteration= 8	 chi2= 735241.315722	 time= 0.424676	 cumTime= 3.88387	 edges= 9799	 schur= 0	 lambda= 1.022189	 levenbergIter= 1
iteration= 9	 chi2= 285010.315820	 time= 0.424344	 cumTime= 4.30821	 edges= 9799	 schur= 0	 lambda= 0.340730	 levenbergIter= 1
iteration= 10	 chi2= 171308.106999	 time= 0.421721	 cumTime= 4.72993	 edges= 9799	 schur= 0	 lambda= 0.185811	 levenbergIter= 1
iteration= 11	 chi2= 145525.374043	 time= 0.425215	 cumTime= 5.15515	 edges= 9799	 schur= 0	 lambda= 0.061937	 levenbergIter= 1
iteration= 12	 chi2= 142421.028242	 time= 0.427099	 cumTime= 5.58225	 edges= 9799	 schur= 0	 lambda= 0.020646	 levenbergIter= 1
iteration= 13	 chi2= 137589.533237	 time= 0.421541	 cumTime= 6.00379	 edges= 9799	 schur= 0	 lambda= 0.006882	 levenbergIter= 1
iteration= 14	 chi2= 131299.868729	 time= 0.425563	 cumTime= 6.42935	 edges= 9799	 schur= 0	 lambda= 0.002294	 levenbergIter= 1
iteration= 15	 chi2= 128029.472808	 time= 0.422409	 cumTime= 6.85176	 edges= 9799	 schur= 0	 lambda= 0.000765	 levenbergIter= 1
iteration= 16	 chi2= 127588.350402	 time= 0.423095	 cumTime= 7.27485	 edges= 9799	 schur= 0	 lambda= 0.000255	 levenbergIter= 1
iteration= 17	 chi2= 127578.186995	 time= 0.422654	 cumTime= 7.69751	 edges= 9799	 schur= 0	 lambda= 0.000085	 levenbergIter= 1
iteration= 18	 chi2= 127578.157870	 time= 0.423425	 cumTime= 8.12093	 edges= 9799	 schur= 0	 lambda= 0.000028	 levenbergIter= 1
iteration= 19	 chi2= 127578.157859	 time= 0.423385	 cumTime= 8.54432	 edges= 9799	 schur= 0	 lambda= 0.000019	 levenbergIter= 1
iteration= 20	 chi2= 127578.157859	 time= 0.424681	 cumTime= 8.969	 edges= 9799	 schur= 0	 lambda= 0.000013	 levenbergIter= 1
iteration= 21	 chi2= 127578.157859	 time= 0.426756	 cumTime= 9.39575	 edges= 9799	 schur= 0	 lambda= 0.000008	 levenbergIter= 1
iteration= 22	 chi2= 127578.157859	 time= 0.421864	 cumTime= 9.81762	 edges= 9799	 schur= 0	 lambda= 0.000006	 levenbergIter= 1
iteration= 23	 chi2= 127578.157859	 time= 2.49601	 cumTime= 12.3136	 edges= 9799	 schur= 0	 lambda= 0.122207	 levenbergIter= 6
iteration= 24	 chi2= 127578.157859	 time= 4.16716	 cumTime= 16.4808	 edges= 9799	 schur= 0	 lambda= 2866509743109.458984	 levenbergIter= 10
保存优化结果！

在这里插入图片描述
关于优化前后位姿图展示视频见优化前和优化后。

YMWM_

关注

2
点赞
踩
8

收藏

觉得还不错? 一键收藏
打赏
2
评论
第10讲后端2

目录1 用g2o_viewer进行优化2 利用g2o默认的顶点和边进行优化3 利用自己定义的顶点和边进行优化1 用g2o_viewer进行优化打开终端，输入g2o_viewer sphere.g2o，得下图，点击左下角Optimize按钮，执行优化，优化结果为，2 利用g2o默认的顶点和边进行优化 cpp文件内容为，#include <iostream>#include <fstream>#include <string>#include &l
复制链接

扫一扫