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 定义如下：

[begin{align} text{Exp}(text{Ad}_{mathbf{T}}cdotpmb{xi}) doteq mathbf{T} text{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) 而言就是矩阵乘法）。公式表达如下：

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

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

表达

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

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

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

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

[begin{align} text{Ad}_{mathbf{T}}cdotpmb{xi} &= text{Log}left(mathbf{T} text{Exp}(pmb{xi}) mathbf{T}^{-1}right)notag \ &= text{Log}left(begin{bmatrix} mathbf{R} & mathbf{t} \ mathbf{0}^T & 1end{bmatrix} begin{bmatrix} sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge}right)^n & left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(pmb{phi}^{wedge}right)^nright]pmb{rho} \ mathbf{0}^T & 1 end{bmatrix} begin{bmatrix} mathbf{R}^T & -mathbf{R}^Tmathbf{t} \ mathbf{0}^T & 1end{bmatrix}right) notag \ &= text{Log}left( begin{bmatrix} mathbf{R} left[sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge}right)^nright] & mathbf{R}left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(pmb{phi}^{wedge}right)^nright]pmb{rho}+mathbf{t} \ mathbf{0}^T & 1 end{bmatrix} begin{bmatrix} mathbf{R}^T & -mathbf{R}^Tmathbf{t} \ mathbf{0}^T & 1end{bmatrix} right) notag \ &= text{Log}left( begin{bmatrix} mathbf{R} left[sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge}right)^nright]mathbf{R}^T & -mathbf{R} left[sum_{n=0}^{infty}frac{1}{n!}left(pmb{phi}^{wedge}right)^nright]mathbf{R}^Tmathbf{t} + mathbf{R}left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(pmb{phi}^{wedge}right)^nright]pmb{rho}+mathbf{t} \ mathbf{0}^T & 1 end{bmatrix} right) notag \ &stackrel{(ref{eq:Rt_hat})}{=} text{Log}left( begin{bmatrix} left[sum_{n=0}^{infty}frac{1}{n!}left((mathbf{R}pmb{phi})^{wedge}right)^nright] & -left[sum_{n=0}^{infty}frac{1}{n!}left((mathbf{R}pmb{phi})^{wedge}right)^nright]mathbf{t} +left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((mathbf{R}pmb{phi})^{wedge}right)^nright]mathbf{R}pmb{rho}+mathbf{t} \ mathbf{0}^T & 1 end{bmatrix} right)notag \ &= begin{bmatrix}bar{pmb{rho}} \ bar{pmb{phi}}end{bmatrix} end{align}]

上式 (text{Log}(cdot)) 之后的结果是一个 (6 times 1) 的向量（等于 (text{Ad}_{mathbf{T}}cdotpmb{xi})），而 (pmb{xi}) 是 (6 times 1)，所以 (text{Ad}_{mathbf{T}}) 是 (6 times 6)。上式的最后一个等号，参照公式 (ref{eq:exp_xi})，可以得到
[begin{align} bar{pmb{phi}} &= mathbf{R}pmb{phi} notag \ &left[sum_{n=0}^{infty}frac{1}{(n+1)!}left(bar{pmb{phi}}^{wedge}right)^nright]bar{pmb{rho}} notag \ &= -left[sum_{n=0}^{infty}frac{1}{n!}left((mathbf{R}pmb{phi})^{wedge}right)^nright]mathbf{t} +left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((mathbf{R}pmb{phi})^{wedge}right)^nright]mathbf{R}pmb{rho}+mathbf{t} notag \ &= -left[sum_{n=0}^{infty}frac{1}{n!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{t} +left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{R}pmb{rho}+mathbf{t} notag \ &= left(mathbf{I} - left[sum_{n=0}^{infty}frac{1}{n!}left((bar{pmb{phi}})^{wedge}right)^nright]right)mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{R}pmb{rho} notag \ &= left(mathbf{I} - frac{1}{0!}left((bar{pmb{phi}})^{wedge}right)^0 - left[sum_{n=1}^{infty}frac{1}{n!}left((bar{pmb{phi}})^{wedge}right)^nright]right)mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{R}pmb{rho} notag \ &= -left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^{n+1}right] mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{R}pmb{rho} notag \ &= -left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright](bar{pmb{phi}})^{wedge}mathbf{t} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{R}pmb{rho} notag \ &= left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{t}^{wedge}bar{pmb{phi}} + left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright]mathbf{R}pmb{rho} notag \ &= left[sum_{n=0}^{infty}frac{1}{(n+1)!}left((bar{pmb{phi}})^{wedge}right)^nright](mathbf{t}^{wedge}mathbf{R}pmb{phi} + mathbf{R}pmb{rho})end{align}]

所以

[begin{align} text{Ad}_{mathbf{T}}cdotpmb{xi} &doteq text{Ad}_{mathbf{T}} cdot begin{bmatrix}pmb{rho} \ pmb{phi}end{bmatrix}notag \ &doteq begin{bmatrix}bar{pmb{rho}} \ bar{pmb{phi}}end{bmatrix} notag \ &= begin{bmatrix} mathbf{t}^{wedge}mathbf{R}pmb{phi} + mathbf{R}pmb{rho} \ mathbf{R}pmb{phi} end{bmatrix} notag \ &= begin{bmatrix} mathbf{R} & mathbf{t}^{wedge}mathbf{R} \ mathbf{0}^T & mathbf{R}end{bmatrix} cdot begin{bmatrix}pmb{rho} \ pmb{phi}end{bmatrix} end{align}]

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

应用

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

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

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

[begin{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})。

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

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

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

所以

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

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

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

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

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

所以

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

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

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

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

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

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

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

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

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

所以

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

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

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

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

[begin{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}]