下面主要记录一些李代数涉及到的求导，其中主要提供左扰动与常规加法更新求导两种方式。因为不同的求导方式，会影响变量的更新法则，所以要注意区分。

推导过程主要用到的公式可参考李群李代数工具。

1. SO(3) 求导
(1.1) 左扰动求导
$$
\begin{aligned}
\frac{ \partial R\mathbf{p}}{ \partial \phi}
&=\lim_{\delta \phi  \rightarrow 0} \frac{\exp(\delta \phi^\wedge)R\mathbf{p} – R\mathbf{p}}{\delta \phi}\\
& \approx \lim_{\delta \phi  \rightarrow 0} \frac{(I + \delta \phi^\wedge)R\mathbf{p} – R\mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{\delta \phi^\wedge R\mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{-(R\mathbf{p})^\wedge \delta \phi}{\delta \phi}\\
&= -(R\mathbf{p})^\wedge
\end{aligned}
$$

(1.2) 常规加法更新求导
$$
\begin{aligned}
\frac{ \partial R\mathbf{p}}{ \partial \phi}
&=\frac{ \partial \exp(\phi^\wedge)\mathbf{p}}{ \partial \phi}\\
&=\lim_{\delta \phi  \rightarrow 0} \frac{\exp((\phi +\delta \phi)^\wedge)\mathbf{p} – R\mathbf{p}}{\delta \phi}\\
& \approx \lim_{\delta \phi  \rightarrow 0} \frac{\exp((J_l \delta \phi)^\wedge)R\mathbf{p} – R\mathbf{p}}{\delta \phi}\\
& \approx \lim_{\delta \phi  \rightarrow 0} \frac{(I+(J_l \delta \phi)^\wedge)R\mathbf{p} – R\mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{(J_l \delta \phi)^\wedge R\mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{-(R\mathbf{p})^\wedge J_l \delta \phi}{\delta \phi}\\
&= -(R\mathbf{p})^\wedge J_l
\end{aligned}
$$

(1.3) 左扰动求导
$$
\begin{aligned}
\frac{ \partial R^T\mathbf{p}}{ \partial \phi}
&=\lim_{\delta \phi  \rightarrow 0} \frac{(\exp(\delta \phi^\wedge)R)^T\mathbf{p} – R^T\mathbf{p}}{\delta \phi}\\
&=\lim_{\delta \phi  \rightarrow 0} \frac{R^T\exp(-\delta \phi^\wedge)\mathbf{p} – R^T\mathbf{p}}{\delta \phi}\\
& \approx \lim_{\delta \phi  \rightarrow 0} \frac{R^T(I – \delta \phi^\wedge)\mathbf{p} – R^T\mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{-R^T\delta \phi^\wedge \mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{R^T \mathbf{p}^\wedge \delta \phi}{\delta \phi}\\
&= R^T \mathbf{p}^\wedge
\end{aligned}
$$

(1.4) 常规加法更新求导
$$
\begin{aligned}
\frac{ \partial R^T\mathbf{p}}{ \partial \phi}
&=\frac{ \partial \exp(\phi^\wedge)^T\mathbf{p}}{ \partial \phi}\\
&=\lim_{\delta \phi  \rightarrow 0} \frac{\exp((\phi +\delta \phi)^\wedge)^T\mathbf{p} – R^T\mathbf{p}}{\delta \phi}\\
& \approx \lim_{\delta \phi  \rightarrow 0} \frac{\exp(-(J_r \delta \phi)^\wedge)R^T\mathbf{p} – R^T\mathbf{p}}{\delta \phi}\\
& \approx \lim_{\delta \phi  \rightarrow 0} \frac{(I-(J_r \delta \phi)^\wedge)R^T\mathbf{p} – R^T\mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{-(J_r \delta \phi)^\wedge R^T\mathbf{p}}{\delta \phi}\\
&= \lim_{\delta \phi  \rightarrow 0} \frac{(R^T\mathbf{p})^\wedge J_r \delta \phi}{\delta \phi}\\
&= (R^T\mathbf{p})^\wedge J_r
\end{aligned}
$$
上面公式中，\(\phi\)为\(R\)对应的李代数向量，\(\mathbf{p}\)与\(R\)无关。

(1.5) 左扰动求导
$$
\begin{aligned} 
\dfrac{\partial \log(R_1R_2R_3)^\vee}{\partial \phi_2} &= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1\exp(\delta\phi_2^\wedge)R_2R_3)^\vee-\log(R_1R_2R_3)^\vee}{\delta \phi_2}
\\
 &= \lim_{\delta \phi_2 \rightarrow 0}\dfrac{\log(R_1R_2R_3\exp(((R_2R_3)^T\delta \phi_2)^\wedge))^\vee-\log(R_1R_2R_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0}\dfrac{\phi'+J_r'^{-1}(R_2R_3)^T\delta \phi_2-\phi'}{\delta \phi_2}
\\
&=J_r'^{-1}(R_2R_3)^T
\end{aligned}
$$

(1.6) 常规加法更新求导
$$
\begin{aligned} 
\dfrac{\partial \log(R_1R_2R_3)^\vee}{\partial \phi_2} &= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1\exp((\delta\phi_2+\phi_2)^\wedge)R_3)^\vee-\log(R_1R_2R_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1R_2\exp((J_{r2}\delta\phi_2)^\wedge)R_3)^\vee-\log(R_1R_2R_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1R_2R_3\exp((R_3^TJ_{r2}\delta\phi_2)^\wedge))^\vee-\log(R_1R_2R_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\phi'+J_r'^{-1}R_3^TJ_{r2}\delta\phi_2-\phi'}{\delta \phi_2}
\\
&=J_r'^{-1}R_3^TJ_{r2}
\end{aligned}
$$
上面公式(1.5)-(1.6)中，\(R_1,R_2,R_3\)相互之间无关，\(\phi'\)为\(R_1R_2R_3\)对应的李代数向量，\(J_r'\)为\(R_1R_2R_3\)对应右雅克比矩阵。

(1.7) 左扰动求导
$$
\begin{aligned} 
\dfrac{\partial \log(R_1R_2^TR_3)^\vee}{\partial \phi_2} &= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1(\exp(\delta \phi_2^\wedge)R_2)^TR_3)^\vee-\log(R_1R_2^TR_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1R_2^TR_3\exp(-(R_3^T\delta\phi_2)^\wedge)^\vee-\log(R_1R_2^TR_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\phi''-J_r''^{-1}R_3^T\delta\phi_2-\phi''}{\delta \phi_2}
\\
&=-J_r''^{-1}R_3^T
\end{aligned}
$$

(1.8) 常规加法更新求导
$$
\begin{aligned} 
\dfrac{\partial \log(R_1R_2^TR_3)^\vee}{\partial \phi_2} &= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1\exp((\delta\phi_2+\phi_2)^\wedge)^TR_3)^\vee-\log(R_1R_2^TR_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1R_2^T\exp((-J_{l2}\delta\phi_2)^\wedge)R_3)^\vee-\log(R_1R_2^TR_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\log(R_1R_2^TR_3\exp((-R_3^TJ_{l2}\delta\phi_2)^\wedge))^\vee-\log(R_1R_2^TR_3)^\vee}{\delta \phi_2}
\\
&= \lim_{\delta \phi_2 \rightarrow 0} \dfrac{\phi''-J_r''^{-1}R_3^TJ_{l2}\delta\phi_2-\phi''}{\delta \phi_2}
\\
&=-J_r''^{-1}R_3^TJ_{l2}
\end{aligned}
$$

上面公式(1.7)-(1.8)中，\(R_1,R_2,R_3\)相互之间无关，\(\phi''\)为\(R_1R_2^TR_3\)对应的李代数向量，\(J_r''\)为\(R_1R_2^TR_3\)对应右雅克比矩阵。

2. SE(3) 求导
(2.1)
$$
\mathbf{z} = R \mathbf{p} + \mathbf{t}\\
\Downarrow
\\
\begin{bmatrix}
\mathbf{z}\\
1
\end{bmatrix}
= \begin{bmatrix}
R & \mathbf{t} \\ 
\mathbf{0}^T & 1
\end{bmatrix}
\begin{bmatrix}
\mathbf{p}\\
1
\end{bmatrix}
= T\mathbf{p}'
$$

(2.2) 左扰动求导
$$
\begin{aligned}
\frac{\partial T \mathbf{p}'}{ \partial \xi}
&=\lim_{\delta \xi  \rightarrow 0} \frac{\exp(\delta \xi^\wedge)T\mathbf{p}' – T \mathbf{p}'}{\delta \xi}\\
& \approx \lim_{\delta \xi  \rightarrow 0} \frac{(I + \delta \xi^\wedge)T\mathbf{p}' – T \mathbf{p}'}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{\delta \xi^\wedge T\mathbf{p}'}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{
\begin{bmatrix}
\delta \phi^\wedge & \delta \rho \\ 
\mathbf{0}^T & 0
\end{bmatrix}
\begin{bmatrix}
R & \mathbf{t} \\ 
\mathbf{0}^T & 1
\end{bmatrix}
\begin{bmatrix}
\mathbf{p} \\ 
1
\end{bmatrix}
}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{
\begin{bmatrix}
\delta \phi^\wedge (R\mathbf{p} + \mathbf{t}) + \delta \rho \\ 
0
\end{bmatrix}
}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{
\begin{bmatrix}
-(R\mathbf{p} + \mathbf{t})^\wedge \delta \phi + \delta \rho \\ 
0
\end{bmatrix}
}{\delta \xi}\\
&=\begin{bmatrix}
I & -(R\mathbf{p} + \mathbf{t})^\wedge\\ 
0  & 0
\end{bmatrix}
\end{aligned}
\\
\Downarrow
\\
\frac{\partial (R \mathbf{p} + \mathbf{t})}{ \partial \xi}= \begin{bmatrix}
I & -(R\mathbf{p} + \mathbf{t})^\wedge
\end{bmatrix}
$$

(2.3) 常规加法更新求导
$$
\begin{aligned}
\frac{\partial T \mathbf{p}'}{ \partial \xi}
&=\lim_{\delta \xi  \rightarrow 0} \frac{\exp((\xi + \delta \xi)^\wedge)\mathbf{p}' – T \mathbf{p}'}{\delta \xi}\\
& \approx\lim_{\delta \xi  \rightarrow 0} \frac{\exp((\mathcal{J}_l \delta \xi)^\wedge)T\mathbf{p}' – T \mathbf{p}'}{\delta \xi}\\
& \approx \lim_{\delta \xi  \rightarrow 0} \frac{(I + (\mathcal{J}_l\delta \xi)^\wedge)T\mathbf{p}' – T \mathbf{p}'}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{(\mathcal{J}_l\delta \xi)^\wedge T\mathbf{p}'}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{
\begin{bmatrix}
(J_l\delta \phi)^\wedge & J_l \delta \rho + Q_l \delta \phi \\ 
\mathbf{0}^T & 0
\end{bmatrix}
\begin{bmatrix}
R & \mathbf{t} \\ 
\mathbf{0}^T & 1
\end{bmatrix}
\begin{bmatrix}
\mathbf{p} \\ 
1
\end{bmatrix}
}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{
\begin{bmatrix}
(J_l \delta \phi)^\wedge (R\mathbf{p} + \mathbf{t}) + J_l \delta \rho + Q_l \delta \phi \\ 
0
\end{bmatrix}
}{\delta \xi}\\
&= \lim_{\delta \xi  \rightarrow 0} \frac{
\begin{bmatrix}
-(R\mathbf{p} + \mathbf{t})^\wedge (J_l\delta \phi) + J_l \delta \rho + Q_l \delta \phi \\ 
0
\end{bmatrix}
}{\delta \xi}\\
&=\begin{bmatrix}
J_l & Q_l -(R\mathbf{p} + \mathbf{t})^\wedge J_l\\ 
0  & 0
\end{bmatrix}
\end{aligned}
\\
\Downarrow
\\
\frac{\partial (R \mathbf{p} + \mathbf{t})}{ \partial \xi}= \begin{bmatrix}
J_l & Q_l -(R\mathbf{p} + \mathbf{t})^\wedge J_l
\end{bmatrix}
$$

结合(1.1),(1.2),(2.2),(2.3)可以得到，对于同属于一个SE3中的变量，有如下结果

(2.4) 左扰动求导
$$
\begin{aligned}
\frac{\partial J_l \rho}{ \partial \phi} &=
\frac{\partial \mathbf{t}}{ \partial \phi} = -\mathbf{t}^\wedge
\end{aligned}
$$

(2.5) 常规加法更新求导
$$
\frac{\partial \mathbf{t}}{ \partial \phi} = Q_l -\mathbf{t}^\wedge J_l
$$

(2.4)(2.5)不仅给出了偏移与\(\phi\)的求导关系，还给出了雅可比矩阵求导与\(\phi\)的关系

(2.6) 左扰动求导
$$
\begin{aligned}
\frac{\partial \mathbf{t}}{ \partial \rho} &= I
\end{aligned}
$$

(2.7) 常规加法更新求导
$$
\frac{\partial \mathbf{t}}{ \partial \rho} = J_l
$$

从公式(2.4),(2.5)可以得到左雅克比矩阵的求导

(2.8)左扰动求导

$$
\begin{aligned}
\frac{\partial J_l \mathbf{p}}{ \partial \phi} &= -(J_l \mathbf{\mathbf{p}})^\wedge
\end{aligned}
$$

(2.9) 常规加法更新求导

$$
\begin{aligned}
\frac{\partial J_l \mathbf{p}}{ \partial \phi} &=Q_l -(J_l \mathbf{\mathbf{p}})^\wedge J_l
\end{aligned}
$$

注意，这里\(Q_l\)当中的\(\rho\)用\(\mathbf{p}\)代替。

而对于右雅克比矩阵，有

(2.10)

$$
\begin{aligned}
\frac{\partial J_r \mathbf{p}}{ \partial \phi} = \frac{\partial R^T J_l \mathbf{p}}{ \partial \phi}= \frac{\partial R^T (J_l \mathbf{p})}{ \partial \phi} + R^T \frac{\partial   J_l \mathbf{p}}{ \partial \phi}
\end{aligned}
$$

将(1.3),(1.4),(2.8),(2.9)代入公式(2.10)可以得到

(2.11)左扰动求导

$$
\frac{\partial J_r \mathbf{p}}{ \partial \phi} = \mathbf{0}
$$

(2.12)常规加法更新求导

$$
\frac{\partial J_r \mathbf{p}}{ \partial \phi} =R^T(\mathbf{p}^\wedge J_l + Q_l – (J_l\mathbf{p})^\wedge J_l)
$$

(2.13) 左扰动求导

$$
\begin{aligned}
\dfrac{\partial \log(T_1T_2T_3)^\vee}{\partial\xi_2} &\approx \lim_{\delta \xi_2 \rightarrow 0} \dfrac{\log(T_1\exp(\delta \xi_2^\wedge)T_2T_3)^\vee – \log(T_1T_2T_3)^\vee}{\delta \xi_2}
\\
&= \lim_{\delta \xi_1 \rightarrow 0}
\dfrac{\log(T_1T_2T_3\exp((\mathcal{T}(T_2T_3)^{-1} \delta \xi_2)^\wedge))^\vee-\log(T_1T_2T_3)^\vee}{\delta \xi_2}
\\
&\approx \lim_{\delta \xi_2 \rightarrow 0}
\dfrac{\log(\exp((\xi_{123}+\mathcal{J}_{r123}^{-1}\mathcal{T}(T_2T_3)^{-1} \delta \xi_2)^\wedge))^\vee-\log(\exp(\xi_{123}^\wedge))^\vee}{\delta \xi_2}
\\
&=\mathcal{J}_{r123}^{-1}\mathcal{T}(T_2T_3)^{-1}
\end{aligned}
$$

(2.14) 常规加法求导
$$
\begin{aligned}
\dfrac{\partial \log(T_1T_2T_3)^\vee}{\partial\xi_2} &\approx \lim_{\delta \xi_2 \rightarrow 0} \dfrac{\log(T_1\exp((\xi_2 +\delta \xi_2)^\wedge)T_3)^\vee – \log(T_1T_2T_3)^\vee}{\delta \xi_2}
\\
&\approx \lim_{\delta \xi_1 \rightarrow 0}
\dfrac{\log(T_1T_2\exp((\mathcal{J}_{r2}\delta \xi_2)^\wedge)T_3)^\vee-\log(T_1T_2T_3)^\vee}{\delta \xi_2}
\\
&= \lim_{\delta \xi_1 \rightarrow 0}
\dfrac{\log(T_1T_2T_3\exp((\mathcal{T}(T_3)^{-1}\mathcal{J}_{r2}\delta \xi_2)^\wedge))^\vee-\log(T_1T_2T_3)^\vee}{\delta \xi_2}
\\
&\approx \lim_{\delta \xi_2 \rightarrow 0}
\dfrac{\log(\exp((\xi_{123}+\mathcal{J}_{r123}^{-1}\mathcal{T}(T_3)^{-1}\mathcal{J}_{r2} \delta \xi_2)^\wedge))^\vee-\log(\exp(\xi_{123}^\wedge))^\vee}{\delta \xi_2}
\\
&=\mathcal{J}_{r123}^{-1}\mathcal{J}_{r2}\mathcal{T}(T_3)^{-1}
\end{aligned}
$$
上面公式中 \(\mathcal{J}_{r123}\)为 \(T_1T_2T_3\)的右雅可比矩阵，\(\mathcal{J}_{r2}\)为 \(T_2\)的右雅可比矩阵。

(2.15) 左扰动求导
$$
\begin{aligned}
\dfrac{\partial \log(T_1T_2^{-1}T_3)^\vee}{\partial\xi_2} &\approx \lim_{\delta \xi_2 \rightarrow 0} \dfrac{\log(T_1(\exp(\delta \xi_2^\wedge)T_2)^{-1}T_3)^\vee – \log(T_1T_2^{-1}T_3)^\vee}{\delta \xi_2}
\\
&=\lim_{\delta \xi_2 \rightarrow 0} \dfrac{\log(T_1T_2^{-1}\exp(-\delta \xi_2^\wedge)T_3)^\vee – \log(T_1T_2^{-1}T_3)^\vee}{\delta \xi_2}
\\
&= \lim_{\delta \xi_1 \rightarrow 0}
\dfrac{\log(T_1T_2^{-1}T_3\exp((-\mathcal{T}(T_3)^{-1} \delta \xi_2)^\wedge))^\vee-\log(T_1T_2^{-1}T_3)^\vee}{\delta \xi_2}
\\
&\approx \lim_{\delta \xi_2 \rightarrow 0}
\dfrac{\log(\exp((\xi_{123}'-\mathcal{J}_{r123}'^{-1}\mathcal{T}(T_3)^{-1} \delta \xi_2)^\wedge))^\vee-\log(\exp(\xi_{123}'^\wedge))^\vee}{\delta \xi_2}
\\
&=-\mathcal{J}_{r123}'^{-1}\mathcal{T}(T_3)^{-1}
\end{aligned}
$$

(2.16) 常规加法求导
$$
\begin{aligned}
\dfrac{\partial \log(T_1T_2^{-1}T_3)^\vee}{\partial\xi_2} &\approx \lim_{\delta \xi_2 \rightarrow 0} \dfrac{\log(T_1(\exp((\xi_2+\delta \xi_2)^\wedge)T_2)^{-1}T_3)^\vee – \log(T_1T_2^{-1}T_3)^\vee}{\delta \xi_2}
\\
&\approx \lim_{\delta \xi_2 \rightarrow 0} \dfrac{\log(T_1(T_2\exp((\mathcal{J}_{r2}\delta \xi_2)^\wedge))^{-1}T_3)^\vee – \log(T_1T_2^{-1}T_3)^\vee}{\delta \xi_2}
\\
&= \lim_{\delta \xi_2 \rightarrow 0} \dfrac{\log(T_1\exp(-(\mathcal{J}_{r2}\delta \xi_2)^\wedge))T_2^{-1}T_3)^\vee – \log(T_1T_2^{-1}T_3)^\vee}{\delta \xi_2}
\\
&= \lim_{\delta \xi_1 \rightarrow 0}
\dfrac{\log(T_1T_2^{-1}T_3\exp((-\mathcal{T}(T_2^{-1}T_3)^{-1}\mathcal{J}_{r2} \delta \xi_2)^\wedge))^\vee-\log(T_1T_2^{-1}T_3)^\vee}{\delta \xi_2}
\\
&\approx \lim_{\delta \xi_2 \rightarrow 0}
\dfrac{\log(\exp((\xi_{123}'-\mathcal{J}_{r123}'^{-1}\mathcal{T}(T_2^{-1}T_3)^{-1}\mathcal{J}_{r2} \delta \xi_2)^\wedge))^\vee-\log(\exp(\xi_{123}'^\wedge))^\vee}{\delta \xi_2}
\\
&=-\mathcal{J}_{r123}'^{-1}\mathcal{T}(T_2^{-1}T_3)^{-1}\mathcal{J}_{r2}
\end{aligned}
$$
上面公式中 \(\mathcal{J}_{r123}'\)为 \(T_1T_2^{-1}T_3\)的右雅可比矩阵，\(\mathcal{J}_{r2}\)为 \(T_2\)的右雅可比矩阵
 

(2.17) 左扰动求导

$$\begin{aligned}
\dfrac{ \partial T^{-1}\mathbf{p}'}{ \partial \xi}&=\lim_{\delta \xi \rightarrow 0}\dfrac{(\exp(\delta \xi^\wedge)T)^{-1}\mathbf{p}'-T^{-1}\mathbf{p}'}{\delta \xi}
\\
&=\lim_{\delta \xi \rightarrow 0}\dfrac{T^{-1}\exp(-\delta \xi^\wedge)\mathbf{p}'-T^{-1}\mathbf{p}'}{\delta \xi}
\\
&\approx \lim_{\delta \xi \rightarrow 0}\dfrac{T^{-1}(I-\delta \xi^\wedge)\mathbf{p}'-T^{-1}\mathbf{p}'}{\delta \xi}
\\
&=\lim_{\delta \xi \rightarrow 0}\dfrac{-T^{-1}\delta \xi^\wedge\mathbf{p}'}{\delta \xi}
\\
&=\lim_{\delta \xi \rightarrow 0}-\dfrac{
\begin{bmatrix}
R^T&-R^T\mathbf{t}
\\
\mathbf{0}^T&1
\end{bmatrix}
\begin{bmatrix}
\delta \phi^\wedge & \delta \rho
\\
\mathbf{0}^T & 0
\end{bmatrix}
\begin{bmatrix}
\mathbf{p}
\\
1
\end{bmatrix}
}{\delta \xi}
\\
&=\lim_{\delta \xi \rightarrow 0}-\dfrac{
\begin{bmatrix}
R^T(\delta \phi^\wedge \mathbf{p} + \delta \rho)
\\
0
\end{bmatrix}
}{\delta \xi}
\\
&=\begin{bmatrix}
-R^T & R^T\mathbf{p}^\wedge
\\
0&0
\end{bmatrix}
\end{aligned}$$

(2.18)左扰动求导

$$\begin{aligned}
\dfrac{\partial ((T^{-1})^T\mathbf{p}')}{\partial \xi} &= \lim_{\delta \xi \rightarrow 0} \dfrac{((\exp(\delta \xi^\wedge)T)^{-1})^T\mathbf{p}'-(T^{-1})^T\mathbf{p}'}{\delta \xi}
\\
&=
\lim_{\delta \xi \rightarrow 0} \dfrac{\exp(-\delta \xi^\wedge)^T(T^{-1})^T\mathbf{p}'-(T^{-1})^T\mathbf{p}'}{\delta \xi}
\\
&\approx \lim_{\delta \xi \rightarrow 0} \dfrac{(I-\delta \xi^\wedge)^T(T^{-1})^T\mathbf{p}'-(T^{-1})^T\mathbf{p}'}{\delta \xi}
\\
&= \lim_{\delta \xi \rightarrow 0} \dfrac{(-\delta \xi^\wedge)^T(T^{-1})^T\mathbf{p}'}{\delta \xi}
\\
&=\lim_{\delta \xi \rightarrow 0} \dfrac{
\begin{bmatrix}
(-\delta \phi^\wedge)^T &\mathbf{0}
\\
-\delta \rho^T & 0
\end{bmatrix}
\begin{bmatrix}
R & \mathbf{0}
\\
-\mathbf{t}^TR & 1
\end{bmatrix}
\begin{bmatrix}
\mathbf{p}
\\
1
\end{bmatrix}
}{\delta \xi}
\\
&=\lim_{\delta \xi \rightarrow 0}\dfrac{\begin{bmatrix}
\delta \phi^\wedge R\mathbf{p}
\\
-\delta \rho^T R\mathbf{p}
\end{bmatrix}
}{\delta \xi}
\\
&=\lim_{\delta \xi \rightarrow 0}\dfrac{\begin{bmatrix}
– (R\mathbf{p})^\wedge \delta \phi
\\
– (R\mathbf{p})^T\delta \rho
\end{bmatrix}
}{\delta \xi}
\\
&=\begin{bmatrix}
\mathbf{I} \times 0 & -(R \mathbf{p})^\wedge
\\
-(R\mathbf{p})^T & \mathbf{0}^T
\end{bmatrix}
\end{aligned}$$

(2.19)左扰动求导

$$\begin{aligned} 
\dfrac{\partial (T^T\mathbf{p}')}{\partial \xi} &=\lim_{\delta \xi\rightarrow 0}\dfrac{(\exp(\delta \xi^\wedge)T)^T\mathbf{p}'-T^T\mathbf{p}'}{\delta \xi}
\\
&=\lim_{\delta \xi\rightarrow 0}\dfrac{T^T(
    I+\delta \xi^\wedge)^T\mathbf{p}'-T^T\mathbf{p}'}{\delta \xi}
\\
&=\lim_{\delta \xi\rightarrow 0}\dfrac{
\begin{bmatrix}
R^T & \mathbf{0}
\\
\mathbf{t}^T & 1
\end{bmatrix}
\begin{bmatrix}
-\delta \phi^\wedge & \mathbf{0}
\\
\delta \rho^T & 0
\end{bmatrix}
\begin{bmatrix}
\mathbf{p}
\\
1
\end{bmatrix}
}{\delta \xi}
\\
&=\lim_{\delta \xi\rightarrow 0}\dfrac{
\begin{bmatrix}
R^T\mathbf{p}^\wedge \delta\phi
\\
\mathbf{t}^T\mathbf{p}^\wedge\delta \phi+\mathbf{p}^T\delta \rho
\end{bmatrix}
}{\delta \xi}
\\
&=\begin{bmatrix}
\mathbf{I} \times 0 & R^T\mathbf{p}^\wedge
\\
\mathbf{p}^T & \mathbf{t}^T\mathbf{p}^\wedge
\end{bmatrix}
\end{aligned}$$

PS：EKF本身是一种数学工具，具体的原理推导可以参考文献[1]。对于使用者来说，只要理解这个工具对应的数学模型，以及对应系数的更新方式，就可以套用EKF的求解模式进行求解。

1 模型
在离散线性系统中，对于某种状态\(\mathcal{x}\)

定义运动方程 
(1.1)
$$
\mathcal{x}_{k} = A_{k-1} \mathcal{x}_{k-1} + \mathcal{v}_k + \mathcal{w}_k
$$

公式中\(\mathcal{w}_k \in \mathcal{N}(\mathbf{0}, Q_k)\)的过程噪声，其中\(A_{k-1}\)称为转移矩阵，\(\mathcal{x}_0 \in \mathcal{N}(\mathbf{0}, P_0)\),\(P_0\)为\(\mathcal{x}_0\)的初始协方差矩阵。

同样，定义观测方程
(1.2)
$$
\mathcal{y}_{k} = C_k \mathcal{x}_k + \mathcal{n}_k
$$
公式中\(\mathcal{n}_k \in \mathcal{N}(\mathbf{0}, R_k)\)的测量噪声,其中\(C_{k}\)称为观测矩阵。
 

2 卡尔曼滤波
下面为常规的扩展卡尔曼滤波更新过程[1]
1.预测 
(2.1)
$$
\begin{aligned} 
P_k &= A_{k-1}P_{k-1}A_{k-1}^T + Q_k\\
\mathcal{x}_k &=A_{k-1}\mathcal{x}_{k-1}+\mathcal{v}_k
\end{aligned} 
$$

2.卡尔曼增益
(2.2)
$$
\begin{aligned}
K &= P_kC_k^T(C_kP_kC_k^T+R_k)^{-1}\\
\end{aligned}
$$

3.更新
(2.3)
$$
\begin{aligned}
P_k&= (I – KC_k)P_k\\
\mathcal{x}_k &= \mathcal{x}_k + K(\mathcal{y}_k – C_k\mathcal{x}_k)
\end{aligned}
$$

即，每次可以使用公式(2.1),(2.2),(2.3)进行过程更新。

3 理解
在每一轮更新中，先根据运动方程进行预测，然后结合观测方程估计出一个修正系数，最后使用系数对变量进行修正。

仔细观察上面更新过程以及对应数学模型，可以发现，在本质上来说可以通过运动方程直接递推出下一个状态，但是考虑到一些噪音（为了提高精度），这里会使用观测方程来对递推出的状态进行修正，也因此可以理解成两类方程耦合

 

注意，上面公式中协方差矩阵\(P_{k}\)本应该服务的对象，下面将以SLAM当中的例子加以说明。

4 SLAM当中的EKF 

4.1 运动方程
对于位姿\(T\)，以及它对应的广义速度\(\varpi\)，它们满足运动学方程  
(4.1)
$$
\dot{T} = \varpi ^\wedge T
$$
对上面的两个变量，分边添加一个微小的扰动，然后忽略二阶小量有 
(4.2)
$$
\begin{aligned} 
\frac{d}{dt}((I + \delta \xi^\wedge)T) &= (\delta \varpi + \varpi)^\wedge ((I +\delta \xi ^\wedge) T)\\
\delta \dot{\xi}^\wedge T + \dot{T} + \delta \xi ^\wedge \dot{T} &=\varpi ^\wedge T + \varpi ^\wedge \delta \xi^\wedge T + \delta \varpi^\wedge T + \delta \varpi^\wedge \delta \xi ^\wedge T\\
& \approx \varpi ^\wedge T + \varpi ^\wedge \delta \xi^\wedge T + \delta \varpi^\wedge T
\end{aligned}
$$

将(4.1)带入(4.2)有 
(4.3)
$$
\begin{aligned}
\delta \dot{\xi}^\wedge &= \varpi ^\wedge \delta \xi ^\wedge – \delta \xi ^\wedge \varpi ^\wedge + \delta \varpi ^\wedge\\
&=(\varpi ^\curlywedge \delta \xi )^\wedge+ \delta \varpi ^\wedge\\
&=(\varpi ^\curlywedge \delta \xi + \delta \varpi)^\wedge
\end{aligned}
\\
\Downarrow
\\
\delta \dot{\xi} = \varpi ^\curlywedge \delta \xi + \delta \varpi
$$
一般把(4.1)称为标称运动学方程，(4.2)称为扰动运动学方程。
对上面公式进行离散化可得到 
(4.4)
$$
\begin{aligned}
T_k &= \exp((t_k – t_{k-1}) \varpi ^\wedge)T_{k-1}\\
\delta \xi_k &= \exp((t_k – t_{k-1})\varpi ^\curlywedge) \delta \xi _{k-1} + \mathcal{w}_k
\end{aligned}
$$

\(t_{k}\)表示对应时刻值，\(\mathcal{w}_{k} \in \mathcal{N}(0, Q_k)\)为过程噪声。 

4.2 观测方程
这里假设具有如下的观测模型 
(4.5)
$$
\mathcal{y} =  T \mathcal{p} + n
$$
同样，两边同时加一个微小的扰动有 
(4.6)
$$
\begin{aligned}
\mathcal{y} + \delta \mathcal{y} &= (I+ \delta \xi^\wedge)Tp + n\\
&= Tp + \delta \xi ^\wedge Tp + n\\
&= Tp + (Tp)^ {\odot} \delta \xi + n
\end{aligned}
$$
这里将(4.5)的标称解带入(4.6)有 
(4.7)
$$
\delta y = (Tp)^{\odot} \delta \xi + n
$$
即有最终离散化公式 
(4.8)
$$
\delta y_k = (Tp)^{\odot} \delta \xi_k + n_k
$$

上面\(\mathcal{n}_k \in \mathcal{N}(\mathbf{0}, R_k)\)

4.3 EKF过程
这里先约定如下变量 
(4.9)
$$
\begin{aligned}
O_k &= \exp((t_k – t_{k-1}) \varpi_{k-1} ^\wedge) \\
G_k &= \exp((t_k – t_{k-1})\varpi_{k-1} ^\curlywedge)\\
C_k &= (T_kp_k)^{\odot}
\end{aligned}
$$

最终结果如下 
4.3.1 预测   
(4.10)
$$
\begin{aligned}
T_k &= O_k T_{k-1}\\
P_k &= G_k P_{k-1}G_k^T + Q_k
\end{aligned}
$$

4.3.2 卡尔曼增益 
(4.11)
$$
\begin{aligned}
K = P_k C_k^T (C_k P_k C_k^T + R_k)^{-1}
\end{aligned}
$$

4.3.3 矫正 
(4.12)
$$
\begin{aligned}
P_k &= (I – KC_k)P)k\\
\mathcal{y}_k' &= T_k \mathcal{p}_k\\
T_k &= \exp((K(\mathcal{y}_k – \mathcal{y}_k'))^\wedge)T_k
\end{aligned}
$$
 参考
[1] 高翔,谢晓佳. 机器人学中的状态估计[M]. 西安交通大学出版社,2018

1 前言
在一些场景里，可能会遇到坐标系转换与刚体变换的问题，对于空间点来说，它们的变换形式是一致的，而对于带旋转的位姿来说，它们的变换方式却不一致。

一个对象进行刚体变换，整体表现为旋转与平移，不改变坐标系本身约定的性质（各个轴之间的相对关系不会改变）。刚体变换可以理解成在同一个约定的坐标系下，从一个相对坐标系到另一个相对坐标系的变换过程！（PS：世界坐标系也可看成一个特殊的相对坐标系）

坐标系转换，从名字上就可以看出，其本身是对约定的系统进行改变。比如，左右坐标系的转换，它们各个轴的相对关系因坐标系的不同而不同。

不管是刚体变换还是坐标系之间的转换，主要的难点是旋转矩阵的表现形式。本文这里仅简单记录。

注意： 
(1) 本文中没有特别说明的，其表示方式均为世界坐标系的表示方式。 
(2) 本文当中的3D点\(\mathbf{p}\)为了表示方便，均以齐次坐标形式表示，即 
(1.1)
$$
\mathbf{p}=\begin{bmatrix} 
x\\
y\\
z\\
1
\end{bmatrix}
$$

 

2 刚体变换

刚体变换的主体思想是，对操作对象进行旋转与平移，这里约定有如下(2.1)的变换矩阵，然后用这个矩阵对操作对象进行操作即达到刚体变换效果。 
(2.1)
$$
T =  \begin{bmatrix} 
R & \mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
$$

 

2.1 点的变换
对于空间点\(\mathbf{p}_0\)，假设在刚体变换\(T\)后变为\(\mathbf{p}_1\)，那么变换前后的关系可以表示为 
(2.2)

$$
\mathbf{p}_1 = T \mathbf{p}_0
$$

 

2.2 位姿的变换
对于空间中，有一个相机的位姿为\(T_0\)，假设在刚体变换\(T\)后变为\(T_1\)。同样，空间点\(\mathbf{p}_0\)，在刚体变换\(T\)后变为\(\mathbf{p}_1\)。由于刚体变换不会改变空间中对象的相对位置信息，因此可知点\(\mathbf{p}_0\)在相机\(T_0\)里的相对位置不会改变，因此可以得到 
(2.3)

$$
T_0\mathbf{p}_0 = T_1\mathbf{p}_1=T_1T\mathbf{p}_0
$$

由于对任意\(\mathbf{p}_0\)均要满足公式(2.3)，因此有 
(2.4)

$$
T_1 = T_0 T^{-1}
$$

注意，这里的位姿\(T_0、T_1\)是世界坐标系到相机坐标系的表示方式, 即如果一个世界坐标系的点\(\mathbf{p}_0\)，通过公式\(T_0 \mathbf{p}_0\)将变换到相机坐标系下的位置。如果\(T_0、T_1\)相机坐标系到世界坐标系的表示方式, 即如果一个世界坐标系的点\(\mathbf{p}_0\)，通过公式\(T_0^{-1} \mathbf{p}_0\)将变换到相机坐标系下的位置，那么根据公式(2.4)，可以得到如下位姿变换结果

(2.5)

$$
T_1 = TT_0
$$

3 坐标系变换
有时我们会在左右手坐标系之间进行转换，有时我们需要将x、y轴进行对调，有时我们需要将当前坐标系按某种形式进行旋转……这一系列的坐标系变换，可以看成一个镜像、旋转、偏移的组合，这里以如下的变换矩阵来表示 
(3.1)
$$
T =  \begin{bmatrix} 
R' & \mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
$$
注意：这里的\(R'\)的行列式不一定为1，也可能为-1。

 

3.1 点的坐标系变换
对于空间点\(\mathbf{p}_0\)，假设在坐标系变换\(T\)后变为\(\mathbf{p}_1\)，那么变换前后的关系可以表示为 
(3.2)

$$
\mathbf{p}_1 = T \mathbf{p}_0
$$

 

3.2 位姿的坐标系变换

3.2.1 相机坐标系按约定变换，世界坐标系按约定变换
对于空间中，有一个相机的位姿为\(T_0\)，假设经过坐标系变换系数\(T\)后变为\(T_1\)。同样，空间点\(\mathbf{p}_0\)，在经过坐标系变换\(T\)后变为\(\mathbf{p}_1\)。由于\(\mathbf{p}_0\)在相机\(T_0\)里的相对位置不会改变。但是，需要注意的是，虽然相对位置不会改变，但是按约定，坐标系进行了变化（例如，在世界坐标系，我们约定x轴向前，y轴向右，z轴向上；在相机坐标系，我们也会默认x轴向前，y轴向右，z轴向上），则可得到如下结果 

这里假设，在变换前的坐标系下，\(\mathbf{p}_0\)相对于\(T_0\)的位置为\(\mathbf{p}'\)，则可以得到 
(3.3)

$$
\mathbf{p}' = T_0 \mathbf{p}_0
$$

\(\mathbf{p}'\)在经过坐标系变换\(T\)后变为\(\mathbf{p}''\)，则有  
(3.4)

$$
\mathbf{p}'' = T \mathbf{p}'
$$

由于在变换后的坐标系下，\(\mathbf{p}_1\)相对于\(T_1\)的位置也为\(\mathbf{p}''\)，即 
(3.5)

$$
\mathbf{p}'' = T_1 \mathbf{p}_1
$$
根据公式(3.2)-(3.5)可得到 
(3.6)

$$
(T_1T – TT_0)\mathbf{p}_0 = \mathbf{0}
$$
由于对任意\(\mathbf{p}_0\)均要满足公式(3.6)，因此有 
(3.7)

$$
T_1 = T T_0 T^{-1}
$$

对(3.7)进行展开有   
(3.8)

$$
\begin{bmatrix} 
R_1 & \mathbf{t}_1\\
\mathbf{0}^T & 1
\end{bmatrix}
=
\begin{bmatrix} 
RR_0R^{-1} &R\mathbf{t}_0 + \mathbf{t} – RR_0R^{-1}\mathbf{t}\\
\mathbf{0}^T & 1
\end{bmatrix}
$$

即旋转部分与偏移部分的变换结果分别为 
(3.9)

$$
\begin{aligned} 
R_1 &= RR_0R^{-1}\\
\mathbf{t}_1 &= R\mathbf{t}_0 + \mathbf{t} – RR_0R^{-1}\mathbf{t}
\end{aligned} 
$$

特别地，当\(\mathbf{t}=\mathbf{0}\)时，有 
(3.10)
$$
\begin{aligned} 
R_1 &= RR_0R^{-1}\\
\mathbf{t}_1 &= R\mathbf{t}_0
\end{aligned} 
$$

 

3.2.2 相机坐标系不变，世界坐标系按约定变换

这里同样使用3.2.1当中的符号表达，由于这里约定相机坐标系不变，因此(3.5)式变为

(3.11)

$$
\mathbf{p}' = T_1 \mathbf{p}_1
$$

由公式(3.2),(3.3),(3.11)可以得到

(3.12)

$$
(T_1T – T_0)\mathbf{p}_0 = \mathbf{0}
$$

由于对任意\(\mathbf{p}_0\)均要满足公式(3.12)，因此有 
(3.13)

$$
T_1 = T_0T^{-1}
$$

 

3.2.3 相机坐标系按约定变换，世界坐标系不变

这里同样使用3.2.1当中的符号表达，由于这里约定世界坐标系不变，因此(3.5)式变为

(3.14)

$$
\mathbf{p}'' = T_1 \mathbf{p}_0
$$

由公式(3.3),(3.4),(3.14)可以得到

(3.15)

$$
(TT_0 – T_1)\mathbf{p}_0 = \mathbf{0}
$$

由于对任意\(\mathbf{p}_0\)均要满足公式(3.15)，因此有 
(3.16)

$$
T_1 = TT_0
$$

 

3.3 相机坐标系变换总结

最终，相机坐标系变换方式，可分3种情况进行：

情况1：相机坐标系按约定变换，世界坐标系按约定变换，这时使用公式(3.7)

情况2：相机坐标系不变，世界坐标系按约定变换，这时使用公式(3.13)

情况3：相机坐标系按约定变换，世界坐标系不变，这时使用公式(3.16)

 

4 例子
现在有一个原始的相机位姿为\(T_0\) 
(4.1)
$$
T_0 =  \begin{bmatrix} 
R_0 & \mathbf{t}_0\\
\mathbf{0}^T & 1
\end{bmatrix}
$$
因为一些接口需要，我们希望将原始坐标系的x轴指向新坐标系的z轴反向，原坐标系的y轴指向新坐标系的x轴反向，原坐标系的z轴指向新坐标系y轴。根据需求，可得到变换矩阵的旋转部分 
(4.2)

$$
R = \begin{bmatrix} 
0 & -1 & 0\\
0 & 0 & 1\\
-1 & 0 & 0
\end{bmatrix}
$$
根据需求，偏移部分为 
(4.3)

$$
\mathbf{t} = \begin{bmatrix} 
0\\
0\\
0
\end{bmatrix}
$$
因此最终得到扩展后的变换矩阵 
(4.4)
$$
T = \begin{bmatrix} 
0 & -1 & 0 & 0\\
0 & 0 & 1 &0 \\
-1 & 0 & 0 &0\\
0 & 0 & 0 & 1
\end{bmatrix}
$$

按公式(3.7)，可得到最终的坐标系变换结果 
$$
T_1 = T T_0 T^{-1}
$$

 

5 延伸

5.1 按指定向量旋转

很多时候，我们可能希望得到某些点以\(\mathbf{p}_0\)为原心，由向量\(\mathbf{n}_1\) 旋转到向量\(\mathbf{n}_2\)后的位置，这个本质上是求公式（3.2）当中的变换矩阵\(T\)。

很明显，第一步，可以将所有点以\(\mathbf{p}_0\)为原心，进行平移；第二步，由于向量\(\mathbf{n}_1\) 与向量\(\mathbf{n}_2\)叉乘后的单位轴向量\(\mathbf{a}\)同时垂直于\(\mathbf{n}_1\)、\(\mathbf{n}_2\)，因此\(\mathbf{a}\)可以作为旋转轴，而旋转角度，可以通过余弦公式计算得出。因此可先按如下方式确定对应变量：

(5.1)

$$
\begin{aligned}
\mathbf{a} &=\frac{ \mathbf{n}_1 \times \mathbf{n}_2}{|\mathbf{n}_1||\mathbf{n}_2|}\\
\theta &= \mathbf{arccos}(\frac{\mathbf{n}_1 \cdot \mathbf{n}_2}{|\mathbf{n}_1||\mathbf{n}_2|})
\end{aligned}
$$

结合SLAM之李群李代数工具中的公式(1.5)，可以得到旋转矩阵\(R\)，最终变换矩阵形式如下

(5.2)

$$
T = \begin{bmatrix}  
R & -R\mathbf{p}_0\\
\mathbf{0} & 1
\end{bmatrix}
$$

在常规的插值中，我们会根据n个已知点的值，插值出给定点的结果。其中主要利用的思想是，通过这n个点建立某种数学模型，进而推导出这个待插值点的结果。本文这里仅考虑2个已知点的情况，对应到我们常规插值中，就表现为线性插值。但本文这里，由于插值出的结果不是一个数值，是一个需要满足某种性质的矩阵，因此，不能单纯采用矩阵的线性加减。

1 SO(3)旋转矩阵插值

这里直接给出一种插值模型

(1)$$\begin{aligned}  
R &= (R_1R_0^T)^aR_0\\   
&= R_{10}^aR_0\\   
&=\exp(\varphi ^ \wedge)^aR_0\\ 
&=\exp(a \varphi ^ \wedge)R_0 \\
&=\exp((a \varphi )^ \wedge)R_0 
\end{aligned}$$

上面公式中的\(a\)表示插值点的值,其取值范围在[0,1]，当\(a = 0\)时，对应值为旋转矩阵\(R_0\)，当\(a = 1\)时，对应值为旋转矩阵\(R_1\)，公式中的\(\varphi\)对应的矩阵为 \(R_1R_0^T\)

 

2 Sim(3)位姿矩阵插值

先申明如下SE(3)矩阵格式

(2)$$
T_0 = \begin{bmatrix}  
s_0R_0 & t_0\\
0 & 1
\end{bmatrix}\\T_1 = \begin{bmatrix}  
s_1R_1 & t_1\\
0 & 1
\end{bmatrix}\\T = \begin{bmatrix}  
sR & t\\
0 & 1
\end{bmatrix}$$

即，现在我们想找到一种插值模型，使得\(a = 0\)时，插值的\(T = T_0\);当\(a = 1\)时，\(T = T_1\)，当\(a\)取其它值时，插值的\(T\)满足Sim(3)性质。其中插值点\(a\)取值在[0,1]。

即现在需要通过\(T_0,T_1\)插值出\(T\)当中的\(s,R,t\)

2.1 无光心约束插值

与旋转矩阵插值类似，可以构建如下的插值模型

(3)$$\begin{aligned}  
T &= (T_1T_0^{-1})^aT_0\\   
&= T_{10}^aT_0\\   
&=\exp(\xi ^ \wedge)^aT_0\\ 
&=\exp(a \xi ^ \wedge)T_0 \\
&=\exp((a \xi) ^ \wedge)T_0
\end{aligned}$$

这里将公式(3)展开有

(4)$$T=\begin{bmatrix}  
(s_1/s_0)^a(R_1R_0^T)^a & a(t_1 – (s_1/s_0)R_1R_0^Tt_0)\\
0 & 1
\end{bmatrix}
\begin{bmatrix}  
s_0R_0 & t_0\\
0 & 1
\end{bmatrix}$$

通过公式(4)可以得到最终的插值结果

(5)$$\begin{aligned}  
s &= s_0(\frac{s_1}{s_0})^a\\
R &= (R_1R_0^T)^aR_0\\
t &= (\frac{s_1}{s_0})^a(R_1R_0^T)^at_0 + a(t_1-\frac{s_1}{s_0}R_1R_0^Tt_0)
\end{aligned}$$

2.2 有光心约束插值

当我们使用Sim(3)来表示一个位姿的时候，我们知道一种位姿在空间会对应一个相机中心坐标。当给定两个位姿，然后在它们之间进行插值时，我们可以假设它们插值的相机中心也满足两个位姿相机中心的线性插值。即有如下约束

(6)$$\frac{R^Tt}{s} =(1-a) \frac{R_0^Tt_0}{s_0}  + a\frac{R_1^Tt_1}{s_1}$$

公式(6)是一个含有3个等式的约束，如果插值结果的旋转\(R\)与缩放部分\(s\)采用公式(5)的结果，那结合公式(6)刚好可以求出插值的\(t\),因此最终的插值结果可表示为

(7)$$\begin{aligned}  
s &= s_0(\frac{s_1}{s_0})^a\\
R &= (R_1R_0^T)^aR_0\\
t &=sR ((1-a) \frac{R_0^Tt_0}{s_0}  + a\frac{R_1^Tt_1}{s_1})
\end{aligned}$$

1 前言
在使用单目相机的无人驾驶中，有时我们需要构建位姿与现实中某一个平面的关系，以便为后续其它操作提供基础！

 

2 原理

2.1 位姿与平面间的耦合

假设有相机1（\(s_1\)、\(R_1\)、\(t_1\)）与相机2（\(s_2\)、\(R_2\)、\(t_2\)），一个平面在相机1坐标系下的方程为
(1)
$$
n^Tp+d=0
$$
上面公式中\(n\)是相机平面法向量，\(p\)是一个在相机1坐标系下路平面上的点，\(d\)实际就是相机到平面的距离。对公式(1)进行变形有
(2)
$$
1=-\frac{n^Tp}{d}
$$

根据SLAM之位姿到位姿的相对测量中的公式(5)可以得到相机2与相机1的相对位姿关系
(3)
$$
\begin{aligned}
R &= R_2R_1^Ts_2/s_1
\\
t &= t_2 – Rt_1
\\
s_2R_2 &= s_1RR_1
\\
t_2 &= t + Rt_1
\end{aligned}
$$

现在(1)的平面上，有一个点被相机1与相机2看到，其在世界坐标系下的位置为\(p_w\)，其在相机1坐标系下的坐标为\(p_c\)，其在相机1平面上看到的坐标为\(c_1(u_1,v_1,1)\)，在相机2平面上看到点坐标为\(c_2(u_2,v_2,1)\)，假设相机1与相机2的内参分别为\(K_1\)、\(K_2\)，现在令
(4)
$$
p_1=K_1^{-1}c_1\\
p_2=K_2^{-1}c_2
$$

根据针孔相机模型可以得到
(5)
$$
z_1c_1 = K_1(s_1R_1p_w+t_1)=K_1p_c\\
\Downarrow \\
p_c = z_1K_1^{-1}c_1 = z_1p_1
$$

(6)
$$
\begin{aligned}
z_2c_2&=K_2(s_2R_2p_w+t_2)
\\
&=K_2(s_1RR_1p_w+t+Rt_1)
\\
&=K_2(R(s_1R_1p_w+t1)+t)
\\
&=K_2(Rp_c+t)
\\
&=K_2(Rp_c+t(-\frac{n^Tp_c}{d}))
\\
&=K_2(R-\frac{tn^T}{d})p_c
\\
&=K_2(R-\frac{tn^T}{d})z_1p_1
\end{aligned}
\\
\Downarrow
\\
z_2p_2=z_2K_2^{-1}c_2=(R-\frac{tn^T}{d})z_1p_1
$$

公式(6)建立了平面与位姿的关系。这里需要理解：

1)公式当中是2个位姿共同耦合1个平面。

2)耦合的方程中还包含两个相机共同观测到平面上的点。

3)公式当中的平面是相机1坐标系下的平面，如果要建立世界坐标系下平面与位姿的关系，则将平面方程进行坐标系转换一下即可（相机1坐标系转到世界坐标系）

 

2.2 根据观测求取相对测量与平面
公式(5)、(6)当中的\(z_1\)、\(z_2\)是使得相机归一化后的最后一维的值，这里令\(s = z_2/z_1\)则公式(6)最终可表示为 
(7)
$$
sp_2=(R-\frac{tn^T}{d})p_1
$$

且复原到\(c_1\)、\(c_2\)有 
(8)
$$
sc_2=K_2(R-\frac{tn^T}{d})K_1^{-1}c_1=Hc_1
$$
SLAM当中将\(H\)叫做单应矩阵，一般会使用这个矩阵来恢复出相对测量\(R\)、\(t\)，下面将公式(8)展开有
(9)
$$
s\begin{bmatrix}
u_2\\ 
v_2\\
1
\end{bmatrix}
=
\begin{bmatrix}
h_1 & h_2 & h_3\\
h_4 & h_5 & h_6\\
h_7 & h_8 & h_9
\end{bmatrix}
\begin{bmatrix}
u_1\\ 
v_1\\
1
\end{bmatrix}
\\
\Downarrow
\\
\begin{aligned}
su_2 &= h_1u_1+h_2v_1+h_3\\
sv_2 &= h_4u_1+h_5v_1+h_6\\
s&=h_7u_1+h_8v_1+h_9
\end{aligned}
$$

在公式(9)当中消去\(s\)有
(10)
$$
\begin{aligned}
u_2 &= \frac{h_1u_1+h_2v_1+h_3}{h_7u_1+h_8v_1+h_9}\\
v_2 &= \frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9}
\end{aligned}
$$
仔细观察公式(10)可以发现，对于\(H\)中的元素同时乘以一个非0的常数，并不改变公式(10)的结果，且这样操作后等价于\(H\)的特征值同时乘以这个非0常数。因此这里不妨假设\(h_9=1\)，当然也可以假设\(H\)的其它元素值为1(这里需要注意，假设的元素的值本身可能为0，所以保险做法是取几个元素置为1分别尝试计算\(R\)、\(t\))，因此最终方程(10)可变为
(11)
$$
\begin{bmatrix}
u_1 & v_1 & 1 & 0 & 0 & 0 & -u_1u_2 & -v_1u_2
\\
0 & 0 & 0 & u_1 & v_1 & 1 & -u_1v_2 & -v_1v_2
\end{bmatrix}
\begin{bmatrix}
h_1\\
h_2\\
h_3\\
h_4\\
h_5\\
h_6\\
h_7\\
h_8
\end{bmatrix}
=
\begin{bmatrix}
u_2\\
v_2
\end{bmatrix}
$$

当有多组观测点都落在这个平面时，可以通过组装的线性方程组求解出一个最佳的\(H\)，由于在求解过程中假设\(h_9=1\)，因此真实的\(H\)可表示为\(\alpha H\)

因此根据公式(8)最终可以得到
(12)
$$
(R-\frac{tn^T}{d}) = \alpha K_2^{-1}HK_1 = \alpha G
$$

对于公式(1)的平面方程来说，两边同时乘以一个非0系数，不改变平面性质，因此对于公式(12)当中的\(d\)，不妨令其等于1(实际当中一般需要建立相机与平面的关系时，这个相机一般距离平面有一定距离，因此参数\(d\)为0的可能性很小)，则公式(12)则变成
(13)
$$
(R-tn^T) = \alpha G
$$
在上面的公式中\(G\)已经得到，对于公式(13)的形式，根据Ezio Malis[1]的结果，最终可以求解出\(R\)、\(t\)、\(n\)、\(\alpha \)

 

2.3 已知相对测量求平面方程

和上面类似，仔细观察公式(7)可知，同时缩放\(n\)、\(d\)一个非0系数，它们的平面性质不会改变(公式(1)当中两边同时乘以一个非0常数，表示的平面不变)，这时不妨设$d=1$，则公式(7)最终变为
(14)
$$
sp_2=Rp_1-tn^Tp_1=Rp_1-tp_1^Tn
$$
将公式(14)中的未知数都移动到一边有
(15)
$$
tp_1^Tn+sp_2=Rp_1
$$

从公式(15)可以看出，这个里面有3个线性方程4个未知数(\(n\)为3个未知数，\(s\)为1个未知数)。

对于一个平面上的多个匹配点对，由于每个(匹配对)点对应的\(s\)不一样，因此如果平面上同时有\(m\)个点被两个相机同时看见，这时根据(15)可以构建一个$3m \times (3 + m)$的线性矩阵来求解出平面方程系数\(n\)。

如果两个相机的连线与所在平面平行，现在设两个相机连线在世界坐标系下的方向向量为\(v\)，则它们与公式(1)当中的\(n\)满足如下关系
(16)
$$
n^TR_1v=0
$$
上面公式(16)是一个3个未知数(\(n\))的线性方程，因此如果将公式(16)带入公式(15)，\(n\)的求解维度可以进一步降低到2维。进而可以进行求解。

参考

[1] Ezio Malis, Manuel Vargas. Deeper understanding of the homography decomposition for vision-based control. 2007