\[SE(3)=T=\begin{bmatrix}R&p\\0&1\end{bmatrix}\]
SE(3)是一个４X4的矩阵．它具有的性质如下：- 两个SE(3)的乘积仍是一个SE(3)
- SE(3)的逆： \[T^{-1}=\begin{bmatrix}R^T&-R^Tp\\0&1\end{bmatrix}\]
\(T=(R,p)\in R^3, x,y\in R^3\),则有：
- \(||Tx-Ty||=||x-y||,||．||\)指标准欧式距离，如\(||x||=\sqrt{x^Tx}\)
- \(<Tx-Ty,Ty-Tz>=<x-y,y-z>\),\(<.>\)指内积，如\(<x,y>=x^Ty\) 上述性质中，T是某一点的一个transformation，如T将一点x转换至Tx．３中第一个性质保持距离不变，第二个性质保持角度不变．
在上图中，有\[R_{ab}R_{bc}=R_{ac}\] \[\vec u+\vec v=\vec w \qquad u_a=\vec u \qquad v_b=\vec v\] 现在直接将\(\vec u\)与\(\vec v\)想加是没有意义的，因为他们是基于不同的坐标系的．所以：\[v_a=R_{ab}\to u_a+R_{ab}v_b=W_a\]\[\begin{bmatrix}R_{ab}&u_a\\0&1\end{bmatrix}\begin{bmatrix}R_{bc}&v_b\\0&1\end{bmatrix}=\begin{bmatrix}R_{ab}R_{bc}&u_a+R_{ab}v_b\\0&1\end{bmatrix}\] 因此：\[T_{ab}T_{bc}=T_{ac}\]\(T_{aa}=I\qquad T_{ab}^{-1}=T_{ab}\)
Screw Motion
这是在Exponential Coordinate下同时表示旋转和平移的一种表示．
给定\(w,v\in R^3\):\[S=\begin{bmatrix}w\\v\end{bmatrix}\in R^6\] S被称为twist，定义\([S]\in R^{4X4}\)是如下的矩阵：\[[S]=\begin{bmatrix}[w]&v\\0&0\end{bmatrix} , [w]=\begin{bmatrix}0&-w_3&w_2\\w_3&0&-w_1\\-w_2&w_1&0\end{bmatrix}\]根据以上的定义，我们需要推到出\(e^{[S]\theta}\).
现在给定一个旋转和平移的transform，如何求得对应在Exponential Coordinate下的\(e^{[S]\theta}\)?以下给出算法：
已知\((R,p)\in SE(3)\),求\(e^{[S]\theta}\)：- 如果\(R=I\),那么\(w=0, v=p/||p||, \theta=||p||\)
- 如果\(tr R=-1\),那么\(\theta=\pi , [w]=logR\)
- 否则：\[\theta=cos^{-1}(\frac{trR-1}{2})\in [0,\pi)\]\[[w]=\frac{1}{2sin\theta}(R-R^T)\] \[v=G^{-1}(\theta)p\] 其中，\(G^{-1}(\theta)=\frac{1}{\theta}I+\frac{1}{2}[w]+(\frac{1}{\theta}-\frac{1}{2}cot\frac{\theta}{2})[w]^2\)
给定\(S=(w,v)\in R^6,S'=(w',v')\in R^6,T=(R,p)\in SE(3)\),Adjoint map \(S'=Ad_T(S)\)定义：
\[\begin{align*}S&=\begin{bmatrix} w' \\ v' \end{bmatrix} \\ &= \begin{bmatrix} R&0 \\ [p]R&R \end{bmatrix}\end{align*}\]
\([Ad_T]\)是个６X6矩阵． 使用以上的定义，我们可以有：\[S'=[Ad_T]S\] 还可以这样表示：\[[S']=T[S]T^{-1}\]\[\begin{bmatrix}[w']&v'\\0&0\end{bmatrix}=\begin{bmatrix}R[w]R^T&[p]Rw+Rv\\0&0\end{bmatrix}??\]性质：
- \(Ad_{T_1}(Ad_{T_2}(S))=Ad_{T_1T_2}(S)\)
- \(Ad_T^{-1}=Ad_{T^{-1}}\)
- \(Ad_{T^{-1}}(Ad_T(S))=Ad_{T^{-1}T}(S)=Ad_I(S)=S\)
假定\(S_a=(w_a,v_a)\)是在a坐标系下的screw motion,\(S_b=(w_b,v_b)\)是在b坐标系下的screw motion,则：
\[s_b=Ad_{T_{ba}}(S_a)\]\[s_a=Ad_{T_{ab}}(S_b)\]Spatial Velocities
\(\dot RR^{-1}\)或者\(R^{-1}\dot R会得到角速度向量． 类似的我们可以得到：\)
\[\begin{align*}T^{-1}\dot T&=\begin{bmatrix}R^T&-R^Tp\\0&1\end{bmatrix}\begin{bmatrix}\dot R&\dot p\\0&0\end{bmatrix}\\ &=\begin{bmatrix}R^T\dot R&R^T\dot p\\0&0\end{bmatrix}\\ &=\begin{bmatrix}[w_b]&v_b\\0&0\end{bmatrix}\end{align*}\]
S表示固定参参考系，B表示移动参考系（body coordinate），\(T_{sb}\in SE(3)\):
\[T_{sb}(t)=\begin{bmatrix}R(t) & p(t) \\ 0 & 1\end{bmatrix}\] 所以有：\[T_{sb}^{-1}\dot T_{sb}=[V_b]=\begin{bmatrix}[w_b&v_b\\0&0]\end{bmatrix}\] 以上称之为移动参考系中的Spatial Velocity.\[\dot T_{sb}T_{sb}^{-1}=[V_s]=\begin{bmatrix}[w_s]&v_s\\0&0\end{bmatrix}\] 以上称之为固定参考系中的Spatial Velocity.\(V_s\)和\(V_b\)关系如下：\[V_s=\begin{bmatrix} w_s \\ v_s \end{bmatrix}=\begin{bmatrix} R&0 \\ [p]R&R \end{bmatrix}\begin{bmatrix}w_b\\v_b\end{bmatrix} = Ad_{T_{sb}}(V_b)\]\[V_b=\begin{bmatrix} w_b \\ v_b \end{bmatrix}=\begin{bmatrix} R^T&0 \\ -R^T[p]&R^T \end{bmatrix}\begin{bmatrix} w_s \\ v_s \end{bmatrix} = Ad_{T_{bs}}(V_s)\]Spacial Force
假设一个力f作用在刚体的一点p上．给定一个a参考系，\(f_a \in R^3\)是在a坐标系下的f的表示，产生力矩\(m_a\):
\[m_a=r_a\times f_a\] 其中\(r_a\in R^3\)表示坐标系a中原点到p的向量，将力\(f_a\)和力矩\(m_a\)形成\(F_a=(m_a,f_a)\),这个称之为在a坐标系下的Spatial Force. 现在确定两个坐标系a与b中Spatial Force： \(F_a={m_a,f_a}\)与\(F_b={m_b,f_b}\)的关系:\(T_{ab}=\begin{bmatrix}R_{ab} & p_{ab} \\ 0 & 1\end{bmatrix}\)
在上图中，显然\[f_b=R_{ba}f_a\]
\(r_b=R_{ba}(r_a-p_{ab})\),这是因为\(r_a-p_{ab}\)是在a坐标系中表示的，转换到b坐标系下需要诚意R_{ba},注意与之前的在同一个坐标系中表示旋转的向量点的不同:\[r_b=R_{ab}^T(r_a-p_{ab})\]力矩\(m_b=r_b\times f_b\)就可以写成：
\[\begin{align*}m_b &= R_{ab}^T(r_a-p_{ab})\times R_{ab}^Tf_a\\ &=[R_{ab}^Tr_a]R_{ab}^Tf_a-[R_{ab}^Tp_{ab}]R_{ab}^Tf_a\\ &=R_{ab}^T[r_a]f_a-R_{ab}^T[p_{ab}]f_a\\ &=R_{ab}^Tm_a+R_{ab}^T[p_{ab}]^Tf_a\end{align*}\] 因为\([p_{ab}]^T=-[p_{ab}]\),所以：\[\begin{bmatrix} m_b \\ f_b \end{bmatrix}=\begin{bmatrix} R_{ab} & 0 \\ [p_{ab}]R_{ab} & R_{ab}\end{bmatrix}^T\begin{bmatrix} m_a \\ f_a \end{bmatrix}\] 或者可以表示如下：\[F_b=Ad_{T_{ab}}^T(F_a)=[Ad_{T_{ab}}]^TF_a\]