se(3)中对于三维坐标\(p\)进行Ｔ变换后的坐标\(Tp=\exp(\xi^{\wedge})\)与位姿李代数se(3)的导数，采用扰动模型,左乘\(\exp(\delta\xi^{\wedge})\),然后求极限:
\[\begin{aligned} \frac { \partial ( T p ) } { \partial \delta \xi } & = \lim _ { \delta \xi \rightarrow 0 } \frac { \exp \left( \delta \xi ^ { \wedge } \right) \exp \left( \xi ^ { \wedge } \right) p - \exp \left( \xi ^ { \wedge } \right) p } { \delta \xi } \\ & \approx \lim _ { \delta \xi \rightarrow 0 } \frac { \left( I + \delta \xi ^ { \wedge } \right) \exp \left( \xi ^ { \wedge } \right) p - \exp \left( \xi ^ { \wedge } \right) p } { \delta \xi } \\&=\lim_{\delta \rightarrow 0} \frac{\delta\xi^{\wedge}\exp(\xi^{\wedge})p}{\delta\xi} \end{aligned}\]
再针对分子\(\delta\xi^{\wedge}\exp(\xi^{\wedge})p\)进行分析，对扰动\(\delta\xi^{\wedge}\)展开为旋转角扰动和平移扰动，即为\(\delta\xi^{\wedge}＝\left[ \begin{array}{c}{\delta \phi ^{\wedge}}\\{\delta \rho} \end{array}\right] \iff \left[ \begin{array}{c}{\delta \phi ^{\wedge}}& {\delta \rho}\\{0^T}&{0^T} \end{array}\right]\)；而\(\exp(\xi^{\wedge})p\)即为经过变换后的坐标点\(Rp+t\)，为了满足运算表示需要将他记为齐次坐标即：\(\exp(\xi^{\wedge})p \iff \left[\begin{array}{c}{Rp+t}\\{1}\end{array}\right] = \left[ \begin{array}{c}{X'}\\{Y'}\\{Z'}\\{1}\end{array}\right]\)
，然后对分子进行展开：
\[\begin{aligned}\delta\xi^{\wedge}\exp(\xi^{\wedge})p&=\left[ \begin{array}{c}{\delta \phi ^{\wedge}}& {\delta \rho}\\{0^T}&{0^T} \end{array}\right] \left[\begin{array}{c}{Rp+t}\\{1}\end{array}\right] \\ &=\left[ \begin{array}{c}{\delta\phi^{\wedge}(Rp+t)+\delta\rho}\\{0}\end{array} \right] \end{aligned} \]
可以将\(\partial\delta\xi\)，拆解成为两个部分的求导，即对平移的求导\(\partial\delta\rho\)与对旋转的求导 \(\partial\delta\phi^{\wedge}\),即如下：
\[\begin{aligned}&\frac{\partial\delta\phi^{\wedge}(Rp+t)+\delta\rho}{\partial \delta\phi^{\wedge}}=-(Rp+t)^{\wedge} \\&\frac{\partial\delta\phi^{\wedge}(Rp+t)+\delta\rho^{\wedge}}{\partial \delta\rho}=I\end{aligned}\]
注意，\(\delta\phi^{\wedge}(Rp+t)\)可以看成是两个向量的內积，则有\(\delta\phi^{\wedge}(Rp+t)=-(Rp+t)^{\wedge}\delta\phi\) 1
所以我们最终求得对于se(3)在李代数的求导为:
\[\frac{\partial(Tp)}{\partial\delta\xi}=\left[\begin{array}{c}&{\frac{\partial\delta\phi^{\wedge}(Rp+t)+\delta\rho}{\partial \delta\rho}}&{\frac{\partial\delta\phi^{\wedge}(Rp+t)+\delta\rho}{\partial \delta\phi^{\wedge}}}\\&{\frac{\partial 0}{\partial \delta\rho}}&{\frac{0}{\partial \delta\phi^{\wedge}}}\end{array}\right]=\left[\begin{array}{c c}{I} & {-(Rp+t)^{\wedge}} \\{0^{T}}&{0^{T}}\end{array}\right]\]
(*注意g2o种将旋转分量的导数放在三列，平移分量的导数放在后三列)
若已知变换后的坐标为\(\left[ \begin{array}{c}{X'}\\{Y'}\\{Z'}\end{array}\right]=Rp+t\),则该导数为：
\[J=\left[\begin{array}{c c}{1} & {0} & {0} & {0} & {Z'} & {-Y}\\{0}&{1}&{0} &{-Z'} &{0}&{X'}\\{0}&{0}&{1}&{Y'}&{-X'}&{0}\end{array}\right]\]
使用非线性优化方法对3D-3D的匹配点\(P=\left[\begin{array}{c}{X}\\{Y}\\{Z}\end{array}\right],P'=\left[\begin{array}{cc}{X'}\\{Y'}\\{Z'}\end{array}\right]\)进行位姿估计时，建立目标函数如下：
\[\min_{\xi}=\frac{1}{2}\sum_{i=1}^{n}\left||{p_i-\exp(\xi^{\wedge})p'} \right||_2^2\]
记录经过单个坐标变换的点\(\exp(\xi^{\wedge})p'=\left[\begin{array}{c}{\hat{x}}\\\hat{y}\\{\hat{z}}\end{array}\right]\)对单个误差项求导：
\[ \frac{\partial(p-\exp(\xi^{\wedge})p')}{\partial \xi^{\wedge}}=\frac{-\partial (\exp(\xi^{\wedge})p')}{\partial \xi^{\wedge}}=\left[\begin{array}{c c}{1} & {0} & {0} & {0} & {\hat{z}} & {\hat{-y}}\\{0}&{1}&{0} &{-\hat{z}} &{0}&{\hat{x}}\\{0}&{0}&{1}&{\hat{y}}&{-\hat{x}}&{0}\end{array}\right]\]
[1]向量的反对称矩阵与內积的联系：
\[a \times b = \left\| \begin{array} { c c c } { i } & { j } & { k } \\ { a _ { 1 } } & { a _ { 2 } } & { a _ { 3 } } \\ { b _ { 1 } } & { b _ { 2 } } & { b _ { 3 } } \end{array} \right\| = \left[ \begin{array} { c } { a _ { 2 } b _ { 3 } - a _ { 3 } b _ { 2 } } \\ { a _ { 3 } b _ { 1 } - a _ { 1 } b _ { 3 } } \\ { a _ { 1 } b _ { 2 } - a _ { 2 } b _ { 1 } } \end{array} \right] = \left\| \begin{array} { c c c }{0}&{-a_3}&{a_2}\\{a_3}&{0}&{-a_1}\\{-a_2}&{a_1}&{0} \end{array}\right\|b=a^{\wedge}b=-b^{\wedge}a\]
参考:
高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017