关于g2o的使用笔记

G2O (General Graph Optimization)入门及简单使用 g2o全称是General Graph Optimization,也就是图优化,我们在做SLAM后端或者更加常见的任何优化问题(曲线拟合)都可以使用G2O进行处理。 阅读详情

前言

g2o是比较流行的图优化库,在视觉slam中的应用非常广泛,关于如何利用g2o求解BA优化问题,在此作个笔记,以供后面的学习中方便查阅。

一、g2o进行非线性优化

以《视觉slam十四讲》中的求解最小二乘问题为例:

原函数:(1)
二范数:
在这里插入图片描述误差函数:
在这里插入图片描述构建图模型:顶点为待优化变量,边为误差。图的模型大致为下图(有点丑。。。)。

在这里插入图片描述
因为我们需要求的量只有一个,即:vector3d(a,b,c),因此,在这个图中,顶点只有一个,我们通过随机生成n组 x,y,他们之间的误差构成了许许多多的边。构建出了图之后,下面以原书程序进行讲解整个优化的编程实现过程:


  // 构建图优化,先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 定义图的类型,顶点为三维,边为一维
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法,可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
  g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出
  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);
  // 往图中增加边
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      // 观测数值
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵:协方差矩阵之逆
    optimizer.addEdge(edge);
  }
  // 执行优化
  cout << "start optimization" << endl;
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

其实整个图优化的过程大致都是这个框架,下面主要说下注意部分:

v->setId(0);

这句程序是对顶点进行编号,里面的0你可以写成任意的正整数,但是后面设置edge连接顶点时,必须要和这个一致。

  // 往图中增加边
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      // 观测数值
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵:协方差矩阵之逆
    optimizer.addEdge(edge);
  }

该部分程序需要注意的是edge->setVertex(0, v); 其中的0为该边连接的第一个顶点,即前面编号为0的顶点v,因为是一元边,因此只需这一句。根据函数公式:

在这里插入图片描述

可知,y为观测值,故有edge->setMeasurement(y_data[i])。图构建出之后,即可进行迭代求解。

二、g2o进行前端位姿优化

在《视觉slam十四讲》中,前端的位姿优化主要讲了3D-3D的ICP、3D-2D的PNP位姿求解,本文主要讲解ICP的g2o求解。其实,这和前面的非线性优化很相似,两帧图像之间匹配出的3D点的差值构成了该图的一元边,顶点即为两帧之间的运动估计T,也就是相机的位姿,两帧之间优化出一个相机位姿T。

在这里插入图片描述

二范数:
在这里插入图片描述

图模型:
在这里插入图片描述下面为g2o构建图过程以及图优化的代码编写,同样以十四讲书中的程序为例。

 // 设定g2o,构建图模型
  typedef g2o::BlockSolverX BlockSolverType;
  typedef g2o::LinearSolverDense<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);       // 打开调试输出
  
   // vertex
  VertexPose *pose = new VertexPose(); // camera pose
  //两帧优化出一个最优pose  一个edge连接一个vertex
  pose->setId(0);
  pose->setEstimate(Sophus::SE3d());  //李群T
  optimizer.addVertex(pose);

边的部分

  //边的值即为两帧之间匹配点的3D坐标之差
  for (size_t i = 0; i < pts1.size(); i++) {
    EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(
      Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
      //因为是一元边,故只设置连接的第一个顶点
    edge->setVertex(0, pose);
    // P1 = T12P2,故观测值为pts1
    edge->setMeasurement(Eigen::Vector3d(
      pts1[i].x, pts1[i].y, pts1[i].z));
    edge->setInformation(Eigen::Matrix3d::Identity());
    optimizer.addEdge(edge);
  }

对构建的图进行优化

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
  cout << endl << "after optimization:" << endl;
  cout << "T=\n" << pose->estimate().matrix() << endl;

需要注意的是,因为设置的观测值为p1,所以优化的pose为p2到p1的坐标变换。 如果想要求得一个图像序列中,两两帧之间的T,那么只需要循环的调用

void bundleAdjustment(const vector<Point3f> &pts1,const vector<Point3f> &pts2, Mat &R, Mat &t) 

这个函数,传入的参数为两帧之间匹配的3D点对即可。

三、g2o进行后端位姿优化

高翔博士的一起做RGBD-SLAM中有一个是利用g2o对vo进行了一个后端优化的讲解,我们也以此为例。
在前端的vo求解中,利用PNP或者ICP求出了两帧之间的相机运动(局部最优),连接起来即为视觉里程计,但这个过程中存在着误差累积,且一旦出现了错误匹配,里程计的误差将会逐渐增加,将整体的位姿进行优化,有助于误差的消除,达到全局的最优。

原函数:
在这里插入图片描述

二范数:
在这里插入图片描述xi∗表示xi的估计值。

图模型:

在这里插入图片描述
与前面图模型不同的是,后端优化是将所有的pose相连,以第一个pose为初始值,整体进行迭代优化,edge为二元边。

图优化程序如下:

顶点部分:初始化第一个顶点

    /*******************************
    // 新增:有关g2o的初始化
    *******************************/
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> SlamBlockSolver;  //图类型(顶点6维,边6维)
    typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;  //线性求解器类型
    
    //选择优化方法
    auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<SlamBlockSolver>(g2o::make_unique<SlamLinearSolver>()));
    g2o::SparseOptimizer globalOptimizer;  
    globalOptimizer.setAlgorithm( solver );
    // 不要输出调试信息
    globalOptimizer.setVerbose( false );

    // 向globalOptimizer增加第一个顶点
    g2o::VertexSE3* v = new g2o::VertexSE3();
    //第一个顶点为初始值,编号为初始帧currIndex
    v->setId( currIndex );
    v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
    v->setFixed( true ); //第一个顶点固定,不用优化
    globalOptimizer.addVertex( v );

边部分:即为PNP估计出的T,也即相机位姿。
初始化第二个顶点,为当前帧的编号
这部分代码是在一个for循环中,主要就是向图中逐个的加入边和顶点。

        // 向g2o中增加这个顶点与上一帧联系的边
        // 顶点部分
        // 顶点只需设定id即可
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId( currIndex );
        v->setEstimate( Eigen::Isometry3d::Identity() );
        globalOptimizer.addVertex(v);
        // 边部分
        g2o::EdgeSE3* edge = new g2o::EdgeSE3();
        // 连接此边的两个顶点id
        edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
        edge->vertices() [1] = globalOptimizer.vertex( currIndex );
        // 信息矩阵
        Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
        // 信息矩阵是协方差矩阵的逆,表示我们对边的精度的预先估计
        // 因为pose为6D的,信息矩阵是6*6的阵,假设位置和角度的估计精度均为0.1且互相独立
        // 那么协方差则为对角为0.01的矩阵,信息阵则为100的矩阵
        information(0,0) = information(1,1) = information(2,2) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100;
        // 也可以将角度设大一些,表示对角度的估计更加准确
        edge->setInformation( information );
        // 边的估计即是pnp求解之结果
        edge->setMeasurement( T );
        // 将此边加入图中
        globalOptimizer.addEdge(edge);

因为是后端的整体优化,所以需要将全部的相机位姿通过二元边进行连接起来,然后对构建的图进行优化。

        edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
        edge->vertices() [1] = globalOptimizer.vertex( currIndex );

该边连接的第一个顶点为上一帧的T,第二个为当前帧的T。

    globalOptimizer.initializeOptimization();
    globalOptimizer.optimize( 100 ); //可以指定优化步数
    globalOptimizer.clear();

总结

值得注意的是,在利用g2o进行图优化时,我们优化的公式为

在这里插入图片描述
但我们在进行点云拼接时,会想求这样的公式

在这里插入图片描述
中的T,所以需要获得pose->estimate().inverse()进行点云拼接。

参考资料:

《视觉slam十四讲》
一起做RGB-D SLAM (6)

g2o图优化简介与基本使用方法 g2o简介 g2o(General Graphic Optimization)是一个基于图优化的库,将非线性优化与图论结合起来的理论,我们可以利用g2o求解任何可以表示为图优化的最小二乘问题。 图优化就是把优化问题表现成图的方式。图由顶点和边组成,其中顶点表示优化变量,边表示误差项,对任意一个非线性?> 最小二乘问题,我们都可以构建与之对应的图。 (注:这里的图是图论意义上的图,可以用概率论里面的定义,贝叶斯图或因子图。) g2o安装 首先安装g2o的依赖 sudo apt install qt5 阅读详情

相关推荐

《视觉SLAM-非线性优化与g2o-高翔》笔记

1. 从滤波到优化 变量 未知位姿 x1,x2,...,xNx_1, x_2, ..., x_Nx1​,x2​,...,xN​ 未知路标 y1,y2,...,yMy_1, y_2, ..., y_My1​,y2​,...,yM​ 已知输入 uuu 已知观测值 zzz 模型 状态量 x=[x1,x2,...,xN;y1,y2,...,yM]x=[x_1, x_2, ..., x_N; y_1, y_...

sixgod2012的博客 780

【SLAM】G2O优化库超详细解析

通用图优化。为何叫通用呢?G2O的核里带有各种各样的求解器,而它的顶点、边的类型则多种多样。通过自定义顶点和边,事实上,只要一个优化问题能够表达成图,那么就可以用G2O去求解它。常见的,比如bundle adjustment、ICP、数据拟合,都可以用G2O来做。所谓图优化,是把优化问题表现成图的一种方式,这里的图是图论意义上的图。一个图由若干个顶点,以及连着这些顶点的边组成。在这里,我们用顶点表示优化变量,而用边表示残差项。

Yngz_Miao的博客 1万+

利用g2o实现简单BA-视觉SLAM十四讲学习笔记

参考高博博客,见此网址: https://www.cnblogs.com/gaoxiang12/p/5304272.html 实验中选取的结点和边如下: 结点1:相机位姿结点:g2o::VertexSE3Expmap,来自&lt;g2o/types/sba/types_six_dof_expmap.h&gt;; 结点2:特征点空间坐标结点:g2o::VertexSBAPointXYZ,来自&lt;...

weixin_44095441的博客 1744

slam定位学习笔记(七)-g2o学习

根据高博十四讲里面介绍使用g2o的源码分析:

weixin_45572737的博客 2587

RANSAC、ICP、图优化g2o学习笔记

RANSAC随机抽样一致算法(RANdom SAmple Consensus,RANSAC) 参考自wiki: RANSAC WIKI 它采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。 RANSAC是一个非确定性算法,在某种意义上说,它会产生一个在一定概率下合理的结果,而更多次的迭代会使这一概率增加。NOTE : 分成离群(outliers)、内群(inliers) 和普通

csz 3050

图优化g2o教程简介

图优化g2o教程简介,包括g2o间接、如何使用,顶点和边的定义,信息矩阵、鲁棒核函数对于优化的结果的影响。

u010196944的博客 2132

g2o论文阅读笔记

纯粹方便自己理解整理的笔记,以前总是写在纸上,不知不觉都丢了。希望写在博客里,慢慢积累一些。 0、简介 SLAM/BA问题的目标:找到一组参数或状态变量,以最大限度地解释受高斯噪声影响的一组测量值。 这种问题可以被表示为一个图,图中的节点node:待优化的状态变量,图中的边edge:它所连接的两个节点的成对观察。 前人常使用标准方法(如高斯-牛顿、Levenberg-Marquardt、高...

weixin_30325071的博客 412

视觉SLAM笔记(29) g2o

图优化、g2o 的编译与安装、拟合曲线

氢键H-H 1万+

g2o学习笔记

g2o整体框架 综上所述,在g2o中选择优化方法一共需要三个步骤: 选择一个线性方程求解器,从PCG、CSparse、Choldmod三个选一 选择一个BlockSolver 选择一个迭代器,从GN、LM、Doglog中选 自定义顶点(Vertex)和边(Edge) 在整体框架图中,可以看到不管是顶点还是变,都可以说是继承子basexxx这个类的,因此我们在自定义的时候,也可以仿照着继承...

小红鱼的博客 477

g2o学习笔记-orbslam2中如何使用g2o进行非线性优化(三)

g2o学习笔记-orbslam2中如何使用g2o进行非线性优化

cangqiongxiaoye的博客 1257

保姆级教程:在Ubuntu 22.04上,用Xinference为你的RAGFLOW项目接入BGE重排序模型

本教程详细介绍了在Ubuntu 22.04系统上使用Xinference为RAGFLOW项目部署BGE重排序模型的全过程。从环境准备、Xinference服务启动、模型下载到RAGFLOW集成实战,提供了保姆级的步骤指导和常见问题解决方案,帮助开发者高效提升检索结果的精准度。

weixin_29069131的博客 391

G2O学习笔记

G2O 一、概念 G2O(General Graph Optimization)是通用图优化计算库,能将优化问题表达成图,就可以使用G2O进行求解,如常见的Bundle Adjustment,ICP,数据拟合等。 图优化模型中,将待优化的变量作为顶点,条件信息作为边。如上图中,可以解释为三个顶点为不同时刻待优化估计的传感器的位姿,各个边表示传感器位姿量测信息,最终需要估计各个顶点的最优结果。 二、安装使用 G2O库的github仓库:RainerKuemmerle/g2o #g2o安装 #去github下

ZhangP.H的博客 698

ISO15118-20证书结构关系解读(7.3.2.1)

本文主要讲述ISO15118-20:2022标准所有证书结构的关联关系,目的是为后续基于ISO15118-20车桩通讯及PNC充电等提供基础支撑。 本文所有的证书关系图全部来源于ISO15118-20:2022标准规范,在后续的讲解过程会将图的摘取出处给提供出来,便于后期文章和标准对应起来解读。

weixin_40900932的博客 3196

g2o 学习笔记 一

g2o学习笔记

weixin_44278921的博客 766

vue3接入大华摄像头

┃ ┃ ┣ 📜PlaybackModal.vue 回放列表模态框。┃ ┃ ┗ 📜VideoGrid.vue 视频播放区域。┃ ┃ ┣ 📜ControlPanel.vue 控制面板。┃ ┃ ┣ 📜DeviceList.vue 设备列表。┃ ┣ 📂module //大华摄像头文件。

💻 专注 Vue 前端与 Java 后端技术 🌱 目前正在深入 Spring Boot、微服务与性能优化 📝 定期分享实战踩坑经验与学习笔记 🤝 欢迎交流,共同进步 916

强化学习代码实现PPO

梯度更新:通过截断(clipping)限制策略更新幅度,避免策略变化过大影响稳定性。首先是数据预处理,将相关数据(状态、采取的动作、状态奖励、下一个状态)放到gpu上。根据当前状态 state,利用策略网络生成每个动作的概率分布 probs。2. 修改了策略模型的输出,从每个动作的概率分布变成了动作的均值和方差。1. 修改了动作空间,离散空间变成了连续空间,使用正太分布来采样。3. 修改了动作的ratio的计算,从离散分布变成了正太分布计算。Update函数:通过经验缓冲数据来优化策略模型和价值模型。

wlxsp的博客 640

SLAM小白日记——g2o学习笔记

初学SLAM,因为ubuntu中不能使用onenote,所以决定用CSDN来记录自己的学习笔记,发表的目的是想和大家一起分享交流进步,笔记内容多来自各大佬的心得,我也会附上链接,如果有侵权烦请告知。 1、创建一个线性求解器LinearSolver 我们要求的增量方程的形式是:H△X=-b,通常情况下想到的方法就是直接求逆,也就是△X=-H^-1*b。但是当H的维度较大时,矩阵求逆变得很困难,此时...

weixin_45626706的博客 775

告别机载网络通信混乱:利用ARINC825的LCC与带宽管理机制构建高可靠CAN总线

本文深入探讨了ARINC825规范如何通过逻辑通信通道(LCC)和带宽管理机制解决机载CAN总线通信的混乱问题。文章详细解析了ARINC825的核心架构设计、LCC工程实现、时间触发带宽管理等关键技术,展示了其在提升航空电子系统可靠性和实时性方面的显著效果,为现代飞机通信系统提供了高可靠解决方案。

weixin_26878995的博客 241

【g2o】g2o学习笔记 BA相关

g2o

ASUNAchan的博客 458

vue2/3 - 详细对接海康监控实时视频预览和录像回放功能,vue接入使用海康视频监控播放器实时推流等功能,支持hik/rtsp/rtmp/hls/ws取流协议(详细教程与示例代码,一键复制开箱即用

vue2,vue3,海康,vue海康视频监控,海康开放平台,如何接入海康的实时监控,海康威视,Vue实现海康监控视频播放,vue3对接海康视频监控播放、回放的方法流程,实现海康H5视频插件播放视频的实例,web网页接入海康,海康威视的视频vue的录像回放和实时预览,前端vue3项目接入海康威视h5player.js播放器,vue2接入海康威视视频监控平台,使用海康监控H5视频播放,对接海康视频监控,海康视频web插件,web前端在vue中通过海康插件嵌入视频,实现实时预览,vue3海康监控播放器组件,vue

✅ 精选推荐文章 1万+

Unity 工具 之 ZXing QRCode 二维码的生成和识别(可设置生成不同颜色的/带图标的二维码)

Unity 基础 之 ZXing QRCode 二维码的生成和识别(可设置生成不同颜色的/带图标的二维码) 目录 Unity 基础 之 ZXing QRCode 二维码的生成和识别(可设置生成不同颜色的/带图标的二维码) 一、简单介绍 二、实现原理 三、注意实现 四、效果预览 五、ZXing.net ( zxing.unity.dll ) 插件下载 六、实现步骤 七、关键代码 一、简单介绍 Unity 开发中,自己整理的一些游戏开发可能用到的功能,整理归档,方便游...

仙魁XAN 9828

g2o的使用,详细版

https://app.yinxiang.com/shard/s16/nl/38685068/fc17d7ae-5746-4805-95ae-3104fe14ce3a?title=%E5%9B%BE%E8%AE%BA%E4%B8%8EGeneral%20Graph%20Optimize(g2o)

weixin_43492473的博客 1008

Linux 完全卸载 Neovim 的方法:解决权限问题和清除残留文件

Neovim 没有被完全卸载的原因,可能是因为它是通过非官方方式(如手动编译或二进制包)安装的。对于这种情况,使用apt卸载命令无法删除这些文件。因此,需要手动删除和中的相关文件。建议在安装软件时,尽量使用包管理器(如apt)进行安装,以便以后能够方便地卸载。如果你不太熟悉 Linux 系统,避免手动操作系统目录,除非你非常清楚自己的操作。通过上述方法,你应该能够彻底卸载 Neovim 并重新安装。

代码简单说 Vue、JAVA、PHP、Node.js 熟练运用,接口、架构、性能全搞定。 4946
上一篇: Kdevelop中编译ROS程序包的总结
下一篇: ubuntu 16.04 Anaconda下安装tensorflow2.0
slam02∞
博客等级 码龄8年 39粉丝 14原创
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明:

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

余额充值
⌂ 换网址