1 注

高博SLAM 十四讲 P186 中介绍了点的视觉重投影误差相对各个优化变量的雅克比
这儿做一个大概的公式介绍

1.1 投影误差公式

我们选择定义：（与高博书上相反，VINS确是这样定义的，为了与代码对应，我们也这样）
误差 = 理论预测值 - 观测值
十四讲是使用像素平面下的像素坐标做差，我们也为了与VINS对应，使用归一化坐标系的点做差

我们记世界坐标系下一点Pw=[XYZ]P_w=\begin{bmatrix}X\\Y\\Z\end{bmatrix}Pw=⎣⎡XYZ⎦⎤ 通过 TwcT_{wc}Twc转到相机坐标 Pc=[xyz]P_c=\begin{bmatrix}x\\y\\z\end{bmatrix}Pc=⎣⎡xyz⎦⎤ ，再转到归一化坐标系ppp，ppp为理论预测值:

Pc=Rcw(Pw+tcw)=[xyz]P_c=R_{cw}(P_w+t_{cw})=\begin{bmatrix}x\\y\\z\end{bmatrix}Pc=Rcw(Pw+tcw)=⎣⎡xyz⎦⎤
p=[x/zy/z1]p\;=\;\begin{bmatrix}x/z\\y/z\\1\end{bmatrix}p=⎣⎡x/zy/z1⎦⎤
且在相机归一化坐标系下有 ppp 对应的观测值p′=[x′/z′y′/z′1]p^{'} = \begin{bmatrix}x^{'}/z^{'}\\y^{'}/z^{'}\\1\end{bmatrix}p′=⎣⎡x′/z′y′/z′1⎦⎤

投影误差即为：
e=[x/zy/z]−[x′/z′y′/z′]e=\begin{bmatrix}x/z\\y/z\end{bmatrix} - \begin{bmatrix}x^{'}/z^{'}\\y^{'}/z^{'}\end{bmatrix}e=[x/zy/z]−[x′/z′y′/z′]
这样的观测点ppp与路标点PwP_wPw点对，就能构成一项投影残差，实际情况下，就是有很多对这样的点对，组成多项投影误差，然后我们通过调整相机位姿TwcT_{wc}Twc使得整体的误差最小化，这个时候的位姿也就是最优化的位姿，这就是构建最小二乘问题去优化变量。

1.2 投影误差相对于待优化变量的雅克比

待优化的变量即为相机的位姿TwcT_{wc}Twc 由旋转与平移构成，可以用李代数 ξ\xiξ 6维向量表示，也可以用四元数与平移量表示 [tQ]\begin{bmatrix}t&Q\end{bmatrix}[tQ]

十四讲使用的是李代数表示，误差相对于李代数的雅克比为：
∂e∂δξ=∂e∂Pc∂Pc∂δξ\frac{\partial e}{\partial\delta\xi}=\frac{\displaystyle\partial e}{\displaystyle\partial P_c}\frac{\displaystyle\partial P_c}{\displaystyle\partial\delta\xi} ∂δξ∂e=∂Pc∂e∂δξ∂Pc
∂e∂Pc=[∂(x/z)∂x∂(x/z)∂y∂(x/z)∂z∂(y/z)∂x∂(y/z)∂y∂(y/z)∂z]2×3=[1/z0−x/z201/z−y/z2]2×3\frac{\displaystyle\partial e}{\displaystyle\partial P_c}={\begin{bmatrix}\frac{\partial(x/z)}{\partial x}&\frac{\displaystyle\partial(x/z)}{\displaystyle\partial y}&\frac{\displaystyle\partial(x/z)}{\displaystyle\partial z}\\\frac{\displaystyle\partial(y/z)}{\displaystyle\partial x}&\frac{\displaystyle\partial(y/z)}{\displaystyle\partial y}&\frac{\displaystyle\partial(y/z)}{\displaystyle\partial z}\end{bmatrix}}_{2\times3}={\begin{bmatrix}1/z&0&-x/z^2\\0&1/z&-y/z^2\end{bmatrix}}_{2\times3} ∂Pc∂e=⎣⎢⎡∂x∂(x/z)∂x∂(y/z)∂y∂(x/z)∂y∂(y/z)∂z∂(x/z)∂z∂(y/z)⎦⎥⎤2×3=[1/z001/z−x/z2−y/z2]2×3
∂Pc∂δξ=[I3×3−Pc^]3×6\frac{\displaystyle\partial P_c}{\displaystyle\partial\delta\xi}={\begin{bmatrix}I_{3\times3}&-\hat{P_c}\end{bmatrix}}_{3\times6} ∂δξ∂Pc=[I3×3−Pc^]3×6

注：∂Pc∂δξ\frac{\displaystyle\partial P_c}{\displaystyle\partial\delta\xi}∂δξ∂Pc 投影点（投影到相机坐标系）关于位姿的雅克比，见书本推导；误差函数中 [x′/z′y′/z′]\begin{bmatrix}x^{'}/z^{'}\\y^{'}/z^{'}\end{bmatrix}[x′/z′y′/z′] 为观测值，是常数。
VINS中的位姿使用的是四元数与平移量表示，误差相对于四元数与平移量的雅克比为：
∂e∂T=(∂e∂Pc∂Pc∂T)2×7\frac{\displaystyle\partial e}{\displaystyle\partial T}={\begin{pmatrix}\frac{\displaystyle\partial e}{\displaystyle\partial P_c}\frac{\displaystyle\partial P_c}{\displaystyle\partial T}\end{pmatrix}}_{2\times7}\; ∂T∂e=(∂Pc∂e∂T∂Pc)2×7
维度变成 2×72\times72×7,∂e∂Pc\frac{\displaystyle\partial e}{\displaystyle\partial P_c}∂Pc∂e 在上面已经算出
∂Pc∂T3×7=[∂Pc∂t∂Pc∂Q]=[Rcw−Pc^3×3]3×7{\frac{\displaystyle\partial P_c}{\displaystyle\partial T}}_{3\times7}=\;\begin{bmatrix}\frac{\displaystyle\partial P_c}{\displaystyle\partial t}&\frac{\displaystyle\partial P_c}{\displaystyle\partial Q}\end{bmatrix}={\begin{bmatrix}R_{cw}&-{\hat{P_{c\;}}}_{3\times3}\end{bmatrix}}_{3\times7} ∂T∂Pc3×7=[∂t∂Pc∂Q∂Pc]=[Rcw−Pc^3×3]3×7
∂Pc∂T3×7{\frac{\displaystyle\partial P_c}{\displaystyle\partial T}}_{3\times7}∂T∂Pc3×7 的雅克比矩阵维度为 3×73\times73×7，但是最后一列全为0 。
这样就与VINS 对应上了，只不过VINS 中包含IMU 坐标系，还要注意的是这儿讲的是投影误差，并不是重投影误差。

1.3重投影误差公式

通过对上面的投影误差介绍，我们接下来介绍 VINS 中的重投影误差，投影的起点不是从世界坐标系开始，而是从相机i 的归一化平面一点PiP_iPi 开始，重投影到相机j 的归一化平面下为Pi′P_i^{'}Pi′ ，与相机j下的观测值PjP_jPj 做差。

1.4重投影误差相对各个待优化变量的雅克比

整个过程中涉及到的变量有：
相机i的位姿，相机j的位姿，相机与IMU的外参，PiP_iPi 对应的逆深度
雅克比计算过程和上面的类似，VINs在代码中，预先计算了∂e∂Pc\frac{\displaystyle\partial e}{\displaystyle\partial P_c}∂Pc∂e,所有的雅克比都是通过 PcP_cPc作为桥梁，进行链式求导。

并且在对重投影误差乘上了一个协方差

（具体推导后续有空补上）

VINS ---视觉约束的雅克比推导相关推荐

vins 解读_代码解读 | VINS 视觉前端
AI 人工智能代码解读 | VINS 视觉前端本文作者是计算机视觉life公众号成员蔡量力,由于格式问题部分内容显示可能有问题,更好的阅读体验,请查看原文链接:代码解读 | VINS 视觉前端 v ...
机械臂中的四元素转为旋转矩阵_雅克比矩阵（上）雅克比推导
1.前言回顾前面几期的内容,在第一期中介绍了机器人的正/逆运动学建模,正运动学解决的问题是如何从关节空间的关节变量描述操作空间的位姿,反之则是逆运动学的内容.将操作空间和关节的空间的关系用以下关系式 ...
雅克比矩阵（上）-----雅克比推导
1.前言回顾前面几期的内容,在第一期中介绍了机器人的正/逆运动学建模,正运动学解决的问题是如何从关节空间的关节变量描述操作空间的位姿,反之则是逆运动学的内容.将操作空间和关节的空间的关系用以下关系式 ...
VINS-Mono之后端非线性优化 (目标函数中视觉残差和IMU残差，及其对状态量的雅克比矩阵、协方差递推方程的推导)
文章目录 1. 前言 2. 非线性最小二乘 2.1 Guass-Newton 和 Levenberg-Marquardt 2.2 鲁棒核函数下状态量增量方程的构建 3. 局部Bundle Adjust ...
se2lam翻译：基于 SE (2)-XYZ 约束的地面车辆视觉-里程计定位与建图
摘要-本文关注的是地面车辆使用里程计和单目视觉传感器的SLAM问题,为了改进基于视觉的地面车辆估计精度,研究人员利用了近似平面运动的约束,通常将其作为 SE (3)位姿的随机约束来实现,在本文中提出了 ...
一文详解单目VINS论文与代码解读目录
本文旨在对前一阶段学习vins-mono开源框架的总结.结合暑假秋招之前报名的深蓝学院的<从零开始手写VIO>课程,本文从VIO原理以及开源代码分析两部分进行详细介绍.PS:提升代码能力最 ...
视觉SLAM论文list
视觉SLAM论文list 转载:https://blog.csdn.net/heyijia0327/article/details/82855443 基础知识材料 2018年, joan sola 大 ...
图解vins后端边缘化
后端代码逻辑 vins后端大部分代码集中在optimization函数中,而此函数中的边缘化占据相当一部分内容. 对边缘化后Hδx=b的处理因为对于ceres,传入的是雅可比和残差进行迭代,所以首先 ...
VINS技术路线与代码详解
VINS技术路线写在前面:本文整和自己的思路,希望对学习VINS或者VIO的同学有所帮助,如果你觉得文章写的对你的理解有一点帮助,可以推荐给周围的小伙伴们,当然,如果你有任何问题想要交流,欢迎随时探 ...

VINS ---视觉约束的雅克比推导