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}}\cdot\pmb{\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} \exp\left(\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 & 0\end{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)^n\right]\pmb{\rho} \\ \mathbf{0}^T & 1 \end{bmatrix} \notag \\
&= \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1\end{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}}\cdot\pmb{\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 & 1\end{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)^n\right]\pmb{\rho} \\ \mathbf{0}^T & 1 \end{bmatrix} \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T\mathbf{t} \\ \mathbf{0}^T & 1\end{bmatrix}\right) \notag \\
&= \text{Log}\left( \begin{bmatrix} \mathbf{R} \left[\sum_{n=0}^{\infty}\frac{1}{n!}\left(\pmb{\phi}^{\wedge}\right)^n\right] & \mathbf{R}\left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left(\pmb{\phi}^{\wedge}\right)^n\right]\pmb{\rho}+\mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T\mathbf{t} \\ \mathbf{0}^T & 1\end{bmatrix} \right) \notag \\
&= \text{Log}\left( \begin{bmatrix} \mathbf{R} \left[\sum_{n=0}^{\infty}\frac{1}{n!}\left(\pmb{\phi}^{\wedge}\right)^n\right]\mathbf{R}^T & -\mathbf{R} \left[\sum_{n=0}^{\infty}\frac{1}{n!}\left(\pmb{\phi}^{\wedge}\right)^n\right]\mathbf{R}^T\mathbf{t} + \mathbf{R}\left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left(\pmb{\phi}^{\wedge}\right)^n\right]\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)^n\right] & -\left[\sum_{n=0}^{\infty}\frac{1}{n!}\left((\mathbf{R}\pmb{\phi})^{\wedge}\right)^n\right]\mathbf{t} +\left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\mathbf{R}\pmb{\phi})^{\wedge}\right)^n\right]\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}}\cdot\pmb{\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)^n\right]\bar{\pmb{\rho}} \notag \\
&= -\left[\sum_{n=0}^{\infty}\frac{1}{n!}\left((\mathbf{R}\pmb{\phi})^{\wedge}\right)^n\right]\mathbf{t} +\left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\mathbf{R}\pmb{\phi})^{\wedge}\right)^n\right]\mathbf{R}\pmb{\rho}+\mathbf{t} \notag \\
&= -\left[\sum_{n=0}^{\infty}\frac{1}{n!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\mathbf{t} +\left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\mathbf{R}\pmb{\rho}+\mathbf{t} \notag \\
&= \left(\mathbf{I} - \left[\sum_{n=0}^{\infty}\frac{1}{n!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\right)\mathbf{t} + \left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\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)^n\right]\right)\mathbf{t} + \left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\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)^n\right]\mathbf{R}\pmb{\rho} \notag \\
&= -\left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right](\bar{\pmb{\phi}})^{\wedge}\mathbf{t} + \left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\mathbf{R}\pmb{\rho} \notag \\
&= \left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\mathbf{t}^{\wedge}\bar{\pmb{\phi}} + \left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right]\mathbf{R}\pmb{\rho} \notag \\
&= \left[\sum_{n=0}^{\infty}\frac{1}{(n+1)!}\left((\bar{\pmb{\phi}})^{\wedge}\right)^n\right](\mathbf{t}^{\wedge}\mathbf{R}\pmb{\phi} + \mathbf{R}\pmb{\rho})\end{align}\]

所以

\[\begin{align} \text{Ad}_{\mathbf{T}}\cdot\pmb{\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}\]