【slam十四讲第二版】【课本例题代码向】【第十讲~后端2】

已于 2022-08-27 12:08:28 修改
阅读量627 收藏 1
点赞数 1
分类专栏： 视觉SLAM14讲 文章标签： 算法 g2o 位姿图
于 2022-07-28 15:20:33 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_45954434/article/details/126035397
版权

【slam十四讲第二版】【课本例题代码向】【第十讲~后端2】

0 前言

1 实践：位姿图优化

1.1 g2o原生位姿图

1.1.1 pose_graph_g2o_SE3.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(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << 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 << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "saving optimization results ..." << endl;
    optimizer.save("result.g2o");

    return 0;
}

1.1.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(pose_graph_g2o_SE3)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 14)

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

# Eigen
include_directories("/usr/include/eigen3")

# sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

SET(G2O_LIBS  g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension g2o_types_slam3d cxsparse)

add_executable(pose_graph_g2o_SE3 src/pose_graph_g2o_SE3.cpp)
target_link_libraries(pose_graph_g2o_SE3
        ${G2O_LIBS}
        ${CHOLMOD_LIBRARIES} fmt
        )

1.1.3 输出

/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_g2o_SE3/cmake-build-debug/pose_graph_g2o_SE3 ./src/sphere.g2o
read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0	 chi2= 1023011093.967642	 time= 0.391243	 cumTime= 0.391243	 edges= 9799	 schur= 0	 lambda= 805.622433	 levenbergIter= 1
iteration= 1	 chi2= 385118688.233188	 time= 0.288333	 cumTime= 0.679577	 edges= 9799	 schur= 0	 lambda= 537.081622	 levenbergIter= 1
iteration= 2	 chi2= 166223726.693658	 time= 0.286221	 cumTime= 0.965798	 edges= 9799	 schur= 0	 lambda= 358.054415	 levenbergIter= 1
iteration= 3	 chi2= 86610874.269316	 time= 0.272452	 cumTime= 1.23825	 edges= 9799	 schur= 0	 lambda= 238.702943	 levenbergIter= 1
iteration= 4	 chi2= 40582782.710190	 time= 0.271862	 cumTime= 1.51011	 edges= 9799	 schur= 0	 lambda= 159.135295	 levenbergIter= 1
iteration= 5	 chi2= 15055383.753040	 time= 0.271552	 cumTime= 1.78166	 edges= 9799	 schur= 0	 lambda= 101.425210	 levenbergIter= 1
iteration= 6	 chi2= 6715193.487654	 time= 0.27709	 cumTime= 2.05875	 edges= 9799	 schur= 0	 lambda= 37.664667	 levenbergIter= 1
iteration= 7	 chi2= 2171949.168383	 time= 0.27673	 cumTime= 2.33548	 edges= 9799	 schur= 0	 lambda= 12.554889	 levenbergIter= 1
iteration= 8	 chi2= 740566.827049	 time= 0.282254	 cumTime= 2.61774	 edges= 9799	 schur= 0	 lambda= 4.184963	 levenbergIter= 1
iteration= 9	 chi2= 313641.802464	 time= 0.272131	 cumTime= 2.88987	 edges= 9799	 schur= 0	 lambda= 2.583432	 levenbergIter= 1
iteration= 10	 chi2= 82659.743578	 time= 0.277022	 cumTime= 3.16689	 edges= 9799	 schur= 0	 lambda= 0.861144	 levenbergIter= 1
iteration= 11	 chi2= 58220.369189	 time= 0.293291	 cumTime= 3.46018	 edges= 9799	 schur= 0	 lambda= 0.287048	 levenbergIter= 1
iteration= 12	 chi2= 52214.188561	 time= 0.326622	 cumTime= 3.7868	 edges= 9799	 schur= 0	 lambda= 0.095683	 levenbergIter= 1
iteration= 13	 chi2= 50948.580336	 time= 0.28764	 cumTime= 4.07444	 edges= 9799	 schur= 0	 lambda= 0.031894	 levenbergIter= 1
iteration= 14	 chi2= 50587.776729	 time= 0.335924	 cumTime= 4.41037	 edges= 9799	 schur= 0	 lambda= 0.016436	 levenbergIter= 1
iteration= 15	 chi2= 50233.038802	 time= 0.279265	 cumTime= 4.68963	 edges= 9799	 schur= 0	 lambda= 0.010957	 levenbergIter= 1
iteration= 16	 chi2= 49995.082836	 time= 0.274312	 cumTime= 4.96394	 edges= 9799	 schur= 0	 lambda= 0.007305	 levenbergIter= 1
iteration= 17	 chi2= 48876.738968	 time= 0.547949	 cumTime= 5.51189	 edges= 9799	 schur= 0	 lambda= 0.009298	 levenbergIter= 2
iteration= 18	 chi2= 48806.625520	 time= 0.277708	 cumTime= 5.7896	 edges= 9799	 schur= 0	 lambda= 0.006199	 levenbergIter= 1
iteration= 19	 chi2= 47790.891374	 time= 0.534136	 cumTime= 6.32374	 edges= 9799	 schur= 0	 lambda= 0.008265	 levenbergIter= 2
iteration= 20	 chi2= 47713.626578	 time= 0.271654	 cumTime= 6.59539	 edges= 9799	 schur= 0	 lambda= 0.005510	 levenbergIter= 1
iteration= 21	 chi2= 46869.323691	 time= 0.534435	 cumTime= 7.12983	 edges= 9799	 schur= 0	 lambda= 0.007347	 levenbergIter= 2
iteration= 22	 chi2= 46802.585509	 time= 0.273815	 cumTime= 7.40364	 edges= 9799	 schur= 0	 lambda= 0.004898	 levenbergIter= 1
iteration= 23	 chi2= 46128.758046	 time= 0.53324	 cumTime= 7.93688	 edges= 9799	 schur= 0	 lambda= 0.006489	 levenbergIter= 2
iteration= 24	 chi2= 46069.133544	 time= 0.270033	 cumTime= 8.20692	 edges= 9799	 schur= 0	 lambda= 0.004326	 levenbergIter= 1
iteration= 25	 chi2= 45553.862168	 time= 0.53346	 cumTime= 8.74038	 edges= 9799	 schur= 0	 lambda= 0.005595	 levenbergIter= 2
iteration= 26	 chi2= 45511.762622	 time= 0.273562	 cumTime= 9.01394	 edges= 9799	 schur= 0	 lambda= 0.003730	 levenbergIter= 1
iteration= 27	 chi2= 45122.763002	 time= 0.535634	 cumTime= 9.54957	 edges= 9799	 schur= 0	 lambda= 0.004690	 levenbergIter= 2
iteration= 28	 chi2= 45095.174401	 time= 0.26904	 cumTime= 9.81861	 edges= 9799	 schur= 0	 lambda= 0.003127	 levenbergIter= 1
iteration= 29	 chi2= 44811.248507	 time= 0.532803	 cumTime= 10.3514	 edges= 9799	 schur= 0	 lambda= 0.003785	 levenbergIter= 2
saving optimization results ...

进程已结束,退出代码0


g2o_viewer result.g2o

  • sphere.g2o可视化为
    在这里插入图片描述

  • result.g2o可视化为
    在这里插入图片描述

1.2 李代数上的位姿图优化

1.2.1 pose_graph_g2o_lie_algebra.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;
using Sophus::SO3d;

/************************************************
 * 本程序演示如何用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();    // try Identity if you want
    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])
        ));
    }

    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] << " ";

        // information matrix
        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(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << 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 << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "saving optimization results ..." << 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;
}

1.2.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(pose_graph)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 14)

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

# Eigen
include_directories("/usr/include/eigen3")

# sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

SET(G2O_LIBS  g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension g2o_types_slam3d cxsparse)

add_executable(pose_graph_g2o_lie src/pose_graph_g2o_lie_algebra.cpp)
target_link_libraries(pose_graph_g2o_lie
        ${G2O_LIBS}
        ${CHOLMOD_LIBRARIES}
        ${Sophus_LIBRARIES} fmt
        )

1.2.3 输出

/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_g2o_lie_algebra/cmake-build-debug/pose_graph_g2o_lie ./src/sphere.g2o
read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0	 chi2= 674837160.579970	 time= 0.313929	 cumTime= 0.313929	 edges= 9799	 schur= 0	 lambda= 6658.554263	 levenbergIter= 1
iteration= 1	 chi2= 234706314.970484	 time= 0.291987	 cumTime= 0.605915	 edges= 9799	 schur= 0	 lambda= 2219.518088	 levenbergIter= 1
iteration= 2	 chi2= 142146174.348537	 time= 0.277532	 cumTime= 0.883447	 edges= 9799	 schur= 0	 lambda= 739.839363	 levenbergIter= 1
iteration= 3	 chi2= 83834595.145595	 time= 0.279928	 cumTime= 1.16337	 edges= 9799	 schur= 0	 lambda= 246.613121	 levenbergIter= 1
iteration= 4	 chi2= 41878079.903257	 time= 0.273487	 cumTime= 1.43686	 edges= 9799	 schur= 0	 lambda= 82.204374	 levenbergIter= 1
iteration= 5	 chi2= 16598628.119947	 time= 0.272396	 cumTime= 1.70926	 edges= 9799	 schur= 0	 lambda= 27.401458	 levenbergIter= 1
iteration= 6	 chi2= 6137666.739405	 time= 0.272809	 cumTime= 1.98207	 edges= 9799	 schur= 0	 lambda= 9.133819	 levenbergIter= 1
iteration= 7	 chi2= 2182986.250593	 time= 0.275608	 cumTime= 2.25767	 edges= 9799	 schur= 0	 lambda= 3.044606	 levenbergIter= 1
iteration= 8	 chi2= 732676.668220	 time= 0.27199	 cumTime= 2.52967	 edges= 9799	 schur= 0	 lambda= 1.014869	 levenbergIter= 1
iteration= 9	 chi2= 284457.115176	 time= 0.273163	 cumTime= 2.80283	 edges= 9799	 schur= 0	 lambda= 0.338290	 levenbergIter= 1
iteration= 10	 chi2= 170796.109734	 time= 0.272436	 cumTime= 3.07526	 edges= 9799	 schur= 0	 lambda= 0.181974	 levenbergIter= 1
iteration= 11	 chi2= 145466.315841	 time= 0.270867	 cumTime= 3.34613	 edges= 9799	 schur= 0	 lambda= 0.060658	 levenbergIter= 1
iteration= 12	 chi2= 142373.179500	 time= 0.271743	 cumTime= 3.61787	 edges= 9799	 schur= 0	 lambda= 0.020219	 levenbergIter= 1
iteration= 13	 chi2= 137485.756901	 time= 0.276317	 cumTime= 3.89419	 edges= 9799	 schur= 0	 lambda= 0.006740	 levenbergIter= 1
iteration= 14	 chi2= 131202.175668	 time= 0.27323	 cumTime= 4.16742	 edges= 9799	 schur= 0	 lambda= 0.002247	 levenbergIter= 1
iteration= 15	 chi2= 128006.202530	 time= 0.272473	 cumTime= 4.43989	 edges= 9799	 schur= 0	 lambda= 0.000749	 levenbergIter= 1
iteration= 16	 chi2= 127587.860945	 time= 0.272907	 cumTime= 4.7128	 edges= 9799	 schur= 0	 lambda= 0.000250	 levenbergIter= 1
iteration= 17	 chi2= 127578.599359	 time= 0.272402	 cumTime= 4.9852	 edges= 9799	 schur= 0	 lambda= 0.000083	 levenbergIter= 1
iteration= 18	 chi2= 127578.573853	 time= 0.27214	 cumTime= 5.25734	 edges= 9799	 schur= 0	 lambda= 0.000028	 levenbergIter= 1
iteration= 19	 chi2= 127578.573840	 time= 0.272582	 cumTime= 5.52992	 edges= 9799	 schur= 0	 lambda= 0.000018	 levenbergIter= 1
iteration= 20	 chi2= 127578.573840	 time= 0.270247	 cumTime= 5.80017	 edges= 9799	 schur= 0	 lambda= 0.000012	 levenbergIter= 1
iteration= 21	 chi2= 127578.573840	 time= 0.275518	 cumTime= 6.07569	 edges= 9799	 schur= 0	 lambda= 0.000008	 levenbergIter= 1
iteration= 22	 chi2= 127578.573840	 time= 0.793899	 cumTime= 6.86959	 edges= 9799	 schur= 0	 lambda= 0.000044	 levenbergIter= 3
iteration= 23	 chi2= 127578.573840	 time= 0.791677	 cumTime= 7.66127	 edges= 9799	 schur= 0	 lambda= 0.000234	 levenbergIter= 3
iteration= 24	 chi2= 127578.573840	 time= 2.63651	 cumTime= 10.2978	 edges= 9799	 schur= 0	 lambda= 5483030743.383683	 levenbergIter= 10
saving optimization results ...

进程已结束,退出代码0
  • sphere.g2o的可视化

在这里插入图片描述

  • result_lie.g2o的可视化
    在这里插入图片描述
踏破万里无云
关注
  • 1
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
李代数与计算机视觉
01-03
计算机多视角几何中经常看到李代数，指数映射之类的，和我们经常使用的旋转矩阵R不一样，这两个文档很好的说明了他们之间的关系，也可以翻看我的博文讲解http://blog.csdn.net/heyijia0327/article/details/50446140谢谢。
14.视觉SLAM十四讲(高翔第二版)1
08-04
《视觉SLAM十四讲》是高翔撰写的一本关于同时定位与地图构建（Simultaneous Localization and Mapping，简称SLAM）的专业书籍，旨在为读者提供SLAM领域的基础知识和核心算法。该书涵盖了从数学理论到计算机视觉算法...
《视觉 SLAM 十四讲》V2 第 10 讲 后端优化2 简化BA 【位姿图】
最新发布
weixin_46034116的博客
10-18 649
控制 BA 规模：仅保留 离当前时刻最近的 N 个关键帧。在大场景中，大量特征点 会严重降低计算效率，计算量越来越大 ——>滑动窗口法 比较适合VO系统，不适合大规模建图系统。BA能精确地优化每个相机位姿与特征点位置。，推导按照此定义的左乘扰动雅克比矩阵。这样构建的误差理想下是0，因为 ln(BA：带有相机位姿和空间点的图优化。改进： 简化BA 【位姿图】查看待优化的位姿图。第9讲 以BA为主的图优化。如果将位姿图中的误差定义为。本题中 位姿图的误差定义为。位姿图优化【简化的BA】
尝试运行示例：李代数上的位姿图优化
一个搬运知识的笨小孩
05-22 290
尝试把Sophus用到g2o中定义自己的顶点和边 CMakeLists.txt cmake_minimum_required(VERSION 3.13) project(lie_algebra) set(CMAKE_CXX_STANDARD 14) set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so") list(APPEND CMAKE_MODULE_PATH ${lie_algebra_SOURCE_DIR}/cmake_modules) find_
利用李代数做位姿图优化的简单实现
realjc的博客
03-29 508
#include<bits/stdc++.h> #include <ceres/autodiff_cost_function.h> #include <ceres/cost_function.h> #include <ceres/local_parameterization.h> #include <ceres/loss_function.h> #include <ceres/problem.h> #include <ceres.
视觉slam十四讲（2）
liuyangguo1995的博客
06-08 268
目录一、问题二、视觉slam框架三、数学描述四、参数化五、总结Linux实践： 一、问题 我在什么地方？——定位。 周围环境是什么样？——建图。 二、视觉slam框架 ` 图1 视觉SLAM流程图 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机
SLAM中的优化、李群李代数和BA
06-28 2707
Table of Contents 1.SLAM中优化目标函数的表示 2.李群李代数的引入 3.BA 本文为高博《视觉SLAM十四讲》李群李代数部分的学习笔记。 1.SLAM中优化目标函数的表示 SLAM的核心问题就是通过观测数据来得到其对应的位姿，通用的方法就是构建为一个优化问题，然后求解最优的旋转矩阵和平移向量，使得误差最小化。 构建优化问题来求解相机位姿是问题求解的总体思路，但...
SLAM十四讲(第二版)所有第三方库库.zip
03-31
《SLAM十四讲》是一本深入介绍SLAM理论与实践的书籍，第二版则包含了更多更新的内容和技术。这个压缩包文件提供了书中所涉及的一些第三方库，这些库在实现SLAM算法时扮演着至关重要的角色。 1. **Ceres Solver**：...
高翔视觉slam十四讲有完整库的代码文件
09-20
高翔视觉slam十四讲有完整库的代码文件 ： 因为高翔视觉slam十四讲文件里面的，3rdparty文件夹下下载下来是空的，所以这里有专门的已经下载好了的文件夹供大家使用，不用再自己下载了，避免因版本不同而编译错误。...
slam十四讲第二版ch12所需数据集
12-27
总之，通过《SLAM十四讲》第二版的第十二章，读者将深入了解SLAM算法如何利用数据集进行学习和验证。这个过程不仅涉及理论知识，还包括对真实世界数据的理解和处理，这对于任何希望在机器人导航、自动驾驶等领域工作...
高翔视觉slam 14讲 配套代码
07-19
高翔视觉slam 14讲 配套套代码，每一章都有，有助于学习视觉SLAM
《SLAM 14讲》第十讲：后端优化
Simon的博客
04-18 1435
因为《SLAM 14讲》上第十讲代码有点复杂，所以按照书上把解析参数的那个类去掉了，写了一个简单的版本。
实践：g2o位姿图优化，李代数位姿图优化(3)
weixin_46417419的博客
04-25 1270
SLAM十四讲-后端2-ch10-代码注释(位姿图优化)
joun772的博客
11-12 1463
一、李代数上的位姿图优化 源码如下: #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> #includ
slam十四讲(第二版）之最后一讲-小型的salm系统代码分析
yycsin的博客
04-15 487
slam十四讲之最后一讲-小型的salm系统代码分析 Vslam是slam十四讲里面，最后一讲，一个简单的slam系统 本来slam十四讲第一版是放在中间一章的，后面第二版就放在最后一章节了，在系统规模上不但包含前端的VO，也包含后端的一些优化，所以这种方式感觉应该更合理一些。不但考虑到了如何实现VO，也展示了后端优化的一些方法。 代码结构 首先代码结构，包括visualOdometry是一个主函...
《视觉SLAM十四讲》第十讲g2o求解BA问题报错解决办法
yongniao9185的博客
01-06 1505
问题描述： 编译报错如下： /home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp: In function ‘void SetMinimizerOptions(std::shared_ptr<g2o::BlockSolver<g2o::BlockSolverTraits<9, 3> > >&, const BundleParams&, g2o::SparseOptimizer*)’: /home/x
十四讲里面的g2o优化
t_home的博客
11-30 242
123
SLAM第十讲实践：【后端2】基于slambook2的位姿图优化（SE(3)位姿图与李代数上的位姿图优化）
zhangyuanbonj的博客
10-18 721
有关可视化g2o的内容在第六讲已经提到过，安装啥的直接送上传送门。点击optimize优化后为一个球体，可以看作是螺旋上升的位姿。左上角文件添加ch10中的sphere.g2o。那么在g2o包内bin目录下我们。
学习SLAM《视觉SLAM十四讲（高翔著）》 第10讲笔记(BA优化）
开源节流
02-17 459
代码实践 文章目录 10.3实践：g2o 10.4实践：Ceres 程序的详细注释可关注robinhjwy和zkk9527的CSDN博客。 10.3实践：g2o 用g2o求解BA问题。 程序的编译运行方法： mkdir build cd build cmake .. make ./g2o_customBundle -input ../data/problem-16...
视觉slam十四讲 第二版2
08-31
《视觉SLAM十四讲》第二版主要分为三个大部分：激光SLAM，视觉SLAM和激光视觉等多传感器融合SLAM。在激光SLAM部分，书中介绍了激光传感器的原理、产品选型，以及开源激光SLAM系统的介绍、比较和分类。此外，书中还介绍了深度学习在激光SLAM中的应用以及激光SLAM面临的挑战和未来发展。在视觉SLAM部分，书中涵盖了视觉传感器的原理、产品选型，以及开源视觉SLAM系统和视觉惯性系统的介绍、比较和分类。同样地，书中还讨论了深度学习在视觉SLAM中的应用以及视觉SLAM的挑战和未来。最后，在多传感器SLAM系统部分，书中介绍了多传感器之间的内外参标定方法，并从硬件层、数据层和任务层介绍了现有的多传感器融合工具、融合技术和融合产品。书中还探讨了多传感器融合面临的挑战和未来发展。 总而言之，《视觉SLAM十四讲》第二版主要介绍了激光SLAM、视觉SLAM和激光视觉等多传感器融合SLAM的原理、应用、挑战以及未来发展。这本书适合对SLAM感兴趣的读者，无论是从事相关研究还是工程应用的人员都会受益。阅读本书可以帮助读者了解SLAM的基本概念和方法，并深入探讨SLAM在不同传感器和多传感器融合下的应用和挑战。<span class="em">1</span><span class="em">2</span><span class="em">3</span> #### 引用[.reference_title] - *1* [《视觉SLAM十四讲（高翔）》高清中文版PDF 1](https://download.csdn.net/download/weixin_35823755/86294534)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_2"}}] [.reference_item style="max-width: 50%"] - *2* *3* [视觉slam十四讲 第二版 pdf_第二版SLAM上帝视角，它来啦！！！](https://blog.csdn.net/weixin_39620001/article/details/111384898)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_2"}}] [.reference_item style="max-width: 50%"] [ .reference_list ]
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值