视觉SLAM ch6代码总结

已于 2023-04-29 22:25:45 修改
阅读量4.6k 收藏 43
点赞数 10
分类专栏： 视觉SLAM 文章标签： 线性代数 slam c++
于 2022-02-24 16:44:15 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_41451702/article/details/122990734
版权

目录

一、手写高斯牛顿法

二、使用Ceres拟合曲线

三、使用g2o进行曲线拟合

一、手写高斯牛顿法

步骤：
1、先根据模型生成x，y的真值，并在真值中添加高斯分布的噪声
2、使用高斯牛顿法进行迭代
3、求解高斯牛顿法的增量方程 

增量方程的推到过程

 图片来源    该公式等同于133页6.41式

Σ 是 高斯噪声的方差

CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(ch6)

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

#opencv
find_package(OpenCV REQUIRED)

#eigen

include_directories(${Opencv_INCLUDE_DIRS}
		      "/usr/inlcude/eigen3")

add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})

代码：

         理一下思路
         要先算出J 矩阵， 利用J 矩阵 算出H矩阵      
         之后解线性方程组 Hx=b
         判断最后的cost满足调节没有，满足就不用迭代   （cost = error * error）

        步骤
         ①定义误差函数error    yi=y_data[i]    error=yi-exp(ae*x*x+be*x+ce)//用真实值减去估计值得到误差函数
         ②J矩阵是误差函数对各个进行优化的系数的导数所组成的列向量

J[0] =  -xi  * xi * exp(ae * xi * xi + be * xi + ce);    //   d(error)  /d(ae)  
J[1] = -xi * exp(ae * xi * xi + be * xi + ce);           //    d(error) /d(be)
J[2] = -exp(ae * xi * xi + be * xi + ce);                //    d(error) /d(ce)

         ③然后是关于 H 矩阵在程序中的表达形式（看下图，这里出现的 w_sigma 和inv_sigma 是程序开始定义好的）
         

          g(x)  也是同理  g = - inv_sigma  * inv_sigma * error * J  (error是误差函数，也就是高斯牛顿法中的 f(x)  )。

        ④求解 H x = g

        用cholesky分解 （ch3课后题有6种方法求解线性方程组）

        ⑤判断cost的值是否大于lastcost,

        如果本次迭代误差大于上次迭代误差，迭代结束。

（1）w_sigma 和 inv_sigma

设定噪声服从的正态分布的sigma值 double w_sigma = 1.0;

计算sigma的倒数，之后用于计算信息矩阵 double inv_sigma = 1.0 / w_sigma;

（2）rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数

rng.gaussian(w_sigma * w_sigma)为随机数产生高斯噪声

w_sigma*w_sigma是噪声方差

//y=真实函数生成再加上高斯噪声
double  y = exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma);
//这里把每个自变量所对应的真实值（y）算了出来，用作观测值

（3）isnan()函数

判断是否是非法数字， 是非法数字返回真，nan全称为not a number
非法数字包含：负数开方，负数求对数，及0/0，0*inf，inf/inf，inf-inf等未定式

如果方程无解，那么dx[0]是非法字符nan，退出迭代
（4）chrono库

这里可以用c++11的新特性auto定义变量，写代码的时候不用那么麻烦

#include<chrono>
auto t1 = chrono::steady_clock::now();
auto t2 = chrono::steady_clock::now();
auto time_used = t2-t1;
cout<<"迭代所消耗时间 :"<<chrono::duration<double> (time_used).count()<<"秒"<<endl;

c++的优化库：Ceres库 和 基于图优化的g2o库。

二、使用Ceres拟合曲线

Ceres是一个广泛使用的最小二乘问题求解库。

Ceres的求解过程包括代价函数的构建、最小二乘问题的构建以及最小二乘问题的求解。

1.代价函数的构建(cost function)：详细内容看书，代价函数 y-exp(a*x*x+b*x+c)

2.最小二乘问题的构建：

最小二乘问题的构建主要采用Ceres::Problem类进行：

Problem::AddResidualBlock( )
AddResidualBlock()顾名思义主要用于向Problem类传递残差模块的信息，函数原型如下，传递的参数主要包括代价函数模块、损失函数模块和参数模块。

ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function, 
                                          LossFunction *loss_function, 
                                          const vector<double *> parameter_blocks)
  • cost_function：代价函数
  • loss_function：损失函数，若构建的问题为无约束的最小二乘问题，则传入参数为nullptr
  • parameter_blocks：参数块，也即最终求解的参数

 代价函数CostFunction的求导模型:

较为常用的包括以下三种：

     1.自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式，最常用的求导方式。
     2.数值导数（NumericDiffCostFunction）：由用户手动编写导数的数值求解形式，通常在残差函数的计算使用无法直接调用的库函数，导致调用AutoDiffCostFunction类构建时使用；但手动编写的精度和计算效率不如模板类，因此不到不得已，官方并不建议使用该方法。
     3.解析导数（Analytic Derivatives）：当导数存在闭合解析形式时使用，用于可基CostFunciton基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则同样不建议使用。

可以看出，ceres官方极力推荐用户使用自动求导方式AutoDiffCostFunction，这里也主要以AutoDiffCostFunction为例说明。

AutoDiffCostFunction为模板类，构造函数如下：

ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(
                            CostFunctor* functor);

模板参数依次为仿函数（functor）类型CostFunctor，残差维数residualDim和参数维数paramDim，接受参数类型为仿函数指针CostFunctor*。

  • CostFunctor：即为第一步构建的代价函数
  • residualDim：残差块维度，也即模型的输出维度
  • paramDim：参数块维度，也即模型的输入维度
  • functor：指针，为代价函数的构造 

 对于本代码可以进行以下构建：

ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i]))
//1 是问题中的误差ei 3 是曲线的参数

最后对于该问题，构建残差项的配置如下：

//  残差项配置
ceres::Problem problem;
problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
            nullptr,
            abc);

此处，未用到损失函数（无约束）故而传入nullptr；参数块为三维矩阵abc

注意：

由于在优化过程中，我们不希望因为程序的误操作导致操作符()重载的内容被修改，因此需要为函数体加上const关键字修饰。同理，在残差的计算过程中，为了避免除ceres优化之外的误操作引起待优化变量的改变，需要同时使用const关键字修饰参数类型和参数名保证类型和内容均不变；而residual只需要保证类型不变，参数每次都是可变的，因此只需要使用const修饰类型T即可。

3.最小二乘的求解

使用ceres::Solve进行求解，其函数原型如下：

void Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary)
  • options：求解器的配置，求解的配置选项
  • problem：求解的问题，也即我们第二步构建的最小二乘问题
  • summary：求解的优化信息，用于存储求解过程中的优化信息

对求解器的配置介绍：

  • minimizer_type：迭代的求解方式，可选如下：
    • TRUST_REGION：信赖域方式，默认值
    • LINEAR_SEARCH：线性搜索方法
    • 参数通常保持默认值即可
  • trust_region_strategy_type：信赖域策略，可选如下：
    • LEVENBERG_MARQUARDT，列文伯格-马夸尔特方法，默认值
    • DOGLEG：Dog-leg法，俗称狗腿法
  • linear_solver_type：求解线性方程组的方式
    • DENSE_QR：QR分解法，默认值，用于小规模最小二乘求解
    • DENSE_NORMAL_CHOLESKY和 SPARSE_NORMAL_CHOLESKY：CHolesky分解，用于有稀疏性的大规模非线性最小二乘求解
    • CGNR：共轭梯度法求解稀疏方程
    • DENSE_SCHUR和 SPARSE_SCHUR：SCHUR分解，用于BA问题求解
    • ITERATIVE_SCHUR：共轭梯度SCHUR，用于求解BA问题
  • num_threads：求解使用的线程数
  • minimizer_progress_to_stdout：是否将优化信息输出至终端
    • bool类型，默认为false。若设置为true输出根据迭代方法而输出不同：
    • ①信赖域方法
      • cost：当前目标函数的数值
      • cost_change：当前参数变化量引起的目标函数变化
      • |gradient|：当前梯度的模长
      • |step|：参数变化量
      • tr_ratio：目标函数实际变化量和信赖域中目标函数变化量的比值
      • tr_radius：信赖域半径
      • ls_iter：线性求解器的迭代次数
      • iter_time：当前迭代耗时
      • total_time：优化总耗时
    • ②线性搜索方法
      • f：当前目标函数的数值
      • d：当前参数变化量引起的目标函数变化
      • g：当前梯度的模长
      • h：参数变化量
      • s：线性搜索最优步长
      • it：当前迭代耗时
      • tt：优化总耗时

//  Options
ceres::Solver::Options options;
//  cholesky分解
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
//  输出优化信息
options.minimizer_progress_to_stdout = true;

 Solver::Summary

Solver::Summary为求解器以及各个变量的信息，常用成员函数如下：

  • BriefReport()：输出单行的简单总结；
  • FullReport()：输出多行的完整总结。
//优化信息
ceres::Solver::Summary summary;
            
//开始优化
ceres::Solve(options, &problem,&summary);
            
//输出结果
cout<<summary.BriefReport()<<endl;
cout<<"最优结果: a,b,c = "<<endl;
for(auto i : abc){
    cout<<i <<endl;
    }

参考：

Ceres详解（二） CostFunction_yuntian_li的博客-CSDN博客_ceres costfunction

Ceres 库：基础使用，以手写高斯-牛顿法为例_JasonLi的博客-CSDN博客

这里复习下模板内容

模板

c++除了面向对象思想外还有一种编程思想叫 泛型编程，主要利用的技术就是模板。c++提供了两种模板机制：函数模板和类模板。

1.函数模板：

//语法
template<typename T>
写一个函数

template: 声明创建模板

typename : 表示其后面的符号是一种数据类型，可以用class代替

T ：通用的数据类型

例子：交换两个数

template<typename T>
void my_swap(T &a , T &b){
    T temp = a;
    a = b;
    b = temp;    
}

模板的局限性：如果是自定义数据类型，比如 T 是Person类，需要用具体话方式做特殊实现

例子：判断两个Person类对象p1和p2是否相对

//两数是否相等
class Person{
public:  
    Person(string name,int age){
        this->m_name = name;
        this->m_age = age;
    }
    
    string m_name;
    int m_age;
   
};
template<typename T>
void my_Compare(T &a , T &b){
    if(a==b)
    {
        cout<<"两数相等"<<endl;
    }
    else{        
        cout<<"两数不相等"<<endl;
    }
}
void test01(){
    Person p1("Tom",10);
    Person p2("Jerry",10);
    my_Compare(p1,p2);   //程序报错  
    int  a = 10;
    int  b = 10;
    my_Compare(a,b);

}

报错内容：无法比较两个Person类型

 解决方法：① 重载 == 

                  ②利用具体化Person实现代码

template<> void my_Compare(Person &a , Person &b){
    if(a.m_name == b.m_name && a.m_age == b.m_age){
        cout<<"相等"<<endl;
    }else{
        cout<<"不相等"<<endl;
    }
}

2.类模板：

//类模板
template<class NameType , class AgeType>
class Student
{
public:

    Student(NameType name , AgeType age){
        this->m_name = name;
        this->m_age = age;
    }
    NameType m_name;
    AgeType m_age;
};

void test02(){
    Student<string, int> p1("qqq",18);
    Student<string, int> p2("www",17);
    cout<<p1.m_name<<"  "<<p1.m_age<<endl;
}

类模板：使用时必须指定类型 

三、使用g2o进行曲线拟合

g2o是一个基于图优化的库，是一种将非线性优化与图论结合起来的理论。

特点：
1.顶点为优化变量，边为误差项。

2.待估计的参数构成顶点，观测数据构成了边。

3.误差定义在边内，边附着在顶点上。

4.误差关于顶点的偏导数定义在边里的雅克比矩阵J上，而顶点的更新通过内置的oplusImpl函数实现更新。

5.在视觉slam中，相机的位姿与观测的路标构成图优化的顶点；相机的运动、路标的观测构成图优化的边。

学习g2o优化前建议先去看看g2o类图，这个非常重要。

 详细讲解参考高博文章

在g2o中选择优化方法需三个步骤：

1.选择一个线性方程求解器(LinearSolver):PCG, CSparse, Choldmod;
2.选择一个BlockSolver, 包含SparseBlockMatrix，用于计算稀疏雅可比和海塞；BlockSolver用于计算迭代过程中最关键的一步HΔx=−b为一个线性方程的求解器。
3.选择一个迭代策略:GN, LM, Doglog。(一般选取前两种方法之一)

关于g2o头文件的介绍 

#include<g2o/core/base_vertex.h>      //g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include<g2o/core/base_unary_edge.h>  //g2o边（edge）头文件
#include<g2o/core/block_solver.h>  //求解器头文件
#include<g2o/core/optimization_algorithm_levenberg.h>      //列文伯格——马尔夸特算法头文件
#include<g2o/core/optimization_algorithm_gauss_newton.h>  //高斯牛顿算法头文件
#include<g2o/core/optimization_algorithm_dogleg.h> //dogleg算法头文件
#include<g2o/solvers/dense/linear_solver_dense.h>//线性求解

代码步骤：

  1. 定义顶点的类型
  2. 定义边的类型
  3. 构建图优化

    1. 配置块求解器BlockSolver,

    2. 配置线性方程求解器LinearSolver,从PCG, CSparse, Choldmod、Dense中选一个作为求解方法

    3. 配置总求解器solver，并从GN,LM,Dogleg优化算法中选一个，再用上述块求解器BlockSolver初始化

    4. 配置稀疏优化器SparseOptimizer

    5. 往图中增加顶点和边，并添加到SparseOptimizer

  4. 启动优化

流程：

1. 定义顶点的类型

 顶点主要的成员函数（位于 g2o/core/base_vertex.h 中）： 

// 返回优化之后顶点的值.
const EstimateType& estimate() const { return _estimate;}

自定义顶点时，需要重写4个函数:

 virtual void setToOriginImpl();                                // 顶点重置函数
 virtual void oplusImpl(const double* update);                  // 顶点更新函数
 virtual bool read(istream &in);                                // 读操作
 virtual bool write(ostream &out) const;                        // 写操作

setToOriginImpl为顶点的重置函数，用来设定待优化变量的初始值；

oplusImpl为顶点更新函数，主要用于在使用增量方程计算出增量 Δ x 后，用来更新x (k+1) = x(k) +  Δ x;                                                               

read，write 函数可以不进行覆写，仅仅声明一下就可。

 自己定义顶点一般是下面的格式：

class myVertex: public g2::BaseVertex<Dim, Type>  
//Dim 并非是顶点的维度，而是其在流形空间（manifold）的最小表示 
//Type是待估计顶点的类型，本例中顶点类型(优化变量)是三维变量，因此type = Eigen::Vector3d;如果优化变量用李代数表示，type= Sophus::SE3d
{
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      virtual void read(std::istream& is)  {}
      virtual void write(std::ostream& os)  const {}

      virtual void setOriginImpl () override
      {
          _estimate = Type();
      }
      virtual void oplusImpl(const double* update) override
      {
          _estimate += /*update*/;
      }
  }

例1：位姿当作顶点

/// 位姿顶点
typedef Sophus::SE3d SE3;
class VertexPose : public g2o::BaseVertex<6, SE3> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

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

    /// left multiplication on SE3
    virtual void oplusImpl(const double *update) override {
        Vec6 update_eigen;
        update_eigen << update[0], update[1], update[2], update[3], update[4],
            update[5];
        _estimate = SE3::exp(update_eigen) * _estimate;
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }
};

例2  路标点(landmark)当作顶点

// 路标顶点
typedef Eigen::Matrix<double, 3, 1> Vec3;
class VertexXYZ : public g2o::BaseVertex<3, Vec3> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    virtual void setToOriginImpl() override { _estimate = Vec3::Zero(); }

    virtual void oplusImpl(const double *update) override {
        _estimate[0] += update[0];
        _estimate[1] += update[1];
        _estimate[2] += update[2];
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }
};

2.定义边的类型

 边的重要的成员变量和函数：

_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，比如一元边的话，_vertices[] 的大小为1，二元边的话，_vertices[] 的大小为2
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆

自定义边时，需要重写4个函数:

 virtual void computeError();                                   // 误差计算函数
 virtual void linearizeOplus();                                 // 计算雅克比矩阵 
 virtual bool read(istream &in);                                // 读操作
 virtual bool write(ostream &out) const;                        // 写操作

computeError 函数是使用当前顶点的值,计算观测值与真实值之间的误差，是边的误差计算函数。

linearizeOplus 函数是在当前顶点的值下，该误差对优化变量的偏导数，即jacobian矩阵。这个函数计算了每条边相对于顶点的雅克比

 自己定义边的模板

 class myEdge: public g2o::BaseUnaryEdge<errorDim, errorType, Vertex1Type>
//errorDim 是int类型 ，表示误差项的维度
//errorType 误差项的数据类型
//Vertex1Type 就是前面自定义顶点的类型 myVertex
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW      
      myEdge(){}     
      virtual bool read(istream& in) {}
      virtual bool write(ostream& out) const {}      
      virtual void computeError() override
      {
          // ...
          _error = _measurement - Something;
      }      
      virtual void linearizeOplus() override
      {
          _jacobianOplusXi(pos, pos) = something;
          // ...         
          /*
          _jocobianOplusXj(pos, pos) = something;
          ...
          */
      }      
      private:
      // data
  }

例3：估计例1节点的一元边

/// 仅估计位姿的一元边
typedef Eigen::Matrix<double, 2, 1> Vec2;
class EdgeProjectionPoseOnly : public g2o::BaseUnaryEdge<2, Vec2, VertexPose> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjectionPoseOnly(const Vec3 &pos, const Mat33 &K)
        : _pos3d(pos), _K(K) {}

    virtual void computeError() override {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
        SE3 T = v->estimate();
        Vec3 pos_pixel = _K * (T * _pos3d);
        pos_pixel /= pos_pixel[2];
        _error = _measurement - pos_pixel.head<2>();
    }

    virtual void linearizeOplus() override {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
        SE3 T = v->estimate();
        Vec3 pos_cam = T * _pos3d;   //Pc
        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double X = pos_cam[0];
        double Y = pos_cam[1];
        double Z = pos_cam[2];
        double Zinv = 1.0 / (Z + 1e-18);
        double Zinv2 = Zinv * Zinv;
        //187页7.46
        _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2,
            -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv,
            fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2,
            -fy * X * Zinv;
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }

   private:
    Vec3 _pos3d;
    Mat33 _K;
};

例4 估计例1位姿节点和例2路标点节点的二元边

/// 带有地图和位姿的二元边
class EdgeProjection
    : public g2o::BaseBinaryEdge<2, Vec2, VertexPose, VertexXYZ> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    /// 构造时传入相机内外参
    EdgeProjection(const Mat33 &K, const SE3 &cam_ext) : _K(K) {
        _cam_ext = cam_ext;
    }

    virtual void computeError() override {
        const VertexPose *v0 = static_cast<VertexPose *>(_vertices[0]);
        const VertexXYZ *v1 = static_cast<VertexXYZ *>(_vertices[1]);
       //相机位姿
        SE3 T = v0->estimate();
        // v1->estimate()路标点在世界坐标系下的坐标；_cam_ext是相机的外参矩阵，T是相机位姿
        Vec3 pos_pixel = _K * (_cam_ext * (T * v1->estimate()));   
        pos_pixel /= pos_pixel[2];
        _error = _measurement - pos_pixel.head<2>();
    }

    virtual void linearizeOplus() override {
        const VertexPose *v0 = static_cast<VertexPose *>(_vertices[0]);
        const VertexXYZ *v1 = static_cast<VertexXYZ *>(_vertices[1]);
        SE3 T = v0->estimate();
        Vec3 pw = v1->estimate();
        Vec3 pos_cam = _cam_ext * T * pw;
        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double X = pos_cam[0];
        double Y = pos_cam[1];
        double Z = pos_cam[2];
        double Zinv = 1.0 / (Z + 1e-18);
        double Zinv2 = Zinv * Zinv;
        _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2,
            -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv,
            fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2,
            -fy * X * Zinv;
        //式7.48
        _jacobianOplusXj = _jacobianOplusXi.block<2, 3>(0, 0) *
                           _cam_ext.rotationMatrix() * T.rotationMatrix();
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }

   private:
    Mat33 _K;
    SE3 _cam_ext;
};

 3.构建图优化

3.1 配置块求解器  BlockSolver 

// 矩阵块求解器
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;                   
//pose 维度为 6, landmark 维度为 3

3.2 配置线性方程求解器 LinearSolver

// 线性求解器
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

 PCG, CSparse, Choldmod、Dense四种线性方程求解器

  • LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自 LinearSolver
  • LinearSolverCSparse：使用 CSparse 法，继承自LinearSolverCCS；
  • LinearSolverCholmod：使用 sparse cholesky 分解法，继承自LinearSolverCCS；
  • LinearSolverDense ：使用 dense cholesky 分解法，继承自 LinearSolver

3.3 配置总求解器solver，并从GN,LM,Dogleg优化算法中选一个，再用上述块求解器BlockSolver初始化

// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

3.4 配置稀疏优化器SparseOptimizer

g2o::SparseOptimizer optimizer;     // 稀疏优化器，图模型
optimizer.setAlgorithm(solver);     // 设置求解器
optimizer.setVerbose(true);         // 打开调试输出

另一种写法：

//构建图优化
//1. 构建快求解器 BlockSolver   
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1>> BlockSolverType;
            
//2.配置线性方程求解器LinearSolver
unique_ptr<BlockSolverType::LinearSolverType>  linearSolver  ( new g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>());

//3. 配置总求解器solver 
unique_ptr<BlockSolverType> solver_ptr  (new BlockSolverType(move(linearSolver)));    //矩阵块求解器
g2o::OptimizationAlgorithmGaussNewton *solver  = new g2o::OptimizationAlgorithmGaussNewton(move(solver_ptr));

3.5 往图中增加顶点和边，并添加到SparseOptimizer 

// 往图中增加顶点
CurveFittingVertex *vertex = new CurveFittingVertex();
vertex->setEstimate(待优化值);                            // 设置优化初始值
vertex->setId(0);                                         // 设置顶点ID
optimizer.addVertex(vertex);                              // 向稀疏优化器添加顶点

// 往图中增加边
for (int i = 0; i < N; i++){
  CurveFittingEdge *edge = new CurveFittingEdge(边);
  edge->setId(i);
  edge->setVertex(0, vertex);                   // 设置连接的顶点
  edge->setMeasurement(观测数值);               // 设置观测数值 可以理解为误差项中的观测值
  edge->setInformation(信息矩阵);               // 设置信息矩阵：协方差矩阵之逆 该矩阵的维度和误差项有关，如果误差是三维的 信息矩阵的就是Eigen::Matrix3d::Identity()
  optimizer.addEdge(edge);                     // 向稀疏优化器添加边
}

4 启动优化        

optimizer.initializeOptimization();                       // 设置优化初始值
optimizer.optimize(10);                                   // 设置优化次数
estimate = vertex->estimate();                            // estimate就是要求的优化变量

参考：

g2o定义顶点

g2o定义边 

g20曲线拟合实例

g20 Practice

orb-slam2 代码解读（三）

曲线模型顶点和边代码遇到的问题：

1. EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Eigen库为了使用SSE加速，在内存上分配了128位的指针，涉及字节对齐问题。在生成定长的Matrix或Vector对象时，需要开辟内存，调用默认构造函数，通常x86下的指针是32位，内存位数没对齐就会导致程序运行出错。而对于动态变量(例如Eigen::VectorXd)会动态分配内存，因此会自动地进行内存对齐。

解决方法：在public下写一个宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 

参考文章：Eigen字节对齐问题

                  Eigen内存对齐

2. static_cast

static_cast 运算符 | Microsoft Docs

运算符可用于将指向基类的指针转换为指向派生类的指针

CMakeLists.txt

这里因为使用g2o库，执行  cmake  ..  以后由于CMakeLists里书写错误会报错，参考文章

#视觉SLAM书上的程序使用的g2o版本比较旧了，使用的是c++11版本的g20。而自己在编译g2o的时候编译的是最新版本的g2o，里面大量使用了c++14标准库的一些新特性，比如std::index_sequence等等。而书上的CMakeLists.txt默认使用的是c++11进行cmake编译
set(CMAKE_CXX_STANDARD 14)

#g2o  使用它需要写依赖
list(APPEND CMAKE_MODULE_PATH /home/hope/Downloads/g2o/cmake_modules ) 
set(G2O_ROOT /usr/local/include/g2o) 
find_package(G2O REQUIRED)

add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})
雨幕丶
关注
  • 10
    点赞
  • 踩
  • 43
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
专栏目录
高翔视觉slam 14讲 配套代码
07-19
高翔视觉slam 14讲 配套套代码，每一章都有，有助于学习视觉SLAM
视觉slam14讲第六章代码阅读
weixin_42001184的博客
07-19 282
摘录自高翔视觉slam14讲第6章《非线性优化》 #include <iostream> #include <g2o/core/g2o_core_api.h> #include <g2o/core/base_vertex.h> #include <g2o/core/base_unary_edge.h> #include <g2o/core/block_solver.h> #include <g2o/core/optimization_algo
视觉SLAM14讲——ch6源码编译
linxinlrh的博客
01-29 281
视觉SLAM14讲——ch6编译
[学习笔记-SLAM篇]视觉SLAM十四讲ch6
warningm_dm的博客
06-25 991
有些别的事情中断了学习，现在重新拾起来哈。 果然，很多事情还是一鼓作气的好，不然节奏会乱掉。 注： 1）学习视频：【高翔】视觉SLAM十四讲。 视觉SLAM十四讲第6讲6.1 理论部分6.2 实践部分 第6讲 6.1 理论部分 本讲主要是状态估计的问题，大佬以非线性优化为主，也是现在视觉SLAM主流的方向。 1）状态估计 前面我们得到了运动方程（由SE(3)SE(3)SE(3)描述【讲3】，李代数优化【讲4】）和观测方程（由针孔模型给定【讲5】），其中涉及到相机位姿xxx、地标点yyy（前2者合称位姿
第6讲
qq_40213457的博客
06-26 249
#include &lt;iostream&gt;#include &lt;opencv2/core/core.hpp&gt;#include &lt;ceres/ceres.h&gt;#include &lt;chrono&gt; using namespace std; // 代价函数的计算模型struct CURVE_FITTING_COST{    CURVE_FITTING_COST (...
视觉SLAM十四讲配套代码
12-02
这个压缩包文件"slambook2"很可能包含了书中的所有实例代码和相关资源，帮助读者更直观地学习和探索视觉SLAM。 SLAM是机器人和自动驾驶领域的重要技术，它允许无人设备在未知环境中实时构建地图并估计自身位置。...
视觉slam十四讲
02-22
《视觉SLAM十四讲》是一本深入探讨视觉Simultaneous Localization And Mapping（SLAM）技术的资源分享。SLAM是机器人领域的一个核心问题，它涉及如何让机器人在未知环境中同时定位自身位置并构建环境地图。这门技术...
视觉SLAM十四讲-ch13代码祥读（设计SLAM系统）
06-20
1.对视觉SLAM十四讲——ch13代码中的所有文件进行了详细的阅读 2. 主函数的阅读 3. config配置文件 4. visual_odometry.cpp视觉里程计文件 5. frontend.cpp前端文件（重要文件1） 6. backend.cpp后端文件（重要文件2...
slambook2-ch13-VO-comment:视觉SLAM十四讲ch13中VO的详细注释版本，基本覆盖所有理解难点，尤其对入门学者十分友好！！
05-30
slambook2-ch13-VO（详细注释版）视觉SLAM十四讲ch13中VO的详细注释版本，基本覆盖所有理解难点，尤其对入门学者十分友好！！slambook2——VO（详细注释）相较于第一版十四讲，第二版中内容更加丰富，对一些知识点...
视觉SLAM十四讲CH6代码解析及课后习题详解
生活如此美好，致敬每一个热爱“热爱”的人！
10-28 3575
Ipopthttps://github.com/coin-or/Ipopt Overview - NLopt DocumentationNonehttps://nlopt.readthedocs.io/en/latest/ 【论文阅读1】SLAM++: SLAM at the Level of Objects - 知乎去年读了不少论文，但是感觉还是记录下来印象更深刻一些。就在知乎记录吧，内容基本都是是翻译过来的，由于时间有限，没有仔细润色，可能不太通顺，以后再慢慢改。 这里记录第一篇文章： R. F..
slam十四讲-ch6-非线性优化（包含手写高斯牛顿、使用g2o库、使用ceres库三种方法的源码详细注释）
joun772的博客
10-29 1619
一、自写高斯-牛顿法 该程序是要进行一个非线性优化，对非线性函数的系数进行优化 y=exp(ax2+bx+c) 给定初始的系数 ae,be,ce(估计的) ar,br,cr(真实的) 源码如下： // // Created by wenbo on 2020/10/28. // //该程序是手写高斯牛顿法，对一个简单的非线性函数进行优化 //y=exp(ax2+bx+c) #include<iostream> #include<opencv2/opencv.hpp> #in
slam14讲ch6，运行时找不到共享库
你好哇
10-31 825
安装好g2o，编译通过slam14讲ch6里g2o优化代码，但是运行时报错。 /home/ljj/workspace/my_slambook_ws/ch6/g2o_curve_fitting/build-g2o_curve_fitting--Debug/curve_fitting: error while loading shared libraries: libg2o_core.so: cann
Ceres使用
小红鱼的博客
11-09 4253
定义：ceres是一款非线性优化问题的数值求解器。 1. CostFunction 输入为自变量，输出为残差。首先要定义一个用于求解残差的struct，并在内定义重载函数operator()： template bool operator() (const T* const x, T* residual ) const 2. LocalParameterization 输入为全局参数，输出...
Ceres-Solver 从入门到上手视觉SLAM位姿优化问题
CG-SpatialAI
03-24 7821
文章目录概述入门Part 1: Cost FunctionPart 2: AddResidualBlockPart 3: Config & SolveSimple Example Code应用: 基于李代数的视觉SLAM位姿优化 概述 Ceres Solver is an open source C++ library for modeling and solving large, co...
Ceres 中的线性求解器类型（linear_solver_type）
一抹烟霞的博客
03-29 665
Ceres Solver中的线性求解器类型(linear_solver_type)有多个选项，包括： DENSE_QR：使用稠密QR分解方法求解线性方程组。适用于内存足够的小规模问题，求解速度较快。 DENSE_SCHUR：使用Schur补方法进行求解。适用于大规模问题，但内存消耗较大。 SPARSE_NORMAL_CHOLESKY：使用稀疏Cholesky分解方法求解正规方程。适用于具有稀...
视觉SLAM十四讲学习笔记——第七讲 视觉里程计（2）
weixin_61294345的博客
11-20 2146
视觉SLAM的前端也就是视觉里程计实际上要解决的问题是根据匹配的特征点估计相机运动。根据不同的已知条件，选择不同的方法。 1.针对单目相机2D-2D：对极几何 对于单目相机，前后两幅图像之间存在着对极几何约束，我们掌握的信息只有两幅图像中特征点的图像坐标，因此相机运动估计问题变为以下两步： （1）根据匹配点坐标估计本质矩阵E：因为本质矩阵E仅与旋转矩阵R和平移向量t有关，估计本质矩阵的方法包括传统的八点法和RANSAC（随机抽样一致性算法）及其改进算法MASC...
视觉十四讲：第六讲_手写高斯牛顿法
penuel
05-23 360
1.优化问题： \(y=exp(ax^{2}+bx+c)+w\),由y和x，求解a,b,c 误差为：\(e_{i}=y_{i}-exp(ax_{i}^{2}+bx_{i}+c)\) 误差项对每一个待估计量进行求导： \(\frac{\partial e_{i}}{\partial a}=-x^{2}_{i}exp(ax^{2}_{i}+bx_{i}+c)\)\(\frac{\partia...
Cerse入门(一)Helloword解析与自动求导
dushib的博客
01-24 1245
简介： Ceres是谷歌开发的广泛使用的最小二乘问题求解库。用户只需要按照一定的步骤定义待解的优化问题然后交给求解器计算。最小二乘问题一般格式如下： 使用Ceres求解非线性优化问题，一共分为三个部分： 1、 第一部分：构建cost fuction，即代价函数，也就是寻优的目标式。这个部分需要使用仿函数（functor）这一技巧来实现，做法是定义一个cost function的结构体，在结构体内重载（）运算符，具体实现方法后续介绍。 2、 第二部分：通过代价函数构建待求解的优化问题。 3、 第三部分
第6讲 非线性优化（偏实践）
YMWM_的博客
05-09 504
目录1 手写高斯牛顿法2 1 手写高斯牛顿法   对于如下测量方程， y=eax2+bx+c+w y=e^{ax^2+bx+c}+w y=eax2+bx+c+w 其中yyy表示对状态(a,b,c)T(a,b,c)^T(a,b,c)T的测量；www为噪声，它服从均值为0，方差为1的高斯分布。同时，有如下100个数据点， xdata[100]={0,0.01,0.02,…xi…，0.99} x_{data}[100] = \{0, 0.01, 0.02, …x_i…， 0.99 \} xdata​[100]=
视觉slam十四讲ch7代码
最新发布
07-27
抱歉，我无法提供具体的代码，但我可以告诉你《视觉SLAM十四讲》第七章主要涵盖的内容。第七章主要介绍了基于特征点的视觉SLAM算法。在这一章中，作者详细讲解了特征点的提取与描述，以及如何使用RANSAC算法进行特征...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值