SE2雅可比推导

Solving SE2 Derivative by Perturbation Methodology

Wenqiang Li December 13,2018

Introduction

In this paper , we provide a perturbation methodology to solve the derivative of 2D transform, including transition and rotation. We denote $(xy\theta )$ as our vehicle 2D-pose, as most of our project used.

Formation

In this section, We introduce the coordinate convention about the image point and the basic 2D matrix transformation. Then we derive its derivative by perturbation method. And finally we give a example of sliding window optimization as used in most of SLAM proble.

Corrdinate System

We denote the homogeneous coordinate $(uv1)$ as the point in the image . For convenient, we shift it by the image center $(c_x{c}_y)$ since its more convenient for 2D image transformation,as shown in (1). We use $P(u_c{v}_{c}1)$ or $P(p_{c}1)$ as 2D Point coordinates in the following derivatives.
$$\begin{gathered} u_c=u-c_x \\ {v}_c=v-c_y\text{\hspace{1em}(1)} \end{gathered}$$

2D Transformation Matrix

The transformation matrix $T\in SE(2)$ of the pose $\xi (xy\theta )\in se(2)$ is shown as (2),we use it to transform a image point $p_c(u_c{v}_{c}1)$ from image coordinate to the wold coordinate.
$$T=\left[\begin{array}{ccc}cos(\theta )& -sin(\theta )& x\\ sin(\theta )& cos(\theta )& y\\ 0& 0& 0\end{array}\right]=\left[\begin{array}{cc}Rot(\theta )& t\\ 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$
We the right presentation of equation (1) since its more compact.The inverse of T is shown in (3).
$$T^{-1}=\left[\begin{array}{cc}Rot(-\theta )& -Rot(-\theta )t\\ 0& 1\end{array}\right]\text{\hspace{1em}(3)}$$

Derivative of 2D Transformation

In order to solve the derivative of the 2D transformation matrix, we firstly introduce the methodology of perturbation. Let’s first think of the question: To add a small update $\delta \xi (\delta t\delta \theta )$ to the known pose $\xi$ ,what manipulation shall we do to the transformation matrix ? The answer is show in (4).
$$\left[\begin{array}{cc}Rot(\theta )Rot(\delta \theta )& t+\delta t\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}Rot(\theta )& t\\ 0& 1\end{array}\right]\left[\begin{array}{cc}Rot(\delta \theta )& Rot(-\theta )\delta t\\ 0& 1\end{array}\right]\text{\hspace{1em}(4)}$$
For simplicity ,we use per$(\xi )$ denote mapping from a perturbation vector in se(2) to the right multiple matrix in SE(2) as shown in (5)
$$per(\delta \xi )=\left[\begin{array}{cc}Rot(\delta \theta )& Rot(-\theta )\delta t\\ 0& 1\end{array}\right]\text{\hspace{1em}(5)}$$
So the derivative of 2d transformation T can be written as (6).
$$\frac{\partial T}{\partial \delta \xi }=\underset{\partial \delta \xi \to 0}{\lim }\frac{Tper(\delta \xi )P-TP}{\delta \xi }\text{\hspace{1em}(6)}$$
When $\delta \theta$ is very small ,$Rot(\delta \theta )$ can be approximated to (7).
$$\begin{gathered} Rot(\delta \theta )=\left[\begin{array}{cc}cos(\delta \theta )& -sin(\delta \theta )\\ sin(\delta \theta )& cos(\delta \theta )\end{array}\right] \\ =\left[\begin{array}{cc}1& -\delta \theta )\\ \delta \theta & 1\end{array}\right] \\ =I+Rot(\frac{\pi }{2})\delta \theta \text{\hspace{1em}(7)} \end{gathered}$$
Consequently (6) can expand as follow:
$$\begin{gathered} \frac{\partial T}{\partial \delta \xi }=\underset{\partial \delta \xi \to 0}{\lim }\frac{Tper(\delta \xi )P-TP}{\delta \xi } \\ =\underset{\partial \delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}Rot(\theta )& t\\ 0& 1\end{array}\right]\left[\begin{array}{cc}I+Rot(\frac{\pi }{2})\delta \theta & Rot(-\theta )\delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_c\\ 1\end{array}\right]-\left[\begin{array}{cc}Rot(\theta )& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_c\\ 1\end{array}\right]}{\delta \xi } \\ =\frac{\left[\begin{array}{cc}Rot(\frac{\pi }{2})\delta \theta P_c& \delta t\end{array}\right]}{\delta \xi } \\ =\left[\begin{array}{cc}{I}_{2\times 2}& Rot(\frac{\pi }{2})P_c\end{array}\right]\text{\hspace{1em}(8)} \end{gathered}$$
Note the order in $\delta \xi$ is $[\delta t\delta \theta ]$ corresponding to the transition and rotation perturbation.

Example on Sliding Window Optimization

Assume we have a serial pose ${\xi }_{si}i\in [1n]$ corresponding to sliding window, and current pose ${\xi }_c$. Optimization process will adjust current pose making the cost function decrease.The cost function is shown in(9).
$$\begin{gathered} \frac{1}{2}\sum _{i=1}^n\sum _j||e_i|{|}^2=\frac{1}{2}\sum _{i=1}^n\sum _j||I_s((T_{si}^w{)}^{-1}{T}_c^w{P}_j^c)-I_c(P_j^c)|{|}^2 \\ =\frac{1}{2}\sum _{i=1}^n\sum _j||I_s((T_c^{si})P_j^c)-I_c(P_j^c)|{|}^2 \\ =\frac{1}{2}\sum _{i=1}^n\sum _j||e({\xi }_c^{si})|{|}^2 \end{gathered}$$
Note $T_c^w$ denote current pose which is going to be optimize, can also represent as $exp({\xi }_c)$. And $T_{si}^w$ denote the $i_{th}$ pose of sliding window. $P_j^c$ denote the point in the current image. $T_c^{si}$ is a multiple of $T_{si}^{w-1}$ and $T_c^w$ as shown in (9).
$$T_{si}^{w-1}{T}_c^w=\left[\begin{array}{cc}Rot({\theta }_c-{\theta }_{si})& Rot(-{\theta }_{si})(t_c-t_{si})\\ 0& 1\end{array}\right]\text{\hspace{1em}(9)}$$
Now we add perturbation to $T_c^w$ and use approximation of first order Taylor expand as shown in (10):
$$\begin{gathered} e({\xi }_c^{si}\oplus \delta {\xi }_c)=I_s(T_c^{si}per(\delta {\xi }_c)P_j^c)-I_c(P_j^c) \\ \approx I_s(T_c^{si}{P}_j^c)-I_c(P_j^c)+J_c\ast exp(\delta {\xi }_c) \\ {J}_c=\frac{\partial I_s}{\partial u}\ast \frac{\partial u}{\partial \delta {\xi }_c} \\ u=T_c^{si}\ast per(\delta {\xi }_c)P_j^c\text{\hspace{1em}(10)} \end{gathered}$$
$\frac{\partial I_s}{\partial u}$ is the gradient of the image, which can represent as $[I_x^s{I}_y^s]$.
Here we combine (5), (7) and (9) to derive the solution of $\frac{\partial u}{\partial \delta \xi }$ ,as shown in (11)
$$\begin{gathered} \frac{\partial u}{\partial \delta {\xi }_c}=\underset{\delta \xi \to 0}{\lim }\frac{T_c^{si}\ast per(\delta {\xi }_c)P_j^c-T_c^{si}{P}_j^c}{\delta {\xi }_c} \\ = \\ \underset{\delta {\xi }_c\to 0}{\lim }\frac{\left[\begin{array}{cc}Rot({\theta }_c-{\theta }_{si})& Rot(-{\theta }_{si})(t_c-t_{si})\\ 0& 1\end{array}\right]\left[\begin{array}{cc}Rot(\delta \theta c)& Rot(-{\theta }_c)\delta t\\ 0& 1\end{array}\right]P_j^c-T_c^{si}{P}_j^c}{\delta {\xi }_c} \\ =\underset{\delta {\xi }_c\to 0}{\lim }\frac{Rot(-{\theta }_{si})\delta t_c+Rot({\theta }_c-{\theta }_{si}+\frac{\pi )}{2}{P}_c\delta {\theta }_c}{\delta {\xi }_c} \\ =\left[\begin{array}{cc}Rot(-{\theta }_{si})& Rot({\theta }_c-{\theta }_{si}+\frac{\pi )}{2}{P}_c\end{array}\right]\text{\hspace{1em}(11)} \end{gathered}$$
Then we derive the final form :
$$J_c=[I_x^s{I}_y^s]\left[\begin{array}{cc}Rot(-{\theta }_{si})& Rot({\theta }_c-{\theta }_{si}+\frac{\pi )}{2}{P}_c\end{array}\right]\text{\hspace{1em}(12)}$$
Using the similar method we can also derive of jacobian $J_{si}$corresponding to pose ${\xi }_{si}$ ,here we omit the derivation process since its almost the same as mentioned, and directly give the result shown in (13)
$$J_{si}=[I_x^s{I}_y^s]\left[\begin{array}{cc}Rot(-{\theta }_{si})& Rot(\frac{\pi }{2}-{\theta }_{si})(-Rot({\theta }_c)P_c-t_c+t_{si})\end{array}\right]\text{\hspace{1em}(1)}$$