手撕 视觉slam14讲 ch7 / pose_estimation_3d2d.cpp (2)

news2025/7/19 10:45:05

上一篇文章中: 手撕ch7/pose_estimation_3d2d(1),我们调用了epnp的方法进行位姿估计,这里我们使用非线性优化的方法来求解位姿,使用g2o进行BA优化

首先介绍g2o:可参考:g2o详细介绍

1.构建g2o图优化思路:

  • 步骤一: 创建线性方程求解器,确定分解方法
// 每个误差项优化变量维度为3,误差值维度为1
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  
// 创建一个线性求解器 LinearSolver,采用 dense cholesky 分解法
Block::LinearSolverType* linearSolver 
    = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 
  • 步骤二: 构造线性方程的矩阵块,并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );

BlockSolver 内部包含 LinearSolver,用上面定义的线性求解器 LinearSolver 来初始化,前面已经给定了优化变量的维度;

  • 步骤三: 创建总求解器 solver,并从 GN, LM, DogLeg 中选一个,再用上述块求解器 BlockSolver 初始化
g2o::OptimizationAlgorithmLevenberg* solver = 
	new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
  • 步骤四: 创建稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 用前面定义好的求解器作为求解方法:
optimizer.setVerbose( true );       // 打开调试输出
  • 步骤五: 定义图的顶点和边,并添加到 SparseOptimizer 优化器中
// 创建一个顶点
CurveFittingVertex* v = new CurveFittingVertex(); 
// 初始化顶点的值
v->setEstimate( Eigen::Vector3d(0,0,0) );
// 设置顶点的编号
v->setId(0);
// 向图中添加顶点
optimizer.addVertex( v );
      
for ( int i=0; i<N; i++ )    // 往图中增加边
{
    // 创建一条边
    CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
    // 设置边的 id
    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 );
}
  •  步骤六: 设置优化参数,开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100); // 迭代次数
对应原文完整代码如下:
void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {

  // 步骤一:构建图优化,先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  //步骤二: 线性求解器类型
  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);       // 打开调试输出

  //步骤五: 定义图的顶点和边
  // vertex 定义顶点
  VertexPose *vertex_pose = new VertexPose(); // 定义 相机位姿 为顶点
  vertex_pose->setId(0);// 设置顶点的编号
  vertex_pose->setEstimate(Sophus::SE3d());// 初始化顶点的值
  optimizer.addVertex(vertex_pose);// 向图中添加顶点

  // K 相机内参
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges 定义 边
  int index = 1;
  // 往图中增加边
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);// 创建一条边
    edge->setId(index);// 设置边的 id
    edge->setVertex(0, vertex_pose);// 设置边连接的顶点
    edge->setMeasurement(p2d);  // 设置观测数值
    edge->setInformation(Eigen::Matrix2d::Identity());// 设置信息矩阵:协方差矩阵之逆
    optimizer.addEdge(edge);// 将边添加到图中
    index++;
  }

  //步骤六:设置优化参数,开始优化
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  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 << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
  pose = vertex_pose->estimate();
}

其中我们需要关注如何定义顶点(Vertex)和边(edge)

2. 顶点(Vertex):

g2o 提供了一个比较通用的适合大多数情况的模板类 BaseVertex<D, T>,其中D 是 int 类型,表示顶点 Vertex 的最小维度,比如 3D 空间中旋转是 3 维的,那么这里 D=3;
但是在源码注释中说 D 并非是顶点(更确切的说是状态变量)的维度,而是其在流形空间(manifold)的最小表示(SO3->so3,SE3->se3).

T 是待估计 Vertex 的数据类型,比如用四元数表达三维旋转的话,T 就是 Quaternion 类型

我们自定义一个顶点需要重写以下函数

class myVertex: public g2::BaseVertex<Dim, Type>
{
public:
  // 类成员变量如果是固定大小对象需要加上以下的宏定义
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 构造函数
  myVertex(){}
      
  // 读写函数
  virtual void read(std::istream& is) {}
  virtual void write(std::ostream& os) const {}

  // 重置函数
  virtual void setOriginImpl()
  {
      _estimate = Type();
  }
    
  // 更新函数
  virtual void oplusImpl(const double* update) override
  {
      _estimate += /*update*/;
  }
}
对应原文代码:
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> { //优化变量是 6 自由度的李代数
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 //重置,设定被优化变量的原始值
  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }
  /// left multiplication on SE3 //更新
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;  // 左乘更新 SE3 - 旋转矩阵R
  }
  virtual bool read(istream &in) override {} //存盘
  virtual bool write(ostream &out) const override {}//读盘
};

3. 边(edge)

我们一般使用的类是 BaseUnaryEdge,BaseBinaryEdge,BaseMultiEdge 分别表示一元边,两元边,多元边(位于 g2o/g2o/core/base_edge.h 中),类似于顶点,他们又继承自OptimizableGraph::Edge (位于 g2o/g2o/core/optimizable_graph.h 中),hyper_graph::Edge(位于g2o/g2o/core/hyper_graph.h 中)

一元边表示只连接一个顶点,二元边表示连接两个顶点,多元边表示连接 3 个或以上顶点。

主要参数有:D, E, VertexXi, VertexXj

  • D 是 int 型,表示测量值的维度;
  • E 表示测量值的数据类型;
  • VertexXi,VertexXj 分别表示不同顶点的类型。

自定义边需要重写以下成员函数:最重要的是误差计算函数computeError(),计算雅克比矩阵linearizeOplus() 两个函数

class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
      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
}
对应原文代码:
// 仅估计位姿的一元边
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  //构造函数
  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

  // 误差计算函数
  virtual void computeError() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);//将3D世界坐标转为相机像素坐标
    pos_pixel /= pos_pixel[2];
    _error = _measurement - pos_pixel.head<2>(); //误差=测量值-投影得到的值
  }

  // 计算雅克比矩阵 
  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;//相机坐标系下空间点的坐标=相机位姿 * 空间点的坐标
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    // 2*6的雅克比矩阵
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

  //读写操作
  virtual bool read(istream &in) override {}
  virtual bool write(ostream &out) const override {}

private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1102892.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

解决 MyBatis 一对多查询中,出现每组元素只有一个,总组数与元素数总数相等的问题

文章目录 问题简述场景描述问题描述问题原因解决办法 问题简述 笔者在使用 MyBatis 进行一对多查询的时候遇到一个奇怪的问题。对于笔者的一对多的查询结果&#xff0c;出现了这样的一个现象&#xff1a;原来每个组里有多个元素&#xff0c;查询目标是查询所查的组&#xff0c;…

【数据结构】线性表(一)线性表的定义及其基本操作(顺序表插入、删除、查找、修改)

目录 一、线性表 1. 线性表的定义 2. 线性表的要素 二、线性表的基本操作 三、线性表的顺序存储结构 1. 定义 2. 顺序表的操作 a. 插入操作 b. 删除操作 c. 查找操作 d. 修改操作 e. 代码实例 一、线性表 1. 线性表的定义 一个线性表是由零个或多个具有相同…

TCP/IP网络分层模型

TCP/IP当初的设计者真的是非常聪明&#xff0c;创造性地提出了“分层”的概念&#xff0c;把复杂的网络通信划分出多个层次&#xff0c;再给每一个层次分配不同的职责&#xff0c;层次内只专心做自己的事情就好&#xff0c;用“分而治之”的思想把一个“大麻烦”拆分成了数个“…

行业追踪,2023-10-17

自动复盘 2023-10-17 凡所有相&#xff0c;皆是虚妄。若见诸相非相&#xff0c;即见如来。 k 线图是最好的老师&#xff0c;每天持续发布板块的rps排名&#xff0c;追踪板块&#xff0c;板块来开仓&#xff0c;板块去清仓&#xff0c;丢弃自以为是的想法&#xff0c;板块去留让…

华为云云耀云服务器L实例评测|使用sysbench对云耀云服务器mysql的性能测试

目录 引言 1 在centos上安装mysql 1.1 在云服务器上安装 Docker 1.2 在 Docker 中运行 MySQL 容器 2 安装sysbench并进行性能测试 2.1 安装和配置 sysbench 2.2 运行 sysbench 性能测试 3 分析测试结果 3.1 运行结果 3.2 对运行结果进行翻译 3.3 性能分析 4 清理…

AIGC - 入门向量空间模型

文章目录 向量和向量空间向量的运算什么是向量空间&#xff1f;向量空间的几个重要概念向量之间的距离曼哈顿距离&#xff08;Manhattan Distance&#xff09;欧氏距离&#xff08;Euclidean Distance&#xff09;切比雪夫距离&#xff08;Chebyshev Distance&#xff09; 向量…

qml加载ttf字体库

1,下载获取ttf文件 iconfont-阿里巴巴矢量图标库 字体图标下载 - FontAwesome 字体图标中文Icon 2,添加到项目文件 3,项目添加字体库 #include <QGuiApplication> #include <QQmlApplicationEngine> #include <QFontDatabase> #include <QDebug>in…

Error: GlobalConfigUtils setMetaData Fail Cause:java.lang.NullPointerException

文章目录 1、在开发中会出现这样的错误。2、其次&#xff0c;再看其他错误&#xff1a; 1、在开发中会出现这样的错误。 完整错误&#xff1a;Caused by: com.baomidou.mybatisplus.core.exceptions.MybatisPlusException: Error: GlobalConfigUtils setMetaData Fail ! Cause…

白银现货K线走势图分析

K线图又称蜡烛图、阴阳烛&#xff0c;这一理论起源于日本&#xff0c;是最古老的技术分析方法。在众多的现货白银技术分析方法中&#xff0c;K线分析是核心和根本&#xff0c;因为很多的技术分析方法的分析要素和计算方式&#xff0c;都是来自K线中的四个价格。 面对同样一张的…

C++进阶篇1---继承

一、继承的概念和定义 1.1概念 继承机制是面向对象程序设计使代码可以复用的最重要的手段&#xff0c;它允许程序员在保持原有类特性的基础上进行扩展&#xff0c;增加功能&#xff0c;这样产生新的类&#xff0c;称为派生类。继承呈现了面向对象程序设计的层次结构&#xff…

Leetcode刷题详解——长度最小的子数组

1. 题目链接&#xff1a;209. 长度最小的子数组 2. 题目描述&#xff1a; 给定一个含有 n 个正整数的数组和一个正整数 target 。 找出该数组中满足其总和大于等于 target 的长度最小的 连续子数组 [numsl, numsl1, ..., numsr-1, numsr] &#xff0c;并返回其长度**。**如果不…

OpenHarmony页面级UI状态存储:LocalStorage

LocalStorage 是页面级的 UI 状态存储&#xff0c;通过 Entry 装饰器接收的参数可以在页面内共享同一个 LocalStorage 实例。LocalStorage 也可以在 UIAbility 内&#xff0c;页面间共享状态。 本文仅介绍 LocalStorage 使用场景和相关的装饰器&#xff1a;LocalStorageProp 和…

Qt系列-常用控件使用整理

1、QMainWindow介绍 菜单栏最多只有一个 //菜单栏创建 菜单栏最多只能有一个QMenuBar*bar menuBar();//将菜单栏放入到窗口中setMenuBar(bar);//创键菜单QMenu*fileMenubar->addMenu("文件");QMenu*editMenubar->addMenu("编辑");//创建菜单项QActi…

apk和小程序渗透

apk和小程序域服务器通信使用的还是http协议&#xff0c;只是使用了加密。只要可以获取到http的请求报文&#xff0c;就可以回归到web渗透的层面。apk和小程序的渗透很复杂&#xff0c;涉及逆向时要进行脱壳&#xff0c;脱壳后反编译了&#xff0c;源代码没做加密就能直接逆向出…

基于springboot实现家具网站设计与实现平台项目【项目源码+论文说明】计算机毕业设计

摘要 随着移动互联网技术的深入发展&#xff0c;电子商务也不断的完善&#xff0c;线上销售额不断提高&#xff0c;网络消费成为人民日常生活的一部分。并且随着电子商务的发展&#xff0c;也呈现出多元化方向&#xff0c;各种农村电商、生鲜电商、家具电商等&#xff0c;带动…

Location的匹配

nginx的正则表达式: ^:字符串的起始位置 $:字符窜的结束位置 *:匹配所有 :匹配前面的字符一次或者多次 ?:匹配前面的字符0次或者1次 .:任意单个字符 {n}:连续重复出现n次。 {n,m}:连续重复出现n-m次 [a-Z0-9A-Z] [C]:匹配单个字符c ():分组 |:或 一 Location的分类&#xff1a…

笔试强训day01

文章目录 易错点1、字符串格式打印%m.ns\n&#xff08;m&#xff0c;n为整数&#xff09;易错点2、指针的理解易错点3、编程题&#xff1a;组队竞赛 易错点1、字符串格式打印%m.ns\n&#xff08;m&#xff0c;n为整数&#xff09; 正确答案&#xff1a;B。 #include<iostre…

小程序如何设置配送方式

配送方式是一个非常重要的功能&#xff0c;它直接关系到用户的购物体验和商家的运营效率。小程序提供了灵活的配送方式设置&#xff0c;可以根据不同的需求进行系统级别和单个商品的设置。 首先&#xff0c;我们来看系统级别的配送方式设置。在小程序管理员后台->配送设置…

模板学堂|DataEase协助电商企业开展用户运营

DataEase开源数据可视化分析平台于2022年6月正式发布模板市场&#xff08;https://dataease.io/templates/&#xff09;。模板市场旨在为DataEase用户提供专业、美观、拿来即用的仪表板模板&#xff0c;方便用户根据自身的业务需求和使用场景选择对应的仪表板模板&#xff0c;并…

【Java 进阶篇】深入理解 JavaScript DOM Node 对象

在前端开发中&#xff0c;与HTML文档进行交互是一项基本任务。文档对象模型&#xff08;Document Object Model&#xff0c;简称DOM&#xff09;为开发者提供了一种以编程方式访问和操作HTML文档的方式。DOM的核心是节点&#xff08;Node&#xff09;对象&#xff0c;它代表了文…