Adjoint of SE(3)

以前看的书都提到 SE(3) 和 se(3) 的 Adjoint，但是并没有讲这个东西是干什么用的，只是给了一堆性质。这东西来自群论。

参考 Lie Groups for 2D and 3D Transformations 的 2.3。

In Lie groups, it is often necessary to transform a tangent vector from the tangent space around one element to the tangent space. The adjoint performs this transformation.

Tangent Vector 是啥？这个和 Manifold(流型) 有关系。可以看一看 A Framework for Sparse, Non-Linear Least Squares Problems on Manifolds 的 Manifolds 章节，motivation 小结写了为什么要用 Manifold。简单说，Manifold 是一个非线性空间，但是在局部的小区域可以用线性空间拟合。如同一个曲面，在曲面上可导点处的局部性质可以用该点切平面描述。用线性空间拟合后，在局部小区域就可以使用优化算法进行优化了。Tangent Vector 就是这种线性空间中的元素，也就是优化计算出来的增量。

定义

参考 Lie Groups for 2D and 3D Transformations 里的定义，按照我习惯的符号系统，Adjoint 定义如下：

[egin{align} ext{Exp}( ext{Ad}_{mathbf{T}}cdotpmb{xi}) doteq mathbf{T} ext{Exp}(pmb{xi}) mathbf{T}^{-1} label{eq:adj_def} end{align} ]

这是一个同构映射(Homomorphism)，按照维基百科的定义，同构映射是在几何结构(Algebraic structure)中保持结构的（structure-preserving）映射。几何结构中就包括了群（Groups），“结构”没有找到定义，不好理解。也可以参考 Naive Lie Theory 2.2 Crash course on homomorphism，对于群而言，保持结构就是保持定义在群中的二元运算（SE(3) 而言就是矩阵乘法）。公式表达如下：

[egin{align} varphi(g) &= mathbf{T} g mathbf{T}^{-1} otag \ varphi(g_1g_2) &= varphi(g_1)varphi(g_2) end{align} otag]

对于当前的映射，这个是显然成立的。

表达

现在计算 ( ext{Ad}_{mathbf{T}})，参考 State Estimation for Robotics 的公式 (7.33) (7.48)，按照我习惯的符号系统，如下：

[egin{align} expleft(pmb{xi}^{wedge} ight) &= sum_{n=0}^{infty} frac{1}{n!}left(pmb{xi}^{wedge} ight)^n otag \ &= sum_{n=0}^{infty} frac{1}{n!}left(egin{bmatrix}pmb{ ho} \ pmb{phi}end{bmatrix}^{wedge} ight)^n otag \ &= sum_{n=0}^{infty} frac{1}{n!} egin{bmatrix}pmb{phi}^{wedge} & pmb{ ho} \ mathbf{0}^T & 0end{bmatrix} otag \ &= egin{bmatrix} sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge} ight)^n & left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(pmb{phi}^{wedge} ight)^n ight]pmb{ ho} \ mathbf{0}^T & 1 end{bmatrix} otag \ &= egin{bmatrix} mathbf{R} & mathbf{t} \ mathbf{0}^T & 1end{bmatrix} in SE(3) label{eq:exp_xi}end{align}]

[egin{align} mathbf{R}mathbf{t}^{wedge}mathbf{R}^T = (mathbf{R}mathbf{t})^{wedge} label{eq:Rt_hat}end{align} ]

按照 Adjoint 的定义 ( ef{eq:adj_def}) 有如下推导：

[egin{align} ext{Ad}_{mathbf{T}}cdotpmb{xi} &= ext{Log}left(mathbf{T} ext{Exp}(pmb{xi}) mathbf{T}^{-1} ight) otag \ &= ext{Log}left(egin{bmatrix} mathbf{R} & mathbf{t} \ mathbf{0}^T & 1end{bmatrix} egin{bmatrix} sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge} ight)^n & left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(pmb{phi}^{wedge} ight)^n ight]pmb{ ho} \ mathbf{0}^T & 1 end{bmatrix} egin{bmatrix} mathbf{R}^T & -mathbf{R}^Tmathbf{t} \ mathbf{0}^T & 1end{bmatrix} ight) otag \ &= ext{Log}left( egin{bmatrix} mathbf{R} left[sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge} ight)^n ight] & mathbf{R}left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(pmb{phi}^{wedge} ight)^n ight]pmb{ ho}+mathbf{t} \ mathbf{0}^T & 1 end{bmatrix} egin{bmatrix} mathbf{R}^T & -mathbf{R}^Tmathbf{t} \ mathbf{0}^T & 1end{bmatrix} ight) otag \ &= ext{Log}left( egin{bmatrix} mathbf{R} left[sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge} ight)^n ight]mathbf{R}^T & -mathbf{R} left[sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge} ight)^n ight]mathbf{R}^Tmathbf{t} + mathbf{R}left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(pmb{phi}^{wedge} ight)^n ight]pmb{ ho}+mathbf{t} \ mathbf{0}^T & 1 end{bmatrix} ight) otag \ &stackrel{( ef{eq:Rt_hat})}{=} ext{Log}left( egin{bmatrix} left[sum_{n=0}^{infty}frac{1}{n!}left((mathbf{R}pmb{phi})^{wedge} ight)^n ight] & -left[sum_{n=0}^{infty}frac{1}{n!}left((mathbf{R}pmb{phi})^{wedge} ight)^n ight]mathbf{t} +left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((mathbf{R}pmb{phi})^{wedge} ight)^n ight]mathbf{R}pmb{ ho}+mathbf{t} \ mathbf{0}^T & 1 end{bmatrix} ight) otag \ &= egin{bmatrix}ar{pmb{ ho}} \ ar{pmb{phi}}end{bmatrix} end{align}]

上式 ( ext{Log}(cdot)) 之后的结果是一个 (6 imes 1) 的向量（等于 ( ext{Ad}_{mathbf{T}}cdotpmb{xi})），而 (pmb{xi}) 是 (6 imes 1)，所以 ( ext{Ad}_{mathbf{T}}) 是 (6 imes 6)。上式的最后一个等号，参照公式 ( ef{eq:exp_xi})，可以得到

[egin{align} ar{pmb{phi}} &= mathbf{R}pmb{phi} otag \ &left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(ar{pmb{phi}}^{wedge} ight)^n ight]ar{pmb{ ho}} otag \ &= -left[sum_{n=0}^{infty}frac{1}{n!}left((mathbf{R}pmb{phi})^{wedge} ight)^n ight]mathbf{t} +left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((mathbf{R}pmb{phi})^{wedge} ight)^n ight]mathbf{R}pmb{ ho}+mathbf{t} otag \ &= -left[sum_{n=0}^{infty}frac{1}{n!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{t} +left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{R}pmb{ ho}+mathbf{t} otag \ &= left(mathbf{I} - left[sum_{n=0}^{infty}frac{1}{n!}left((ar{pmb{phi}})^{wedge} ight)^n ight] ight)mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{R}pmb{ ho} otag \ &= left(mathbf{I} - frac{1}{0!}left((ar{pmb{phi}})^{wedge} ight)^0 - left[sum_{n=1}^{infty}frac{1}{n!}left((ar{pmb{phi}})^{wedge} ight)^n ight] ight)mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{R}pmb{ ho} otag \ &= -left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^{n+1} ight] mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{R}pmb{ ho} otag \ &= -left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight](ar{pmb{phi}})^{wedge}mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{R}pmb{ ho} otag \ &= left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{t}^{wedge}ar{pmb{phi}} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight]mathbf{R}pmb{ ho} otag \ &= left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((ar{pmb{phi}})^{wedge} ight)^n ight](mathbf{t}^{wedge}mathbf{R}pmb{phi} + mathbf{R}pmb{ ho})end{align}]

所以

[egin{align} ext{Ad}_{mathbf{T}}cdotpmb{xi} &doteq ext{Ad}_{mathbf{T}} cdot egin{bmatrix}pmb{ ho} \ pmb{phi}end{bmatrix} otag \ &doteq egin{bmatrix}ar{pmb{ ho}} \ ar{pmb{phi}}end{bmatrix} otag \ &= egin{bmatrix} mathbf{t}^{wedge}mathbf{R}pmb{phi} + mathbf{R}pmb{ ho} \ mathbf{R}pmb{phi} end{bmatrix} otag \ &= egin{bmatrix} mathbf{R} & mathbf{t}^{wedge}mathbf{R} \ mathbf{0}^T & mathbf{R}end{bmatrix} cdot egin{bmatrix}pmb{ ho} \ pmb{phi}end{bmatrix} end{align}]

[egin{align} ext{Ad}_{mathbf{T}} = egin{bmatrix} mathbf{R} & mathbf{t}^{wedge}mathbf{R} \ mathbf{0}^T & mathbf{R}end{bmatrix} end{align} ]

应用

前面讲到了这个东西就是将一个 Tangent Vector 从一个 Vector Space 转换到另外一个 Vector Space。

用定义 ( ef{eq:adj_def}) 求 DSO 中相对位置姿态对绝对位置姿态的偏导，也是解决我在 DSO windowed optimization 公式 开头提到的问题。

首先，涉及到的位置姿态关系在线性化点处如下：

[egin{align} mathbf{T}_{th} = mathbf{T}_{wt}^{-1} mathbf{T}_{wh} end{align} ]

在某一次优化迭代中，(mathbf{T}_{th}, mathbf{T}_{wh}, mathbf{T}_{wt}) 已经离开线性化点一段距离 (pmb{xi}_{th}, pmb{xi}_{h}, pmb{xi}_{t})，在本次迭代中的更新为 (pmb{xi}_{th} leftarrow pmb{xi}_{th} + delta pmb{xi}_{th}, pmb{xi}_{h} leftarrow pmb{xi}_{h} + delta pmb{xi}_{h}, pmb{xi}_{t} leftarrow pmb{xi}_{t} + delta pmb{xi}_{t})。

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th})mathbf{T}_{th} = mathbf{T}_{wt}^{-1} ext{Exp}(pmb{xi}_h+delta pmb{xi}_{h}) mathbf{T}_{wh} end{align} ]

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th}) &= mathbf{T}_{wt}^{-1} ext{Exp}(pmb{xi}_h+delta pmb{xi}_{h}) mathbf{T}_{wt} mathbf{T}_{wt}^{-1} mathbf{T}_{wh} mathbf{T}_{th}^{-1} otag \ &stackrel{( ef{eq:adj_def})}{=} ext{Exp}( ext{Ad}_{mathbf{T}_{wt}^{-1}}(pmb{xi}_h+delta pmb{xi}_{h})) mathbf{T}_{wt}^{-1} mathbf{T}_{wh} mathbf{T}_{th}^{-1} otag \ &= ext{Exp}( ext{Ad}_{mathbf{T}_{wt}^{-1}}(pmb{xi}_h+delta pmb{xi}_{h})) end{align}]

[egin{align} pmb{xi}_{th}+delta pmb{xi}_{th} = ext{Ad}_{mathbf{T}_{wt}^{-1}}(pmb{xi}_h+delta pmb{xi}_{h}) end{align} ]

所以

[frac{partial pmb{xi}_{th}}{partial pmb{xi}_h} = ext{Ad}_{mathbf{T}_{wt}^{-1}} ]

这个结果和 Lie Groups for 2D and 3D Transformations 的结论（公式(97)）一致。

再算 (frac{partial pmb{xi}_{th}}{partial pmb{xi}_t})。

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th})mathbf{T}_{th} = ( ext{Exp}(pmb{xi}_t+delta pmb{xi}_{t})mathbf{T}_{wt})^{-1} mathbf{T}_{wh} end{align} ]

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th}) &= mathbf{T}_{wt}^{-1} ext{Exp}(-(pmb{xi}_t+delta pmb{xi}_{t})) mathbf{T}_{wt}mathbf{T}_{wt}^{-1}mathbf{T}_{wh}mathbf{T}_{th}^{-1} otag \ &= ext{Exp}(- ext{Ad}_{mathbf{T}_{wt}^{-1}}(pmb{xi}_t+delta pmb{xi}_{t}))end{align}]

所以

[frac{partial pmb{xi}_{th}}{partial pmb{xi}_t} = - ext{Ad}_{mathbf{T}_{wt}^{-1}} ]

但是这个结果和 DSO windowed optimization 公式 中提到的，从代码中推算出来的结果不同。这只是两边所理解的 (pmb{xi}) 不同而已。

看最关键的一行将优化计算的 se(3) 更新到 SE(3) 的代码，是“worldToCam”，所以对应的位姿应该用 (mathbf{T}_{cw}) 表达，而不是“camToWorld”—— (mathbf{T}_{wc})。

重新算一遍。这次涉及到的位置姿态关系在线性化点处如下：

[egin{align} mathbf{T}_{th} = mathbf{T}_{tw} mathbf{T}_{hw}^{-1} end{align} ]

算 (frac{partial pmb{xi}_{th}}{partial pmb{xi}_h})。

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th})mathbf{T}_{th} = mathbf{T}_{tw} ( ext{Exp}(pmb{xi}_h+delta pmb{xi}_{h}) mathbf{T}_{hw})^{-1} end{align} ]

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th}) &= mathbf{T}_{tw} mathbf{T}_{hw}^{-1} ext{Exp}(-(pmb{xi}_h+delta pmb{xi}_{h}))mathbf{T}_{th}^{-1} otag \ &= mathbf{T}_{th} ext{Exp}(-(pmb{xi}_h+delta pmb{xi}_{h}))mathbf{T}_{th}^{-1} otag \ &stackrel{( ef{eq:adj_def})}{=} ext{Exp}(- ext{Ad}_{mathbf{T}_{th}}(pmb{xi}_h+delta pmb{xi}_{h})) end{align}]

[egin{align} pmb{xi}_{th}+delta pmb{xi}_{th} = - ext{Ad}_{mathbf{T}_{th}}(pmb{xi}_h+delta pmb{xi}_{h}) end{align} ]

所以

[frac{partial pmb{xi}_{th}}{partial pmb{xi}_h} = - ext{Ad}_{mathbf{T}_{th}} ]

算 (frac{partial pmb{xi}_{th}}{partial pmb{xi}_t})。

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th})mathbf{T}_{th} = ext{Exp}(pmb{xi}_t+delta pmb{xi}_{t})mathbf{T}_{tw} mathbf{T}_{hw}^{-1} end{align} ]

[egin{align} ext{Exp}(pmb{xi}_{th}+delta pmb{xi}_{th}) &= ext{Exp}(pmb{xi}_t+delta pmb{xi}_{t})mathbf{T}_{tw} mathbf{T}_{hw}^{-1}mathbf{T}_{th}^{-1} otag \ &= ext{Exp}(pmb{xi}_t+delta pmb{xi}_{t}) end{align}]

[egin{align} pmb{xi}_{th}+delta pmb{xi}_{th} = pmb{xi}_t+delta pmb{xi}_{t} end{align} ]

所以

[frac{partial pmb{xi}_{th}}{partial pmb{xi}_t} = mathbf{I} ]