[SLAM][En] Errors and Jacobian Derivations for SLAM Part 2

7. IMU measurement error

In order to obtain the IMU measurement error, you must first know about the IMU preintegration technique and error-state modeling.  The following figure expresses the overall IMU measurement error-based optimization process.

 

The overall IMU measurement error-based optimization process, with detailed explanations and color highlighting added for better understanding, is as follows. 

TBA...

 

 

 

8. Other jacobians

8.1. Jacobian of unit quaternion

NOMENCLATURE of jacobian of unit quaternion

• $\mathbf{X} =  [X,Y,Z,1]^{\intercal} = [\tilde{\mathbf{X}}, 1]^{\intercal} \in \mathbb{P}^{3}$
• $\tilde{\mathbf{X}} = [X,Y,Z]^{\intercal} \in \mathbb{P}^{2}$
• $\mathbf{q} = [w,x,y,z]^{\intercal} = [w, \mathbf{v}]^{\intercal}$
  
  • A quaternion expressed in hamilton notation.  For more information on this, please refer to this post.

 

The Jacobians described earlier in the reprojection error section are:

\begin{equation}
  \begin{aligned}
    \mathbf{J}_{c}& = \frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}} 
    \frac{\partial \tilde{\mathbf{p}}}{\partial \mathbf{X}'}
    \frac{\partial \mathbf{X}'}{\partial [\mathbf{R}, \mathbf{t}]} \\
      \end{aligned} 
\end{equation}

 

Among them, $\frac{\partial \mathbf{X}'}{\partial \mathbf{R}}$ is the Jacobian that can be used when the rotation is expressed as the rotation matrix $\mathbf{R}$.  This section describes the Jacobian $\frac{\partial \mathbf{X}'}{\partial \mathbf{q}}$ that can be used when rotation is expressed as a unit quaternion $\mathbf{q}$ .

 

Given a point $\mathbf{X}$ in 3D space, the point $\mathbf{X}'$ rotated through an arbitrary unit quaternion $\mathbf{q}$ can be expressed as follows.

\begin{equation}
\begin{aligned}
\tilde{\mathbf{X}'} & = \mathbf{q} \otimes \tilde{\mathbf{X}} \otimes \mathbf{q}^{*} \end{aligned}
\end{equation}

 

Unpacking and expanding it, we get:

\begin{equation}
\begin{aligned}
\tilde{\mathbf{X}'} & = \mathbf{q} \otimes \tilde{\mathbf{X}} \otimes \mathbf{q}^{*} \\
& = (w+\mathbf{v}) \otimes \tilde{\mathbf{X}} \otimes (w-\mathbf{v}) \\
& = w^{2}\tilde{\mathbf{X}} + w(\mathbf{v}\otimes \tilde{\mathbf{X}} - \tilde{\mathbf{X}}\otimes \mathbf{v}) - \mathbf{v}\otimes \tilde{\mathbf{X}} \otimes \mathbf{v} \\
& = w^{2}\tilde{\mathbf{X}}+2w(\mathbf{v}\times \tilde{\mathbf{X}})- [(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}} + \mathbf{v}\times \tilde{\mathbf{X}}) \otimes \mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v}\times \tilde{\mathbf{X}}) -[(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} + (\mathbf{v}\times \tilde{\mathbf{X}})\otimes \mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v} \times \tilde{\mathbf{X}}) - [(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} - \cancel{ (\mathbf{v}\times \tilde{\mathbf{X}})^{\intercal}\mathbf{v} } +(\mathbf{v} \times \tilde{\mathbf{X}})\times \mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v} \times \tilde{\mathbf{X}}) - [(-\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} + (\mathbf{v}^{\intercal}\mathbf{v})\tilde{\mathbf{X}} - (\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v}] \\
& = w^{2}\tilde{\mathbf{X}} + 2w(\mathbf{v}\times \tilde{\mathbf{X}}) + 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}})\mathbf{v} - (\mathbf{v}^{\intercal}\mathbf{v})\tilde{\mathbf{X}}
\end{aligned}
\end{equation}

 

You can use this to find the Jacobian $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{q}}$ for a quaternion. Dividing into the scalar part $\frac{\partial \tilde{\mathbf{X}'}}{\partial w}$ and the vector part $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}}$, it is obtained as follows.

\begin{equation}
\begin{aligned}
& \frac{\partial \tilde{\mathbf{X}'}}{\partial w} && =2(w \tilde{\mathbf{X}} + \mathbf{v} \times \tilde{\mathbf{X}}) \\
& \frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}} && = -2w[\tilde{\mathbf{X}}]_{\times} + 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}}\mathbf{I} + \mathbf{v}\tilde{\mathbf{X}}^{\intercal}) - 2 \tilde{\mathbf{X}}\mathbf{v}^{\intercal} \\
& && = 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}}\mathbf{I} + \mathbf{v}\tilde{\mathbf{X}}^{\intercal} - \tilde{\mathbf{X}}\mathbf{v}^{\intercal} - w[\tilde{\mathbf{X}}]_{\times})
\end{aligned}
\end{equation}

 

At this time, $\tilde{\mathbf{X}}$, which enters the center of quaternion multiplication, is actually transformed into a pure quaternion $[0, X, Y, Z]^{\intercal}$ form with a scalar value of 0 without actually entering a 3-dimensional vector. Therefore, in the above expression, since the Jacobian $\frac{\partial \tilde{\mathbf{X}'}}{\partial w}$ for scalar is not used during actual optimization, only the Jacobian  $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}}$ for the vector is obtained without obtaining it separately.

\begin{equation}
\begin{aligned}
\tilde{\mathbf{X}'}  = \mathbf{q} \otimes \tilde{\mathbf{X}} \otimes \mathbf{q}^{*} \quad  \rightarrow \quad \begin{bmatrix} 0 \\ \tilde{\mathbf{X}'} \end{bmatrix} & = \mathbf{q} \otimes \begin{bmatrix} 0 \\  \tilde{\mathbf{X}} \end{bmatrix} \otimes \mathbf{q}^{*} \quad \cdots \text{ strict notation} \\
\text{Then, } \frac{\partial \tilde{\mathbf{X}'}}{\partial w} &\text{ is going to be useless}
\end{aligned}
\end{equation}

 

In addition, the quaternion $\mathbf{q}$ is similar to the method used to approximate a sufficiently small rotation matrix with $\mathbf{R} \approx \mathbf{I} + [\mathbf{w}]_{\times}$. Assuming it is small enough, it can be approximated as an identity ($\mathbf{q} \approx \mathbf{q}_{1} = [1, 0, 0, 0]^{\intercal}$).

\begin{equation}
\begin{aligned}
& \left. \frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{v}}\right\vert_{\mathbf{q} \approx \mathbf{q}_{1}}
&& = 2(\mathbf{v}^{\intercal}\tilde{\mathbf{X}}\mathbf{I} + \mathbf{v}\tilde{\mathbf{X}}^{\intercal} - \tilde{\mathbf{X}}\mathbf{v}^{\intercal} - w[\tilde{\mathbf{X}}]_{\times}) \\ & && = -2[\tilde{\mathbf{X}}]_{\times}
\end{aligned}
\end{equation}

 

Finally,  the Jacobian $\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{q}}$ for the unit quaternion is:

\begin{equation}
\boxed {\begin{aligned}
\frac{\partial \tilde{\mathbf{X}'}}{\partial \mathbf{q}} =  -2[\tilde{\mathbf{X}}]_{\times} = -2 \begin{bmatrix} 0 & -Z & Y \\ Z & 0 & -X \\ -Y & X & 0 \end{bmatrix} \in \mathbb{R}^{3\times3}
\end{aligned} }
\end{equation}

 

8.1.1. Code Implementations

• ProSLAM code: trajectory_analyzer.cpp#L253
  • refered to jinyongjeong blog post

 

8.2. Jacobian of camera intrinsics

NOMENCLATURE of jacobian of camera intrinsics

• $\pi^{-1}(\cdot) = Z\mathbf{K}^{-1}(\cdot)$:  Back-projection function that back-projects a point on an image onto a 3D space
• $\pi(\cdot) = \pi_{k}(\pi_{h}(\cdot)) = \mathbf{K}(\frac{1}{Z} \cdot)$: Projection function that projects a point in 3D space onto an image plane.
• $\mathbf{K} = \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{bmatrix}$: Camera intrinsic matrix
• $\mathbf{K}^{-1} = \begin{bmatrix} f_x^{-1} & 0 & -f_x^{-1}c_{x} \\ 0 & f_{y}^{-1} & -f_{y}^{-1}c_{y} \\ 0&0&1  \end{bmatrix}$
• $\tilde{\mathbf{K}} = \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y}  \end{bmatrix}$:  I omitted the last line of internal parameters to project to $\mathbb{P}^{2} \rightarrow \mathbb{R}^{2}$
• $\mathbf{X} = [\tilde{\mathbf{X}}, 1]^{\intercal}$

 

When camera calibration is performed to perform SLAM, the intrinsic matrix $\mathbf{c} = [f_x, f_y, c_x, c_y]$ and the lens distortion parameter $\mathbf{d} = [k_1, k_2, p_1 , p_2]$ can be obtained.  However, since the calibration value does not exactly match the parameters of the actual sensor, it can be fine-tuned through optimization.  This section describes the process of deriving the Jacobian $\mathbf{J}_{\mathbf{c}}$ for $\mathbf{c}$.  At this time, it is assumed that the focal length is $f_{x} \neq f_{y}$.

 

For example, let's say we want to find $\mathbf{J}_{\mathbf{c}}$ for photometric error.  This can be expressed as:

\begin{equation}
 \begin{aligned}
    \mathbf{J}_{\mathbf{c}} & = \frac{\partial \mathbf{e}}{\partial \mathbf{c}}  \\ & =  \frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}} \frac{\partial \mathbf{p}_{2}}{\partial \tilde{\mathbf{p}}_{2}} \frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'} \frac{\partial \mathbf{X}'}{\partial \mathbf{c}}  \\
& = \mathbb{R}^{1\times2} \cdot \mathbb{R}^{2\times3} \cdot \mathbb{R}^{3\times4} \cdot \mathbb{R}^{4\times4} = \mathbb{R}^{1\times4} 
  \end{aligned}
\end{equation}

 

At this time, the first $\frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}}$ term is the Jacobian required to obtain the photometric error, and the remaining three Jacobians are reprojection. , is a term that always needs to be found, regardless of the photometric error term. Therefore, if $\frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial \mathbf{c}}$ is obtained, it can be applied to all error terms generally used in SLAM.

 

The relationship between the points $\mathbf{p}_{1}$ and $\mathbf{p}_{2}$ on the image plane of the two cameras $\{C_{1}\}, \{C_{2}\}$ is It can be written as:

\begin{equation}
 \begin{aligned}
    \mathbf{p}_{1}  & =\begin{bmatrix} u_{1} & v_{1} \end{bmatrix}^{\intercal} \\     \mathbf{p}_{2}  & =\begin{bmatrix} u_{2} & v_{2} \end{bmatrix}^{\intercal}
  \end{aligned}
\end{equation}

 

\begin{equation}
 \begin{aligned}
    \mathbf{p}_{2}  & = \pi(\mathbf{X}') \\
& = \pi(\mathbf{RX} + \mathbf{t}) \\
& = \pi(\mathbf{R}\pi^{-1}(\mathbf{p}_{1}) + \mathbf{t})  \quad \cdots \text{ apply back-proj}\\
& = \pi(\mathbf{R}(Z\mathbf{K}^{-1}\mathbf{p}_{1}) + \mathbf{t}) \\
& = \pi_{k}(\pi_{h}(\mathbf{R}(Z\mathbf{K}^{-1}\mathbf{p}_{1}) + \mathbf{t})) \\
& =  \pi_{k}(\frac{Z}{Z'}\mathbf{R}\mathbf{K}^{-1}\mathbf{p}_{1} + \frac{1}{Z'}\mathbf{t}) \quad \cdots \text{ apply } \pi_{h}(\cdot) \\
&= \frac{Z}{Z'} \tilde{\mathbf{K}}\mathbf{R}\mathbf{K}^{-1}\mathbf{p}_{1} + \frac{1}{Z'}\tilde{\mathbf{K}}\mathbf{t} \quad \cdots \text{ apply } \pi_{k}(\cdot)
  \end{aligned}
\end{equation}

 

Since back projection $\rightarrow$ transformation $\rightarrow$ projection process occurs in a chain in $\mathbf{p}_{1}$ and becomes $\mathbf{p}_{2}$, the above complicated formula is obtained.  As shown in the above expression, $\frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p} }_{2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial \mathbf{c}}$ is $\mathbf{p}_{2}$ From $\mathbf{c}$ includes parameters.  Therefore, $\frac{\partial \mathbf{p}_{2}}{\partial \mathbf{c}}$ must be computed at once by grouping three Jacobians.

\begin{equation}
\begin{aligned}
\frac{\partial \mathbf{p}_{2}}{\partial \mathbf{c}}
&= \frac{\partial} {\partial \mathbf{c}} \begin{bmatrix} u_{2} \\ v_{2} \end{bmatrix} \\ &= \frac{\partial} {\partial [f_x, f_y, c_x, c_y]} \begin{bmatrix}  f_{x}\tilde{u}_{2} + c_{x} \\ f_{y}\tilde{v}_{2} + c_{y} \end{bmatrix} \\
& = \begin{bmatrix}
\frac{\partial u_{2}}{\partial f_{x}} & \frac{\partial u_{2}}{\partial f_{y}}
& \frac{\partial u_{2}}{\partial c_{x}}
& \frac{\partial u_{2}}{\partial c_{y}}
\\
\frac{\partial v_{2}}{\partial f_{x}} & \frac{\partial v_{2}}{\partial f_{y}}
& \frac{\partial v_{2}}{\partial c_{x}}
& \frac{\partial v_{2}}{\partial c_{y}}
\end{bmatrix} \\
&=
\begin{bmatrix}
\tilde{u}_{2} + f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{x}} 
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{y}}
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{x}} + 1
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{y}}
\\
f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{x}} 
& \tilde{v}_{2} + f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{y}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{x}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{y}}+1
\end{bmatrix} \in \mathbb{R}^{2\times4}
\end{aligned} \label{eq:intrinsic2}
\end{equation}

 

Next, we need to calculate the elements of the above equation.

\begin{equation} \begin{aligned}
\begin{pmatrix} \frac{\partial \tilde{u}_{2}}{\partial f_{x}} & \frac{\partial \tilde{u}_{2}}{\partial f_{y}} & \frac{\partial \tilde{u}_{2}}{\partial c_{x}} & \frac{\partial \tilde{u}_{2}}{\partial c_{y}} \\ \frac{\partial \tilde{v}_{2}}{\partial f_{x}} & \frac{\partial \tilde{v}_{2}}{\partial f_{y}} & \frac{\partial \tilde{v}_{2}}{\partial c_{x}} & \frac{\partial \tilde{v}_{2}}{\partial c_{y}} \end{pmatrix} 
\end{aligned} \label{eq:intrinsic1}  \end{equation}

 

To find this, first find $\tilde{\mathbf{p}}_{2} = [\tilde{u}_{2}, \tilde{v}_{2}, 1]^{\intercal}$ As follows.

\begin{equation}
\begin{aligned}
\tilde{\mathbf{p}}_{2} & = [\tilde{u}_{2}, \tilde{v}_{2}, 1]^{\intercal} \\ & = \frac{1}{Z'}\tilde{\mathbf{X}}'  \\
&=
 \frac{1}{Z'}(\mathbf{R}\tilde{\mathbf{X}} + \mathbf{t}) \\
&=
 \frac{Z}{Z'}\mathbf{R}\mathbf{K}^{-1}\mathbf{p}_{1} + \frac{1}{Z'}\mathbf{t} \\
&=
\frac{Z}{Z'} 
\begin{bmatrix}
& & \\
& \mathbf{R} & \\
& &
\end{bmatrix}
\begin{bmatrix}
f_{x}^{-1}& & -f_{x}^{-1}c_{x}\\
& f_{y}^{-1} & -f_{y}^{-1}c_{y} \\
& & 1
\end{bmatrix}
\begin{bmatrix}
u_{1} \\ v_{1} \\ 1
\end{bmatrix} 
+ \frac{1}{Z'}
\begin{bmatrix}
t_{x} \\ t_{y} \\ t_{z} 
\end{bmatrix}  \\
&=
 \frac{Z}{Z'}
\begin{bmatrix}
& & \\
& \mathbf{R} & \\
& &
\end{bmatrix}
\begin{bmatrix}
f_{x}^{-1}(u_{1}-c_{x}) \\ 
f_{y}^{-1}(v_{1}-c_{y}) \\ 
1
\end{bmatrix} 
+ \frac{1}{Z'}
\begin{bmatrix}
t_{x} \\ t_{y} \\ t_{z}
\end{bmatrix} \\
&=
\frac{Z}{Z'}
\begin{bmatrix}
r_{11}f_{x}^{-1}(u_{1}-c_{x}) + r_{12}f_{y}^{-1}(v_{1}-c_{y}) + r_{13} \\ 
r_{21}f_{x}^{-1}(u_{1}-c_{x}) + r_{22}f_{y}^{-1}(v_{1}-c_{y}) + r_{23} \\ 
r_{31}f_{x}^{-1}(u_{1}-c_{x}) + r_{32}f_{y}^{-1}(v_{1}-c_{y}) + r_{33}
\end{bmatrix} 
+ \frac{1}{Z'}
\begin{bmatrix}
t_{x} \\ t_{y} \\ t_{z}
\end{bmatrix} \\
\end{aligned}
\end{equation}

 

The above expression is rearranged as follows.

\begin{equation} \begin{aligned} \begin{bmatrix} \tilde{u}_{2} \\ \tilde{v} _{2}\\1 \end{bmatrix}  &= \begin{bmatrix} \frac {r_{11}f_{x}^{-1}(u_{1}-c_{x}) + r_{12}f_{y}^{-1}(v_{1}-c_{y}) + r_{13} + \frac{1}{Z}t_{x}} {r_{31}f_{x}^{-1}(u_{1}-c_{x}) + r_{32}f_{y}^{-1}(v_{1}-c_{y}) + r_{33} + \frac{1}{Z}t_{z}}  \\   \frac {r_{21}f_{x}^{-1}(u_{1}-c_{x}) + r_{22}f_{y}^{-1}(v_{1}-c_{y}) + r_{23} + \frac{1}{Z}t_{y}} {r_{31}f_{x}^{-1}(u_{1}-c_{x}) + r_{32}f_{y}^{-1}(v_{1}-c_{y}) + r_{33} + \frac{1}{Z}t_{z}}  \\ 1 \end{bmatrix} \end{aligned} \end{equation}

 

Based on this, (\ref{eq:intrinsic1}) is calculated as follows.

\begin{equation} \begin{aligned}
\frac{\partial \tilde{u}_{2}}{\partial f_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{u}_{2} - r_{11})f_{x}^{-2}(u_{1}-c_{x}) \\
\frac{\partial \tilde{u}_{2}}{\partial f_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{u}_{2} - r_{12})f_{y}^{-2}(v_{1}-c_{y}) \\
\frac{\partial \tilde{u}_{2}}{\partial c_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{u}_{2} - r_{11})f_{x}^{-1} \\
\frac{\partial \tilde{u}_{2}}{\partial c_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{u}_{2} - r_{12})f_{y}^{-1} \\
\frac{\partial \tilde{v}_{2}}{\partial f_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{v}_{2} - r_{21})f_{x}^{-2}(u_{1}-c_{x}) \\
\frac{\partial \tilde{v}_{2}}{\partial f_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{v}_{2} - r_{22})f_{y}^{-2}(v_{1}-c_{y}) \\
\frac{\partial \tilde{v}_{2}}{\partial c_{x}} & = \frac{Z}{Z'}(r_{31}\tilde{v}_{2} - r_{21})f_{x}^{-1} \\
\frac{\partial \tilde{u}_{2}}{\partial c_{y}} & = \frac{Z}{Z'}(r_{32}\tilde{v}_{2} - r_{22})f_{y}^{-1}
\end{aligned}  \end{equation}

 

Finally, (\ref{eq:intrinsic2}) is:

\begin{equation} \boxed{
\begin{aligned}
\frac{\partial \mathbf{p}_{2}}{\partial \mathbf{c}}
&=
\begin{bmatrix}
\frac{\partial u_{2}}{\partial f_{x}} & \frac{\partial u_{2}}{\partial f_{y}}
& \frac{\partial u_{2}}{\partial c_{x}}
& \frac{\partial u_{2}}{\partial c_{y}}
\\
\frac{\partial v_{2}}{\partial f_{x}} & \frac{\partial v_{2}}{\partial f_{y}}
& \frac{\partial v_{2}}{\partial c_{x}}
& \frac{\partial v_{2}}{\partial c_{y}}
\end{bmatrix} \\
&=
\begin{bmatrix}
\tilde{u}_{2} + f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{x}} 
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial f_{y}}
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{x}} + 1
& f_{x}\frac{\partial \tilde{u}_{2}}{\partial c_{y}}
\\
f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{x}} 
& \tilde{v}_{2} + f_{y}\frac{\partial \tilde{v}_{2}}{\partial f_{y}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{x}}
& f_{y}\frac{\partial \tilde{v}_{2}}{\partial c_{y}}+1
\end{bmatrix} \\
& = \begin{bmatrix}
\tilde{u}_{2} + \frac{Z}{Z'}f_{x}^{-1}(r_{31}\tilde{u}_{2} - r_{11})(u_{1}-c_{x})&
\frac{Z}{Z'}f_{x}f_{y}^{-2}(r_{32}\tilde{u}_{2} - r_{12})(v_{1}-c_{y})&
\frac{Z}{Z'}(r_{31}\tilde{u}_{2} - r_{11}) + 1&
\frac{Z}{Z'}f_{x}f_{y}^{-1}(r_{32}\tilde{u}_{2} - r_{12})
\\
\frac{Z}{Z'}f_{x}^{-2}f_{y}(r_{31}\tilde{v}_{2} - r_{21})(u_{1}-c_{x})&
\tilde{v}_{2} + \frac{Z}{Z'}f_{y}^{-1}(r_{32}\tilde{v}_{2} - r_{22})(v_{1}-c_{y})&
\frac{Z}{Z'}f_{x}^{-1}f_{y}(r_{31}\tilde{v}_{2} - r_{21})&
\frac{Z}{Z'}(r_{32}\tilde{u}_{2} - r_{12}) + 1
\end{bmatrix} \in \mathbb{R}^{2\times4}
\end{aligned} }
\end{equation}

 

8.2.1. Code Implementations

• DSO code: Residuals.cpp#L123
  
  • For a detailed description of the code, refer to [SLAM] Direct Sparse Odometry (DSO) 논문 및 코드 리뷰 (2).

 

8.3. Jacobian of inverse depth

NOMENCLATURE of jacobian of inverse depth

• $\mathbf{X} =  [X,Y,Z,1]^{\intercal} = [\tilde{\mathbf{X}}, 1]^{\intercal} \in \mathbb{P}^{3}$
• $\tilde{\mathbf{X}} = [X,Y,Z]^{\intercal} \in \mathbb{P}^{2}$
• $\rho = \frac{1}{Z}, \rho^{-1} = Z$

 

8.3.1. Inverse depth parameterization

In SLAM, inverse depth parameterization refers to a method of expressing a 3D point $\mathbf{X}$ using only one parameter (reciprocal $\rho$ of $Z$) instead of using three parameters $[X,Y,Z,1]$. This allows us to perfectly represent the 3D point $\mathbf{X}$ using only the inverse depth $\rho$ if only the position of pixel $\mathbf{p} = [u,v]$ on the image plane is known.  This has a computational advantage since only one parameter needs to be estimated when performing the optimization.

 

8.3.2. Jacobian of inverse depth

Let's assume that $\mathbf{J}_{\rho}$ for the photometric error is obtained when the Jacobian of the inverse depth is $\mathbf{J}_{\rho}$.  This can be expressed as:

\begin{equation}
 \begin{aligned}
    \mathbf{J}_{\rho} & = \frac{\partial \mathbf{e}}{\partial \rho}  \\ & =  \frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}} \frac{\partial \mathbf{p}_{2}}{\partial \tilde{\mathbf{p}}_{2}} \frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'} \frac{\partial \mathbf{X}'}{\partial \rho}  \\
& = \mathbb{R}^{1\times2} \cdot \mathbb{R}^{2\times3} \cdot \mathbb{R}^{3\times4} \cdot \mathbb{R}^{4\times1} = \mathbb{R}^{1\times1} 
  \end{aligned}
\end{equation}

 

At this time, the first $\frac{\partial \mathbf{I}}{\partial \mathbf{p}_{2}}$ term is the Jacobian required to obtain the photometric error, and the remaining three Jacobians are reprojection. , is a term that always needs to be found, regardless of the photometric error term. So $\frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p}}_{ 2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial  \rho}$ can be applied to all error terms generally used in SLAM.

 

First of all, expressing $\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}$ as an inverse depth is as follows.  This is the same as expressed by substituting $\rho' = \frac{1}{Z'}$.

\begin{equation}
\boxed{  \begin{aligned}
    \frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'} & = 
    \frac{\partial [\tilde{u}_{2}, \tilde{v}_{2}, 1]}{\partial \mathbf{X}'} \\
    & = \begin{bmatrix} \rho' & 0 & -\rho'^{2}X' & 0\\
    0 & \rho' & -\rho'^{2}Y' &0 \\ 0 & 0 & 0 & 0 \end{bmatrix} \in \mathbb{R}^{3\times 4}
  \end{aligned} }
\end{equation}

 

Next, we need to find $\frac{\partial \mathbf{X}'}{\partial \rho}$.  First of all, $\mathbf{X}'$ can be written as follows.

\begin{equation}
   \begin{aligned}
\mathbf{X}' = \begin{bmatrix} \tilde{\mathbf{X}'} \\ 1 \end{bmatrix} & = \begin{bmatrix}\mathbf{R}\tilde{\mathbf{X}} + \mathbf{t} \\ 1 \end{bmatrix} \\ & = \begin{bmatrix} \mathbf{R}\big(Z\mathbf{K}^{-1}\tilde{\mathbf{X}} \big) + \mathbf{t} \\ 1 \end{bmatrix} \\ &= \begin{bmatrix} \mathbf{R}\big( \frac{\mathbf{K}^{-1}\tilde{\mathbf{X}}}{\rho} \big) + \mathbf{t} \\ 1 \end{bmatrix}
  \end{aligned} \label{eq:invd1}
\end{equation}

 

Referring to the above expression, $\frac{\partial \mathbf{X}'}{\partial \rho}$ is calculated as follows.

\begin{equation}
  \boxed{ \begin{aligned}
\frac{\partial \mathbf{X}'}{\partial \rho} &= \begin{bmatrix}-\mathbf{R} \big( \frac{\mathbf{K}^{-1}\tilde{\mathbf{X}}}{\rho^{2}} \big) \\ 0 \end{bmatrix} \\
&= \begin{bmatrix} -\frac{\tilde{\mathbf{X}}' - \mathbf{t}}{\rho} \\ 0 \end{bmatrix} \\
& = -\rho^{-1}\begin{bmatrix} X' - t_{x} \\ Y' - t_{y} \\ Z' - t_{z} \\ 0 \end{bmatrix} \in \mathbb{R}^{4\times 1}
  \end{aligned} } 
\end{equation}

 

The second line in the above equation can be obtained by transforming (\ref{eq:invd1}).  Using the above two Jacobians, $\frac{\partial \mathbf{p}_{2}}{\partial \rho}$ is finally obtained as follows.

\begin{equation}
  \boxed{ \begin{aligned}
\frac{\partial \mathbf{p}_{2}}{\partial \rho} &  = \frac{\partial \mathbf{p}_{2}}{\partial\tilde{\mathbf{p}}_{2}}\frac{\partial \tilde{\mathbf{p}}_{2}}{\partial \mathbf{X}'}\frac{\partial \mathbf{X}'}{\partial  \rho}  \\
&=  \begin{bmatrix} f_x & 0 & c_{x} \\ 0 & f_y & c_{y} \end{bmatrix} \begin{bmatrix} \rho' & 0 & -\rho'^{2}X' & 0\\
    0 & \rho' & -\rho'^{2}Y' &0 \\ 0 & 0 & 0 & 0 \end{bmatrix} \cdot  -\rho^{-1}\begin{bmatrix} X' - t_{x} \\ Y' - t_{y} \\ Z' - t_{z} \\ 0 \end{bmatrix}  \\
&= -\rho^{-1}\rho' \begin{bmatrix} f_x(\tilde{u}_{2}t_{z} - t_{x}) \\ f_y(\tilde{v}_{2}t_{z} - t_{y})  \end{bmatrix} \in \mathbb{R}^{2 \times 1}
  \end{aligned} } 
\end{equation}

- $\tilde{u}_{2} = \frac{X'}{Z'} = \rho'  X'$

- $\tilde{v}_{2} = \frac{Y'}{Z'} = \rho'  Y'$

 

8.3.3. Code Implementations

• DSO code: CoarseInitializer.cpp#L424   
  • For a detailed description of the code, refer to [SLAM] Direct Sparse Odometry (DSO) 논문 및 코드 리뷰 (2).

 

References

[1] [Blog] [SLAM] Bundle Adjustment 개념 리뷰: Reprojection error

[2] [Blog] [SLAM] Optical Flow와 Direct Method 개념 및 코드 리뷰: Photometric error

[3] [Blog] [SLAM] Pose Graph Optimization 개념 설명 및 예제 코드 분석: Relative pose error

[4] [Blog] Plücker Coordinate 개념 정리: Line projection error