下面主要记录一些李代数涉及到的求导，其中主要提供左扰动与常规加法更新求导两种方式。因为不同的求导方式，会影响变量的更新法则，所以要注意区分。
推导过程主要用到的公式可参考李群李代数工具。
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}$$