传统ORB-SLam中位姿优化中雅克比矩阵讲解

[学习SLAM]协方差矩阵与雅克比矩阵-(相关性/变换基)||(微分倒数J/H) 协方差矩阵/协方差矩阵的意义 基本的统计学概念,即均值、标准差、方差和协方差 协方差的定义 对于一般的分布,直接代入E(X)之类的就可以计算出来了,但真给你一个具体数值的分布,要计算协方差矩阵,根据这个公式来计算,还真不容易反应过来。网上值得参考的资料也不多,这里用一个例子说明协方差矩阵是怎么计算出来的吧。 记住,X、Y是一个列向量,它表示了每种情况下每个样本可能出现的数。比如给定则X表示x轴可能出现的数,Y表示y轴可能出现的。 用中文来描述,就是:协方差(i,j)=(第i列的所有元素-第i. 阅读详情

由于之前的鱼眼orbslam只有单目部分,所以在优化时也只是用了单目位姿优化和三维坐标点优化,并没有将双目的优化添加进去,不知道是否对结果有影响;

这里添加双目的优化部分,主要是将添加雅克比矩阵;

orbslam中的优化部分使用了g2o库,具体的详细讲解可以参考https://zhuanlan.zhihu.com/p/58521241 讲解的很详细

这里不加赘述,主要从单目的雅克比矩阵讲解,延伸到双目的雅克比矩阵,以及带有鱼眼EUCM模型的鱼眼双目的雅克比矩阵的推导

(雅克比矩阵即为误差函数关于优化项的导数,所以depends on 优化项的个数, 雅克比矩阵的个数也不同)

1. 单目的雅克比矩阵

首先以单目位姿优化为例, 因为只对位姿做优化,所以这里采用一元边BaseUnaryEdge​ 进行边的定义,如下:

BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

误差函数e的公式不在推导,可以参见SLAM十四讲P163的讲解,这里假设空间点P 经过位姿变换后在相机坐标系下的坐标为P' =[X',Y',Z']T
根据相机的投影公式,将坐标P‘ 转换到像素坐标系下公式如图:

 

 接下来要计算的误差函数就是将 实际的特征点的二维坐标 与该投影值u,v 进行比较,求差,因为理想状态下两者应该是完全一致的,但是由于位姿的误差等,导致二者不同,因此误差就是需要经二者的差值最小化,也就是最小化重投影误差函数;

首先考虑误差函数e关于扰动量的导数,利用链式法则可以写成如下形式:

因为上面u,v的公式已经明确,那么e关于P’ 的导数就可以写成

(公式1)

同时,第二项P‘ 关于扰动的导数按照之前的推导为:

(这里需要注意,在g2o中的形式一般为[-P'^ , I ], 需要在写雅克比矩阵的时候注意一下)

这里P '^ 为三维坐标P‘ 坐标的反对成矩阵,因此 —P'^ 为在反对成矩阵的基础上添加负号即可;

             |0   z    -y|

-P’^ =  |z   0    -x|

            |-y   x    0|

最终的误差关于扰动量的导数公式如下:

该公式也就对应了g20库中types_six_dof_expmap.cpp 文件中 EdgeSE3ProjectXYZOnlyPose类 对应的linearizeOplus() 中的雅克比矩阵

  //这里要注意的就是g20中是P'^在前,I矩阵在后
  _jacobianOplusXi(0,0) =  x*y*invz_2 *fx;
  _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
  _jacobianOplusXi(0,2) = y*invz *fx;
  _jacobianOplusXi(0,3) = -invz *fx;
  _jacobianOplusXi(0,4) = 0;
  _jacobianOplusXi(0,5) = x*invz_2 *fx;

  _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
  _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
  _jacobianOplusXi(1,2) = -x*invz *fy;
  _jacobianOplusXi(1,3) = 0;
  _jacobianOplusXi(1,4) = -invz *fy;
  _jacobianOplusXi(1,5) = y*invz_2 *fy;

因为只优化位姿,所以这里定义了一元边,如果是将位姿和三维坐标点一起优化,这里需要定义二元边

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
其中的参数分别是 定义误差函数的维数,坐标形式,顶点0的类型(这里是三维点),顶点1的类型(这里为相机的位姿)

定义完成二元边后,只需要添加误差函数e关于三维空间点P(XYZ)的导数即可,根据SLAM 十四讲的推导,这里只需啊哟对之前P’的求导乘以R矩阵即可,如下:

具体的代码部分可以参考 g2o 库中types_six_dof_expmap.cpp 文件中EdgeSE3ProjectXYZ::linearizeOplus() 函数:只需要添加一个雅克比项即可,但是需要注意的这里要先添加对三维点的优化,再添加对位姿的优化:

Matrix<double,2,3> tmp;
  tmp(0,0) = fx;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*fx;

  tmp(1,0) = 0;
  tmp(1,1) = fy;
  tmp(1,2) = -y/z*fy;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

2. 普通相机双目优化公式:

普通相机的双目优化时,在误差项求解时,除了要优化左相机中的特征点的测量值和观测值之间的差值,还包括右目中对应特征点的横坐标的观测值与测量值之间的关系;

因此测量值相关公式由两个变成三个,如下:

公式改变带来的主要是公式1中误差函数关于相机坐标系下的坐标点的导数,雅克比矩阵也由最开是2x6变成3x6的矩阵

只需要按照这个顺序依次求导带入即可,对应的相关代码为:g2o 库中types_six_dof_expmap.cpp 文件中 EdgeStereoSE3ProjectXYZ::linearizeOplus() 函数对应的雅克比矩阵的下半部分:

  _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2;
  _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2;
  _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2);
  _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3);
  _jacobianOplusXj(2,4) = 0;
  _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2;

同理,对坐标点的优化也要相应的变成3x3;

LM算法参数优化实例——残差函数、雅可比矩阵 【目标】:使用LM算法估计y=exp(at2+bt+c)y=exp(at^2+bt+c)y=exp(at2+bt+c)参数a, b, c。 已知量:ttt及与之对应的含有噪声的量测值y^\hat{y}y^​;同时,模型已知(仅未知参数)。 仿真数据生成 取参数为a=1.0, b=2.0, c=1.0 double a=1.0, b=2.0, c=1.0; // 真实参数值 ... 阅读详情

相关推荐

图优化库g2o中的节点(Vertex)和边(Edge)

图优化库g2o中的节点(Vertex)和边(Edge)

weixin_44884561的博客 2405

ORB-SLAM2从理论到代码实现(十):Bundle Ajustment(下) Jacobian矩阵推导

在上一节内容中,我们讲到使用牛顿法、高斯牛顿法、LM法进行非线性优化,在优化过程中需要计算雅可比矩阵。 一、计算雅可比矩阵 上面公式中表示第i个相机观测的第j个目标的像素坐标和重投影坐标之间的误差,也即是待优化的目标函数。我们知道求雅可比矩阵是函数对其中的变量进行求导,而在该目标函数中的变量是相机的位姿和目标的位置。 其中的维度是2x6 维: 这个推导可以由链式法则得到。 的维度...

qq_20123207的博客 1984

SLAM中常用的雅克比矩阵J

1. 知识背景 1.1 李群SE(3) 与 李代 SO(3)的转换关系 2.常用的雅克比矩阵J 2.1 旋转点的左扰动雅可比 2.2旋转点的右扰动雅可比 2.3旋转连乘的雅可比 2.4旋转连乘的雅可比

惊鸿一博 1731

视觉SLAM位姿优化时误差函数雅克比矩阵的计算

文章目录概述雅克比矩阵 推导Jacobian 0直接法特征点法Jacobian 1Jacobian 2Jacobian 3总结直接法特征点法注意事项参考文献 概述 SLAM,即 同时定位与建图,视觉SLAM的 定位 即 求取相机位姿(旋转和平移 [Rt][\mathbf{R} \quad \mathbf{t}][Rt]);在SLAM中,我们一般使用 李代数 ξ\boldsymbol{\xi}ξ 来...

CG-SpatialAI 8874

别再为PCB和散热器文件发愁了!手把手教你用ADS导出DWG文件给工厂(附单位转换避坑指南)

本文详细解析了从ADS设计到PCB/散热器加工的DWG文件导出全流程,重点介绍了单位转换、分层导出策略及散热器文件处理等关键步骤,帮助射频工程师避免加工误差和返工。特别提供了mil与mm单位转换的精确处理方法,确保设计文件与工厂加工需求无缝对接。

weixin_30814329的博客 617

解决Surface更新后Bitlocker无限循环的问题

在Surface系统更新以后,出现要求输入Bitlocker恢复密钥的问题。已经输入正确密钥以后,依然无法正常进入Windows操作系统,一旦重启依然循环Bitlocker要求输入密钥的界面。 一旦到登录界面,点击“登录”之后又跳转回登录界面.,但就是无法进入桌面。 解决方案: 拒绝暴力重装!这个问题是Bitlock把盘给锁了,所以要解锁 重启之后,直接点击右下角“跳过这个驱动器” 选择“疑难解答”,然后进入命令提示符 先查看判断现在是不是被锁了: 输入: manage-bde -status c

premierapp的博客 1万+

视觉SLAM小知识——雅可比矩阵(Jacobian metrix)

大草坪 4629

利用WI生成无线信道数据

选取DeepMIMO的一个射线追踪场景O1(Outdoor 1)Scenario,工作频率为3.5GHz。有两条街和一个十字路口的户外场景18个基站和超过一百万个用户18个基站(BSs):BS1-BS18.每个基站的高度是8米每个基站的天线阵列是isotropic天线三个统一的x-y用户网格用户的总计数量为:1184923每个用户网格中的第一个用户总是(x,y)坐标最低的用户 所有用户的高度是2m每个用户的每个天线阵列是isotropic天线定义射频追踪场景。

bingbingaidaima的博客 5619

SLAM14讲学习笔记(十一)g2o图优化中的要点与难点(前端VO中雅克比矩阵的定义)

使用g2o优化的前提是,需要对各种误差的理解足够充足。我将雅克比矩阵的详细解析,写在了这里:点击查看 在源码和自定义的类中,各种雅克比矩阵有的是写在源码里的,有的是需要自己修改和定义的。然而在实践中发现,雅克比矩阵的定义往往不一,容易使初学者摸不着头脑。因此,这篇文章我从代码角度出发,简单解析一下g2o定义时候的要点,和内置的雅克比矩阵的书写方式。 在g2o中,内置了三大类: VertexS...

微鉴道长的博客 6396

保姆级教程:用YOLOv11和CRNN从零搭建一个车牌识别系统(附完整代码)

本文提供了一份详细的YOLOv11和CRNN实战指南,教你从零搭建高精度车牌识别系统。内容包括环境配置、数据集处理、模型训练与优化,以及系统集成与性能优化技巧,附完整代码实现。特别介绍了YOLOv11在车牌检测中的创新应用和CRNN在车牌识别中的关键技术。

weixin_28358083的博客 324

从零开始安装 Windows 11 和 Ubuntu 22.04 双系统

本文详解Windows与Ubuntu 22.04双系统安装流程:需提前确认BIOS模式(UEFI/GPT或Legacy/MBR),关闭BitLocker加密、设备加密及快速启动;推荐Rufus/Ventoy制作启动盘。安装时需规划分区方案(建议单独划分EFI系统分区),注意引导器安装位置选择。双系统启动通过BIOS引导顺序管理,安装后需同步Windows与Linux系统时间。教程适配主流UEFI+GPT设备,强调操作前数据备份及Windows更新策略调整,避免兼容性问题。

萑澈的博客 3781

史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证}文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证 该篇博客主要讲解→雅可比矩阵定义、推导、以及其应用,并且有相应的示例解读 雅可比在函数行列式方面有一篇著名的论文:《论行列式的形成与性

江南才尽江南山,年少无知年少狂! 5142

PCIE的MSI中断处理机制与优化策略

PCIe设备的。

xiaoheshang_123的博客 532

ORB3-SLAM 知识梳理

目的:利用IMU测量的角速度、加速度,根据上一时刻导航信息,推算出当前时刻导航信息,包括姿态解算、速度解算、位置解算- 方法:由姿态、速度、位置的微分方程,推导出它们的解,并转变成离散时间下的近似形式,从而可以在离散时间采样下,完成导航信息求解。- 优点:IMU不受外界信号干扰,可以得到稳定、平滑的导航结果- 缺点:误差随时间累计,一般时间越长,累计误差越大。因此需要融合,而导航解算及其误差分析,是融合的基础当我们在跟踪丢失的情况下,不至于在唯一的一张地图中疯狂重定位。

weixin_43391596的博客 5486

AgentScope深入分析-ReActAgent

ReActAgent 是 AgentScope 框架中最核心的智能体实现,它完美诠释了"推理-行动"(Reasoning-Acting)循环的精髓。本文将深入分析 ReActAgent 的类继承关系、核心执行流程,以及状态管理、钩子机制、实时介入等关键技术点。通过阅读本文,你会理解智能体如何与模型交互、如何执行工具、如何处理中断,以及这些机制背后的设计考量。模块化:通过组合不同的组件(模型、格式化器、工具集、记忆)来构建智能体透明性:所有关键流程都对开发者可见,可以精确控制可扩展性。

845

【Block总结】DiffAttention,差分注意力融入CLIP架构,抑制噪声,增强对相关特征的关注|即插即用

DiffCLIP通过微小的结构改动实现了显著的性能提升,为多模态模型的轻量化与高效化提供了新范式。

AI浩 1567

G2O入门

G2O优化方法使用流程: 1、构建模型的顶点: oplusImpl( const double* update ) ——》顶点的更新函数 2、构建模型的边:void computeError() ——》计算曲线模型误差,边的误差计算函数,取出边所连接的顶点的当前估计值,与该顶点的观测值进行比较 3、生...

qq_29230261的博客 759

VB摄像头监控

该VB模块可对摄像头捕获的画面进行录像等,可在创建的窗口进行控制

视觉SLAM理论入门——(8)视觉里程计之特征点法—PnP

PnP是求解3D到2D点对运动的方法

floatinglong的博客 1886

Camera2Demo

在Camera2 的基础上进行封装,使得Camera2一堆初始化逻辑与Activity分离,降低耦合度,完成预览拍照并保存的操作

锂电池充电器电路设计 里面有电路

锂电池充电器电路设计 里面有电路 3.7V升压到5V电路设计。

雅克比矩阵(上)-----雅克比推导

雅克比矩阵(上)-----雅克比推导 1、前言 回顾前面几期的内容,在第一期中介绍了机器人的正/逆运动学建模,正运动学解决的问题是如何从关节空间的关节变量描述操作空间的位姿,反之则是逆运动学的内容。将操作空间和关节的空间的关系用以下关系式进行表达。 机器人正/逆运动学始终在解决上面这个公式,已知末端的位姿,求解关节变量,或者已知关节变量,确定末端的位姿,这些描述的是静态位置之间的关系,属于静态运动学问题。 在第二、三期介绍了机器人动力学建模,使用是拉格朗日法,由于需要计算的每个连杆的动能,因此在第三期介

weixin_38291293的博客 6635

visio安装包

Microsoft Office Visio 是一款专业的制图软件。利用经过更新的形状、协作工具和数据链接图表,创建专业级图表以简化复杂的信息。

SlamBook14讲-ICP

记录SlamBook十四讲自己的学习内容

jiahao_1002的博客 588

SLAM优化位姿时,误差函数的雅可比矩阵的推导。

转载请说明出处: http://blog.csdn.net/zhubaohua_bupt/article/details/74011005 视觉SLAM的核心是求取相机的位姿并建图。 在优化位姿时,其思想是构造一个关于位姿变化的误差函数,当这个误差函数最小时,认为此时估计的位姿最优。 无论是直接法还是特征点法,位姿的迭代优化都是是求解一个最小二乘问题, 在优化...

weixin_30416871的博客 1221

几种常用的优化方法梯度下降法、牛顿法、)

几种常用的优化方法 1. 前言 熟悉机器学习的童鞋都知道,优化方法是其中一个非常重要的话题,最常见的情形就是利用目标函数的导数通过多次迭代来求解无约束最优化问题。实现简单,coding 方便,是训练模型的必备利器之一。   2. 几个数学概念 1) 梯度(一阶导数) 考虑一座在 (x1, x2) 点高度是 f(x1, x2) 的山。那么,某一点的梯度方向是在该点坡度

u011584941的专栏 2万+

g2o中 EdgeSE3Expmap类型Jacobian的计算

位姿优化的时候,两个顶点的类型是SE3,涉及到的误差雅克比是pose error对pose的求导,里面有些知识值得注意,故记录下来。 前期准备 重新翻看Ethan Eade的《Lie Groups for 2D and 3D Transformations》,发现他的文档早已有相关推导。比如针对两个SO3乘积对其中一个求导: 比如同理两个SE3乘积对其中一个求导: 上面这两个

知行合一 1万+

重投影误差和光度误差与雅可比矩阵

2*6的那个雅克比矩阵是误差对于扰动的小量位姿的偏导数

weixin_38216573的博客 2344

3D-2D点对 直接线性变换DLT PnP射影变换 BA最小化重投影误差 求旋转平移矩阵 R t

*单目相机 经过对极几何初始化(八点法 、单应矩阵)求解一些3D点后 * 或则 双目相机 深度相机可以直接得到 三维点 不需要初始化得到最开始的三维点 *可使用3D点-2D点对 使用 直接线性变换DLT(Direct Linear Transform) n点透视问题(PnP) 最小二乘最小化重投影误差 非线性优化算法G2O

万有文的博客 6111

《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导

在上一篇文章《视觉SLAM十四讲》第十章g2o代码的简化中,求解BA的雅克比矩阵,我直接使用了g2o的数值导,因为这个雅克比矩阵自己写还真的很容易出错,不过还是有一些小伙伴想要了解具体的形式,所以在这一篇文章中会具体进行推导。 1 雅克比矩阵数学推导 如果对数学推导不感兴趣的,可以直接跳到代码那里看,另外这里会使用李群和李代数的知识,不清楚的可以看一下《...

JohnnyYeh的博客 4468
上一篇: EUCM鱼眼相机模型详解
下一篇: 舒尔补理论Schur Compliment
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明:

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

余额充值
⌂ 换网址