SE2雅可比推导

最新推荐文章于 2022-12-07 12:05:04 发布

原创最新推荐文章于 2022-12-07 12:05:04 发布 · 1.5k 阅读

12 ·

CC 4.0 BY-SA版权

毕业论文专栏收录该内容

4 篇文章

订阅专栏

本文介绍了一种扰动法来解决二维变换矩阵的导数问题，包括平移和旋转。通过引入坐标约定和基本的2D矩阵变换，详细推导了其导数，并给出了在SLAM问题中滑动窗口优化的例子。

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 $θ)(x\ y\ \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 $1)(u\ v\ 1)$ 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.
$u_{c} = u - c_{x} \\ v_{c} = v - c_{y} \tag{1}$

2D Transformation Matrix

The transformation matrix $T∈SE(2)\ T \in SE(2)$ of the pose $θ)∈se(2)\xi\ (x\ y\ \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.
$\left[ \begin{matrix} cos(\theta) & -sin(\theta) & x \\ sin(\theta) & cos(\theta) & y \\ 0 & 0 & 0 \end{matrix} \right] = \left[ \begin{matrix} Rot(\theta) & t \\ 0 & 1 \\ \end{matrix} \right] \tag{2}$
We the right presentation of equation (1) since its more compact.The inverse of T is shown in (3).
$T^{-1} = \left[ \begin{matrix} Rot(-\theta) & -Rot(-\theta) t \\ 0 & 1 \\ \end{matrix} \right] \tag{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{matrix} Rot(\theta)Rot(\delta \theta) & t +\delta t \\ 0 & 1 \\ \end{matrix} \right] = \left[ \begin{matrix} Rot(\theta) & t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} Rot(\delta \theta) & Rot(-\theta)\delta t \\ 0 & 1 \\ \end{matrix} \right] \tag{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{matrix} Rot( \delta \theta) & Rot(-\theta)\delta t \\ 0 & 1 \end{matrix} \right] \tag{5}$
So the derivative of 2d transformation T can be written as (6).
$\frac{\partial T}{\partial \delta\xi} = \lim_{\partial \delta \xi \to 0} \frac{Tper(\delta\xi)P-TP}{\delta \xi} \tag{6}$
When $δθ\delta\theta$ is very small , $Rot(δθ)Rot(\delta\theta)$ can be approximated to (7).
$Rot(\delta\theta)=\left[ \begin{matrix} cos(\delta\theta) & -sin(\delta\theta) \\ sin(\delta\theta) & cos(\delta\theta) \end{matrix} \right] \\= \left[ \begin{matrix} 1 & -\delta\theta) \\ \delta\theta & 1 \end{matrix} \right] \\ = I + Rot(\frac{\pi}{2})\delta\theta \tag{7}$
Consequently (6) can expand as follow:
$\frac{\partial T}{\partial \delta\xi} =\lim_{\partial \delta \xi \to 0} \frac{Tper(\delta\xi)P-TP} {\delta \xi} \\ =\lim_{\partial \delta\xi \to 0} \frac{ \left[ \begin{matrix} Rot(\theta) & t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} I + Rot(\frac{\pi}{2})\delta\theta & Rot(-\theta)\delta t \\ 0& 1\\\end{matrix} \right] \left[ \begin{matrix} p_{c} \\1 \end{matrix} \right]- \left[\begin{matrix} Rot(\theta) & t \\ 0 & 1 \end{matrix}\right] \left[ \begin{matrix}p_{c} \\ 1 \\ \end{matrix}\right] }{\delta\xi} \\= \frac{ \left[\begin{matrix}Rot(\frac{\pi}{2})\delta\theta P_{c} & \delta t \\\end{matrix}\right] }{ \delta\xi } \\= \left[ \begin{matrix} I_{2\times 2} & Rot(\frac{\pi}{2}) P_{c} \end{matrix}\right]\tag{8}$
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 $n]\xi_{si}\ i\in[1\ n]$ corresponding to sliding window, and current pose $ξc\xi_{c}$ . Optimization process will adjust current pose making the cost function decrease.The cost function is shown in(9).
$\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}$
Note $T_{c}^{w}$ denote current pose which is going to be optimize, can also represent as $exp(ξc)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{matrix} Rot(\theta_{c}-\theta_{si}) & Rot(-\theta_{si})(t_{c}-t_{si}) \\ 0 & 1 \end{matrix}\right] \tag{9}$
Now we add perturbation to $T_{c}^{w}$ and use approximation of first order Taylor expand as shown in (10):
$e(\xi^{si}_{c} \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}* exp(\delta\xi_{c})\tag{10} \\ J_{c} = \frac{\partial I_{s} }{\partial u}* \frac{\partial u}{\partial \delta\xi_{c}} \\ u = T_{c}^{si}*per(\delta\xi_{c})P_{j}^{c}$
$∂Is∂u\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 $∂u∂δξ\frac{\partial u}{\partial\delta\xi}$ ,as shown in (11)
$\frac{\partial u}{\partial\delta\xi_{c}} = \lim_{\delta\xi\to0} \frac{T_{c}^{si}*per(\delta\xi_{c})P_{j}^{c} - T_{c}^{si}P_{j}^{c}} {\delta\xi_{c}}\\=\\ \lim_{\delta\xi_{c}\to0}\frac{ \left[\begin{matrix} Rot(\theta_{c}-\theta_{si}) & Rot(-\theta_{si})(t_{c}-t_{si}) \\ 0 & 1 \end{matrix}\right] \left[\begin{matrix} Rot(\delta\theta{c}) & Rot(-\theta_{c})\delta t \\ 0 &1 \end{matrix}\right]P_{j}^{c}- T_{c}^{si}P_{j}^{c}}{\delta\xi_{c}}\\= \lim_{\delta\xi_{c}\to0}\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{matrix} Rot(-\theta_{si}) & Rot(\theta_{c}-\theta_{si}+\frac{\pi)}{2}P_{c}\end{matrix}\right]\tag{11}$
Then we derive the final form :
$J_{c} = [I_{x}^{s}\ I_{y}^{s}]\left[\begin{matrix} Rot(-\theta_{si}) & Rot(\theta_{c}-\theta_{si}+\frac{\pi)}{2}P_{c}\end{matrix}\right] \tag{12}$
Using the similar method we can also derive of jacobian $J_{si}$ corresponding to pose $ξsi\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{matrix} Rot(-\theta_{si}) & Rot(\frac{\pi}{2}-\theta_{si} )(-Rot( \theta_{c})P_{c} -t_{c}+t_{si})\end{matrix}\right] \tag{1}$