第七篇 相机位姿优化问题二 ：雅克比矩阵推导dR_dV

一 背景概述

在SLAM和自动驾驶中经常遇到如下公式，知道特征集合P’和特征集合P,如何求解R、t。
$$P^{\prime }=R\ast P+t$$
其中R是车辆姿态，t是车辆位置。这里就是要求出特征投影误差对于姿态的雅克比矩阵。求雅克比矩阵时，姿态使用等效旋转向量V表达。

二 解决思路

2.1 左扰动模型

参见SLAM14讲4.3.4.
$$\frac{d(R\ast P)}{dV}=-(R\ast P)\stackrel{ˆ}{}$$

2.2 右扰动模型

参见SLAM14讲4.3.4.
$$\frac{d(R\ast P)}{dV}=-R\ast P\stackrel{ˆ}{}$$

2.3 直接dR_dV

将R由3行3列，变为9行1列，则dR_dV 是9行3列,每一列就是R对V的某一维度求偏导数，后续运算时将dR_dV的一列变成3行3列。
位置误差对姿态向量求雅克比矩阵时，分别单独对姿态向量的某一维度求偏导数，然后合成雅克比，那么求雅克比的关键就是求出dR_dV。
$$J=\left[\begin{array}{c}dR\_dV(:,1{)}_{3\times 3}\ast PdR\_dV(:,2{)}_{3\times 3}\ast PdR\_dV(:,3{)}_{3\times 3}\ast P\end{array}\right]$$
J是3行3列。那么如何求dR_dV 9行3列呢？
$$V=[v1v2v3]$$
模是a,构造如下向量：
$$V2=[v1v2v3a]$$
单位化向量V，组成向量n2。
$$\begin{gathered} n2=[n1n2n3a] \\ n=[n1n2n3] \end{gathered}$$
构造中间向量 nn^T 和 n^成9维列向量
$$\begin{gathered} m1=[cos(a)1-cos(a)sin(a){]}^{\prime } \\ m2=[m1n{n}^{T}n\stackrel{ˆ}{}] \end{gathered}$$
根据罗德里格斯公式：
$$R=cos(a)\ast I+(1-cos(a))\ast n{n}^T+sin(a)\ast n\stackrel{ˆ}{}$$
将上式右边三项均由33换成91，因此根据求雅克比的链式法则
$$\frac{dR}{dV}=\frac{dR}{dm2}\ast \frac{dm2}{dn2}\ast \frac{dn2}{dV2}\ast \frac{V2}{dV}$$

R 大小是 9行1列
m2 大小是 21行1列
n2 大小是 4行1列
V2 大小是 4行1列
V 大小是 3行1列

复杂雅克比求解总结：

1. 矩阵求导复杂时，将矩阵转成单维列向量，然后求解求导。
2. 复杂的公式求解雅克比，充分利用链式法则，生成中间向量求解雅克比。