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

1. Introduction

This post explains the definition of various errors used in SLAM and the Jacobian derivations for nonlinear optimization.

 

The errors covered in this post are:

• Reprojection error $$\begin{array}{}\text{(1)}& \begin{array}{rl}\mathbf{e}& =\mathbf{p}-\hat{\mathbf{p}}\in {\mathbb{R}}^2\end{array}\end{array}$$
• Photometric error $$\begin{array}{}\text{(2)}& \begin{array}{rl}\mathbf{e}& ={\mathbf{I}}_1({\mathbf{p}}_1)-{\mathbf{I}}_2({\mathbf{p}}_2)\in {\mathbb{R}}^1\end{array}\end{array}$$
• Relative pose error (PGO)  $$\begin{array}{}\text{(3)}& \begin{array}{r}{\mathbf{e}}_{ij}=\text{Log}({\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij})\in {\mathbb{R}}^6\end{array}\end{array}$$ 
• Line reprojection error $$\begin{array}{}\text{(4)}& \begin{array}{r}{\mathbf{e}}_l=\left[\begin{array}{cc}\frac{{\mathbf{x}}_s^{⊺}{l}_c}{\sqrt{l_1^2+l_2^2}},& \frac{{\mathbf{x}}_e^{⊺}{l}_c}{\sqrt{l_1^2+l_2^2}}\end{array}\right]\in {\mathbb{R}}^2\end{array}\end{array}$$
  
• IMU measurement error  : TBA $$\begin{array}{}\text{(5)}& \begin{array}{r}{\mathbf{e}}_{\mathcal{B}}=\left[\begin{array}{c}\delta {\alpha }_{b_{k+1}}^{b_k}\\ \delta {\mathit{\theta }}_{b_{k+1}}^{b_k}\\ \delta {\beta }_{b_{k+1}}^{b_k}\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_g\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_w^{b_k}({\mathbf{p}}_{b_{k+1}}^w-{\mathbf{p}}_{b_k}^w-{\mathbf{v}}_{b_k}^w\mathrm{\Delta }{t}_k+\frac{1}{2}{\mathbf{g}}^w\mathrm{\Delta }{t}_k^2)-{\hat{\alpha }}_{b_{k+1}}^{b_k}\\ 2[({\hat{\gamma }}_{b_{k+1}}^{b_k}{)}^{-1}\otimes ({\mathbf{q}}_{b_k}^w{)}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w{]}_{xyz}\\ {\mathbf{R}}_w^{b_k}({\mathbf{v}}_{b_{k+1}}^w-{\mathbf{v}}_{b_k}^w+{\mathbf{g}}^w\mathrm{\Delta }{t}_k)-{\hat{\beta }}_{b_{k+1}}^{b_k}\\ {\mathbf{b}}_{a_{k+1}}-{\mathbf{b}}_{a_k}\\ {\mathbf{b}}_{g_{k+1}}-{\mathbf{b}}_{g_k}\end{array}\right]\end{array}\end{array}$$
    
  

 

Different Jacobians are derived depending on whether the camera pose is expressed as a rotation matrix $\mathbf{R}\in SO(3)$ or a transformation matrix $\mathbf{T}\in SE(3)$.  To obtain the two Jacobians, we derive the Jacobian for SO(3) in case of reprojection error and the Jacobian for SE(3) in case of photometric error. Points in 3D space also have different Jacobians depending on how $\mathbf{X}=[X,Y,Z,W{]}^{⊺}$ is expressed and inverse depth $\rho$ is expressed. .  The Jacobian derivation process for both cases is also described.

 

The Jacobians covered in this post are:

• Camera pose (SO(3)-based) $$\begin{array}{}\text{(6)}& \begin{array}{r}\frac{\partial \mathbf{e}}{\partial \mathbf{R}}\to \frac{\partial \mathbf{e}}{\partial \mathrm{\Delta }\mathbf{w}}\end{array}\end{array}$$
• Camera pose (SE(3)-based) $$\begin{array}{}\text{(7)}& \begin{array}{r}\frac{\partial \mathbf{e}}{\partial \mathbf{T}}\to \frac{\partial \mathbf{e}}{\partial \mathrm{\Delta }\xi }\end{array}\end{array}$$
• Map point $$\begin{array}{}\text{(8)}& \begin{array}{r}\frac{\partial \mathbf{e}}{\partial \mathbf{X}}\end{array}\end{array}$$
• Relative pose (SE(3)-based) $$\begin{array}{}\text{(9)}& \begin{array}{rl}& \frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_i},\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_j}\end{array}\end{array}$$ 
• 3D plücker line $$\begin{array}{}\text{(10)}& \begin{array}{rl}& \frac{\partial {\mathbf{e}}_l}{\partial l},\frac{\partial l}{\partial {\mathcal{L}}_c},\frac{\partial {\mathcal{L}}_c}{\partial {\mathcal{L}}_w},\frac{\partial {\mathcal{L}}_w}{\partial {\delta }_{\mathit{\theta }}}\end{array}\end{array}$$  
• Quaternion representation $$\begin{array}{}\text{(11)}& \begin{array}{rl}& \frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{q}}\end{array}\end{array}$$  
• Camera intrinsics $$\begin{array}{}\text{(12)}& \begin{array}{rl}& \frac{\partial \mathbf{e}}{\partial f_x},\frac{\partial \mathbf{e}}{\partial f_y},\frac{\partial \mathbf{e}}{\partial c_x},\frac{\partial \mathbf{e}}{\partial c_y}\end{array}\end{array}$$  
• Inverse depth  $$\begin{array}{}\text{(13)}& \begin{array}{r}\frac{\partial \mathbf{e}}{\partial \rho }\end{array}\end{array}$$  
• IMU error-state system kinematics : TBA $$\begin{array}{}\text{(14)}& \begin{array}{r}{\mathbf{J}}_{b_{k+1}}^{b_k},{\mathbf{P}}_{b_{k+1}}^{b_k},\mathbf{F},\mathbf{G}\end{array}\end{array}$$
• IMU measurement : TBA $$\begin{array}{}\text{(15)}& \begin{array}{r}\frac{\partial {\mathbf{e}}_{\mathcal{B}}}{\partial [{\mathbf{p}}_{b_k}^w,{\mathbf{q}}_{b_k}^w]},\frac{\partial {\mathbf{e}}_{\mathcal{B}}}{\partial [{\mathbf{v}}_{b_k}^w,{\mathbf{b}}_{a_k},{\mathbf{b}}_{g_k}]},\frac{\partial {\mathbf{e}}_{\mathcal{B}}}{\partial [{\mathbf{p}}_{b_{k+1}}^w,{\mathbf{q}}_{b_{k+1}}^w]},\frac{\partial {\mathbf{e}}_{\mathcal{B}}}{\partial [{\mathbf{v}}_{b_{k+1}}^w,{\mathbf{b}}_{a_{k+1}},{\mathbf{b}}_{g_{k+1}}]}\end{array}\end{array}$$

 

2. Optimization formulation

2.1. Error derivation

In SLAM, error is defined as the difference between an observed value (measurement) $\mathbf{z}$ based on sensor data and an estimated value  $\hat{\mathbf{z}}$ based on mathematical modeling.

$$\begin{array}{}\text{(16)}& \boxed{{\displaystyle \begin{array}{r}\mathbf{e}(\mathbf{x})=\mathbf{z}-\hat{\mathbf{z}}(\mathbf{x})\end{array}}}\end{array}$$

- $\mathbf{x}$: state variable for the model

 

As above, the difference between the observed value and the predicted value is set as the error, and the optimal state variable $\mathbf{x}$ that minimizes the error is calculated as an optimization problem in SLAM.  At this time, since the state variables of SLAM in general include non-linear terms related to rotation, the non-linear least squares method is mainly used.

 

2.2. Error function derivation

In general, when a large amount of sensor data comes in, dozens to hundreds of errors are calculated in the form of vectors.  At this time, it is assumed that the error follows a normal distribution, and the work of transforming it into an error function is performed.

$$\begin{array}{}\text{(17)}& \begin{array}{r}\mathbf{e}(\mathbf{x})=\mathbf{z}-\hat{\mathbf{z}}\sim \mathcal{N}(\hat{\mathbf{z}},\mathbf{\Sigma })\end{array}\end{array}$$

 

The multivariate normal distribution for modeling the error function is

$$\begin{array}{}\text{(18)}& \begin{array}{r}p(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^n|\mathbf{\Sigma }|}}\exp (-\frac{1}{2}(\mathbf{x}-\mu {)}^{⊺}\mathbf{\Omega }(\mathbf{x}-\mu ))\sim \mathcal{N}(\mu ,\mathbf{\Sigma })\end{array}\end{array}$$

- $\mathbf{\Omega }={\mathbf{\Sigma }}^{-1}$ : information matrix (inverse of covariance matrix)

 

We can model the error as a multivariate normal distribution with mean $\hat{\mathbf{z}}$ and variance $\mathbf{\Sigma }$.  $\ln p(\mathbf{z})$ obtained by applying log-likelihood to the expression is as follows.

$$\begin{array}{}\text{(19)}& \begin{array}{rl}\ln p(\mathbf{z})& \propto -\frac{1}{2}(\mathbf{z}-\hat{\mathbf{z}}{)}^T\mathbf{\Omega }(\mathbf{z}-\hat{\mathbf{z}})\\ & \propto -\frac{1}{2}{\mathbf{e}}^{⊺}\mathbf{\Omega }\mathbf{e}\end{array}\end{array}$$

 

If you find the ${\mathbf{x}}^{\ast }$ where the log-likelihood $\ln p(\mathbf{z})$ is maximized, the probability of the multivariate normal distribution is maximized.  This is called Maximum Liklihood Estimation (MLE).  Since $\ln p(\mathbf{z})$ has a negative number (-) in front of it, if you find $\ln p(\mathbf{z})$ that minimizes the negative log-likelihood, you get:

$$\begin{array}{}\text{(20)}& \begin{array}{r}{\mathbf{x}}^{\ast }=\arg maxp(\mathbf{z})=\arg min{\mathbf{e}}^T\mathbf{\Omega }\mathbf{e}\end{array}\end{array}$$

 

When all errors are added together, instead of a single error, it is expressed as follows, and this is called the error function $\mathbf{E}$.  In an actual optimization problem, we find ${\mathbf{x}}^{\ast }$ that minimizes the error function $\mathbf{E}$ rather than a single error ${\mathbf{e}}_i$.

$$\begin{array}{}\text{(21)}& \boxed{{\displaystyle \begin{array}{rl}\mathbf{E}(\mathbf{x})& =\sum _i{\mathbf{e}}_i^T{\mathbf{\Omega }}_i{\mathbf{e}}_i\\ {\mathbf{x}}^{\ast }& =\arg min\mathbf{E}(\mathbf{x})\end{array}}}\end{array}$$

 

2.3. Non-linear least squares

The final optimization equation to be solved is: 

$$\begin{array}{}\text{(22)}& \begin{array}{r}{\mathbf{x}}^{\ast }=\arg min\mathbf{E}(\mathbf{x})=\arg min\sum _i{\mathbf{e}}_i^T{\mathbf{\Omega }}_i{\mathbf{e}}_i\end{array}\end{array}$$

 

In the above formula, we need to find the optimal parameter ${\mathbf{x}}^{\ast }$ that minimizes the error.  However, the above formula does not have a closed-form solution because it usually includes non-linear terms for rotation in SLAM.  Therefore, it is necessary to solve the problem using nonlinear optimization methods (Gauss-Newton (GN), Levenberg-Marquardt (LM)).  Among the actual implemented SLAM codes, information matrix ${\mathbf{\Omega }}_i$ is often set to ${\mathbf{I}}_3$ to find the optimal value for ${\mathbf{e}}_i^{⊺}{\mathbf{e}}_i$.

 

For example, suppose we solve the problem using the GN method.  The order of solving the problem is as follows.

1. define the error function
2. Approximate linearization with Taylor expansion
3. Set to 0 after the first derivative.
4. At this time, find the value and substitute it into the error function.
5. Repeat until the values ​​converge.

 

The detailed expression of the error function $\mathbf{e}$ is equivalent to $\mathbf{e}(\mathbf{x})$, which means that the value of the error function varies depending on the robot's pose vector $\mathbf{x}$. do.  The GN method iteratively updates the incremental amount $\mathrm{\Delta }\mathbf{x}$ in the direction of decreasing error in $\mathbf{e}(\mathbf{x})$.
$$\begin{array}{}\text{(23)}& \begin{array}{r}\mathbf{e}(\mathbf{x}+\mathrm{\Delta }\mathbf{x}{)}^{⊺}\mathrm{\Omega }\mathbf{e}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})\end{array}\end{array}$$

At this time, if $\mathbf{e}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})$ is used for the first-order Taylor expansion around $\mathbf{x}$, the above expression is approximated as follows.
$$\begin{array}{}\text{(24)}& \begin{array}{rl}\mathbf{e}(\mathbf{x}+\mathrm{\Delta }\mathbf{x}){|}_{\mathbf{x}}& \approx \mathbf{e}(\mathbf{x})+\mathbf{J}(\mathbf{x}+\mathrm{\Delta }\mathbf{x}-\mathbf{x})\\ & =\mathbf{e}(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x}\end{array}\end{array}$$

In this case, $\mathbf{J}=\frac{\partial \mathbf{e}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})}{\partial \mathbf{x}}$.  Applying this to the entire error function gives:
$$\begin{array}{}\text{(25)}& \begin{array}{r}\mathbf{e}(\mathbf{x}+\mathrm{\Delta }\mathbf{x}{)}^{⊺}\mathrm{\Omega }\mathbf{e}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})\approx (\mathbf{e}+\mathbf{J}\mathrm{\Delta }\mathbf{x}{)}^{⊺}\mathrm{\Omega }(\mathbf{e}+\mathbf{J}\mathrm{\Delta }\mathbf{x})\end{array}\end{array}$$

Expanding the above expression and substituting it gives:

$$\begin{array}{}\text{(26)}& \begin{array}{rl}& =\underset{\mathbf{c}}{\underbrace{{\mathbf{e}}^{⊺}\mathrm{\Omega }\mathbf{e}}}+2\underset{\mathbf{b}}{\underbrace{{\mathbf{e}}^{⊺}\mathrm{\Omega }\mathbf{J}}}\mathrm{\Delta }\mathbf{x}+\mathrm{\Delta }{\mathbf{x}}^{⊺}\underset{\mathbf{H}}{\underbrace{{\mathbf{J}}^{⊺}\mathrm{\Omega }\mathbf{J}}}\mathrm{\Delta }\mathbf{x}\\ & =\mathbf{c}+2\mathbf{b}\mathrm{\Delta }\mathbf{x}+\mathrm{\Delta }{\mathbf{x}}^{⊺}\mathbf{H}\mathrm{\Delta }\mathbf{x}\end{array}\end{array}$$

Applying this to the total error gives:

$$\begin{array}{}\text{(27)}& \begin{array}{r}\mathbf{E}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})=\sum _i{\mathbf{e}}_i^{⊺}{\mathrm{\Omega }}_i{\mathbf{e}}_i=\mathbf{c}+2\mathbf{b}\mathrm{\Delta }\mathbf{x}+\mathrm{\Delta }{\mathbf{x}}^T\mathbf{H}\mathrm{\Delta }\mathbf{x}\end{array}\end{array}$$

$\mathbf{E}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})$ is the quadratic form of $\mathrm{\Delta }\mathbf{x}$ and $\mathbf{H}={\mathbf{J}}^{⊺}\mathrm{\Omega }\mathbf{J}$ is a positive definite matrix, the first derivative of $\mathbf{E}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})$ is reduced to zero. The set value becomes the minimum of $\mathrm{\Delta }\mathbf{x}$.

$$\begin{array}{}\text{(28)}& \begin{array}{r}\frac{\partial \mathbf{E}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})}{\partial \mathrm{\Delta }\mathbf{x}}\approx 2\mathbf{b}+2\mathbf{H}\mathrm{\Delta }\mathbf{x}=0\end{array}\end{array}$$

Summarizing this, the following formula is derived:
$$\begin{array}{}\text{(29)}& \begin{array}{r}\mathbf{H}\mathrm{\Delta }\mathbf{x}=-\mathbf{b}\end{array}\end{array}$$

$\mathrm{\Delta }\mathbf{x}=-{\mathbf{H}}^{-1}\mathbf{b}$ obtained in this way is updated to $\mathbf{x}$.

$$\begin{array}{}\text{(30)}& \begin{array}{r}\mathbf{x}\leftarrow \mathbf{x}+\mathrm{\Delta }\mathbf{x}\end{array}\end{array}$$

The algorithm that performs the process iteratively so far is called the Gauss-Newton method.  Compared to the GN method, the LM method has the same overall process, but a damping factor $\lambda$ term is added in the formula for calculating the increment.

$$\begin{array}{}\text{(31)}& \begin{array}{rl}& \text{(GN) }\mathbf{H}\mathrm{\Delta }\mathbf{x}=-\mathbf{b}\\ & \text{(LM) }(\mathbf{H}+\lambda \mathbf{I})\mathrm{\Delta }\mathbf{x}=-\mathbf{b}\end{array}\end{array}$$

 

3. Reprojection error

Reprojection error is an error mainly used in feature-based Visual SLAM.  It is mainly used when performing visual odometry (VO) or bundle adjustment (BA) based on feature-based methods.  For more information about BA, see [SLAM] Bundle Adjustment 개념 리뷰 post

NOMENCLATURE of reprojection error

• $\tilde{\mathbf{p}}={\pi }_h(\cdot )$
  • A non-homogeneous transformation of the point ${\mathbf{X}}^{\prime }$ in 3-dimensional space for projection onto the image plane.
  • $\tilde{\mathbf{p}}:\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\\ 1\end{array}\right]\to \frac{1}{Z^{\prime }}{\tilde{\mathbf{X}}}^{\prime }=\left[\begin{array}{c}{X}^{\prime }/Z^{\prime }\\ Y^{\prime }/Z^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{c}\tilde{u}\\ \tilde{v}\\ 1\end{array}\right]$
• $\hat{\mathbf{p}}={\pi }_k(\cdot )$
  • A point projected onto the image plane after correcting for lens distortion.  If distortion correction has already been performed at the input stage, ${\pi }_k(\cdot )=\tilde{\mathbf{K}}(\cdot )$.
  • $\hat{\mathbf{p}}=\tilde{\mathbf{K}}\tilde{\mathbf{p}}=\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]\left[\begin{array}{c}\tilde{u}\\ \tilde{v}\\ 1\end{array}\right]=\left[\begin{array}{c}f\tilde{u}+c_x\\ f\tilde{v}+c_y\end{array}\right]=\left[\begin{array}{c}u\\ v\end{array}\right]$
• $\mathbf{K}=\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\\ 0& 0& 1\end{array}\right]$: Camera intrinsic matrix
• $\tilde{\mathbf{K}}=\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]$: I omitted the last line of internal parameters to project to ${\mathbb{P}}^2\to {\mathbb{R}}^2$.
• $\mathcal{X}=[{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_m,{\mathbf{X}}_1,\cdots ,{\mathbf{X}}_n{]}^{⊺}$: State variable of model
• $m$ : number of camera poses
• $n$: number of 3D points
• ${\mathcal{T}}_i=[{\mathbf{R}}_i,{\mathbf{t}}_i]$
• ${\mathbf{e}}_{ij}={\mathbf{e}}_{ij}(\mathcal{X})$: $\mathcal{X}$ is sometimes omitted for brevity.
• ${\mathbf{p}}_{ij}$: The pixel coordinates of the observed load
• ${\hat{\mathbf{p}}}_{ij}$: Pixel Coordinates of Estimated Feature Points
• ${\mathbf{T}}_i{\mathbf{X}}_j$: Transform the 3D point ${\mathbf{X}}_j$ into the camera coordinate system $\{i\}$, $({\mathbf{T}}_i{\mathbf{X}}_j=\left[\begin{array}{c}{\mathbf{R}}_i{\mathbf{X}}_j+{\mathbf{t}}_i\\ 1\end{array}\right]\in {\mathbb{R}}^{4\times 1})$
  
  • ${\mathbf{X}}^{\prime }=\mathbf{TX}=[X^{\prime },Y^{\prime },Z^{\prime },1{]}^{⊺}=[{\tilde{\mathbf{X}}}^{\prime },1{]}^{⊺}$
• $\oplus$ : Operator that can update the rotation matrix $\mathbf{R}$, three-dimensional vectors $\mathbf{t},and\mathbf{X}$ at once.
• $\mathbf{J}=\frac{\partial \mathbf{e}}{\partial \mathcal{X}}=\frac{\partial \mathbf{e}}{\partial [\mathcal{T},\mathbf{X}]}$
• $\mathbf{w}={\left[\begin{array}{ccc}{w}_x& w_y& w_z\end{array}\right]}^{⊺}$: Angular velocity
• $[\mathbf{w}{]}_{\times }=\left[\begin{array}{ccc}0& -w_z& w_y\\ w_z& 0& -w_x\\ -w_y& w_x& 0\end{array}\right]$ : Skew-symmetric matrix of angular velocity $\mathbf{w}$

${\mathbf{X}}_j$ is projected onto the image plane through the following transformation.

$$\begin{array}{}\text{(32)}& \begin{array}{r}\text{projection model: }{\hat{\mathbf{p}}}_{ij}=\pi ({\mathbf{T}}_i,{\mathbf{X}}_j)\end{array}\end{array}$$

A model that utilizes intrinsic/extrinsic parameters of the camera as above is called a projection model.  The reprojection error through this is defined as follows.

$$\begin{array}{}\text{(33)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{e}}_{ij}& ={\mathbf{p}}_{ij}-{\hat{\mathbf{p}}}_{ij}\\ & ={\mathbf{p}}_{ij}-\pi ({\mathbf{T}}_i,{\mathbf{X}}_j)\\ & ={\mathbf{p}}_{ij}-{\pi }_k({\pi }_h({\mathbf{T}}_i{\mathbf{X}}_j))\end{array}}}\end{array}$$

 

The error function for all camera poses and 3D points is defined as

$$\begin{array}{}\text{(34)}& \begin{array}{rl}\mathbf{E}(\mathcal{X})& =\arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\Vert {\mathbf{e}}_{ij}\Vert }^2\\ & =\arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\mathbf{e}}_{ij}^{⊺}{\mathbf{e}}_{ij}\\ & =\arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j({\mathbf{p}}_{ij}-{\hat{\mathbf{p}}}_{ij}{)}^{⊺}({\mathbf{p}}_{ij}-{\hat{\mathbf{p}}}_{ij})\end{array}\end{array}$$

 

${\Vert \mathbf{e}({\mathcal{X}}^{\ast })\Vert }^2$ that satisfies $\mathbf{E}({\mathcal{X}}^{\ast })$ can be computed iteratively through non-linear least squares.  The optimal state is found by iteratively updating $\mathcal{X}$ in small increments $\mathrm{\Delta }\mathcal{X}$.
$$\begin{array}{}\text{(35)}& \begin{array}{rl}\mathbf{E}(\mathcal{X}+\mathrm{\Delta }\mathcal{X})& =\arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\Vert \mathbf{e}(\mathcal{X}+\mathrm{\Delta }\mathcal{X})\Vert }^2\end{array}\end{array}$$

Strictly speaking, since the state increment $\mathrm{\Delta }\mathcal{X}$ includes the SO(3) rotation matrix, it is correct to add it to the existing state $\mathcal{X}$ through the $\oplus$ operator, but for convenience of expression The $+$ operator is used.

$$\begin{array}{}\text{(36)}& \begin{array}{r}\mathbf{e}(\mathcal{X}\oplus \mathrm{\Delta }\mathcal{X})\to \mathbf{e}(\mathcal{X}+\mathrm{\Delta }\mathcal{X})\end{array}\end{array}$$

 

The above equation can be expressed as follows through Taylor's first-order approximation.

$$\begin{array}{}\text{(37)}& \begin{array}{rl}\mathbf{e}(\mathcal{X}+\mathrm{\Delta }\mathcal{X})& \approx \mathbf{e}(\mathcal{X})+\mathbf{J}\mathrm{\Delta }\mathcal{X}\\ & =\mathbf{e}(\mathcal{X})+{\mathbf{J}}_c\mathrm{\Delta }\mathcal{T}+{\mathbf{J}}_p\mathrm{\Delta }\mathbf{X}\\ & =\mathbf{e}(\mathcal{X})+\frac{\partial \mathbf{e}}{\partial \mathcal{T}}\mathrm{\Delta }\mathcal{T}+\frac{\partial \mathbf{e}}{\partial \mathbf{X}}\mathrm{\Delta }\mathbf{X}\end{array}\end{array}$$

 

$$\begin{array}{}\text{(38)}& \begin{array}{rl}\mathbf{E}(\mathcal{X}+\mathrm{\Delta }\mathcal{X})& \approx \arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\Vert \mathbf{e}(\mathcal{X})+\mathbf{J}\mathrm{\Delta }\mathcal{X}\Vert }^2\end{array}\end{array}$$

 

By differentiating this, the optimal value of increment $\mathrm{\Delta }{\mathcal{X}}^{\ast }$ is obtained as follows.  The detailed derivation process is omitted in this section.  If you want to know more about the induction process, you can refer to the previous section 
$$\begin{array}{}\text{(39)}& \begin{array}{rl}& {\mathbf{J}}^{⊺}\mathbf{J}\mathrm{\Delta }{\mathcal{X}}^{\ast }=-{\mathbf{J}}^{⊺}\mathbf{e}\\ & \mathbf{H}\mathrm{\Delta }{\mathcal{X}}^{\ast }=-\mathbf{b}\end{array}\end{array}$$

 

Since the above equation is in the form of a linear system $\mathbf{Ax}=\mathbf{b}$, various linear algebra techniques such as schur complement and cholesky decomposition can be used to find $\mathrm{\Delta }{\mathcal{X}}^{\ast }$ .  At this time, $\mathbf{t}$ and $\mathbf{X}$ out of the existing states $\mathcal{X}$ exist in the linear vector space, so there is no difference whether adding from the right side or from the left side.  However, since the rotation matrix $\mathbf{R}$ belongs to the nonlinear SO(3) family, depending on whether it is multiplied by the right or left, the pose seen in the local coordinate system (right) or the pose seen in the global coordinate system is updated. (left) will change.  Reprojection errors update the global coordinate system's transformation matrix, so we usually use the left multiplication method.

$$\begin{array}{}\text{(40)}& \begin{array}{r}\mathcal{X}\leftarrow \mathcal{X}\oplus \mathrm{\Delta }{\mathcal{X}}^{\ast }\end{array}\end{array}$$

 

Since $\mathcal{X}$ is composed of $[\mathcal{T},\mathbf{X}]$, it can be written as follows.

$$\begin{array}{}\text{(41)}& \begin{array}{rl}\mathcal{T}\leftarrow \mathcal{T}& \oplus \mathrm{\Delta }{\mathcal{T}}^{\ast }\\ \mathbf{X}\leftarrow \mathbf{X}& \oplus \mathrm{\Delta }{\mathbf{X}}^{\ast }\end{array}\end{array}$$

 

The definition of the left multiplication $\oplus$ operation is as follows.

$$\begin{array}{}\text{(42)}& \begin{array}{rl}\mathbf{R}\oplus \mathrm{\Delta }{\mathbf{R}}^{\ast }& =\mathrm{\Delta }{\mathbf{R}}^{\ast }\mathbf{R}\\ & =\exp ([\mathrm{\Delta }{\mathbf{w}}^{\ast }{]}_{\times })\mathbf{R}\cdots \text{ globally updated (left mult)}\\ \mathbf{t}\oplus \mathrm{\Delta }{\mathbf{t}}^{\ast }& =\mathbf{t}+\mathrm{\Delta }{\mathbf{t}}^{\ast }\\ \mathbf{X}\oplus \mathrm{\Delta }{\mathbf{X}}^{\ast }& =\mathbf{X}+\mathrm{\Delta }{\mathbf{X}}^{\ast }\end{array}\end{array}$$

 

3.1. Jacobian of the reprojection error

3.1.1. Jacobian of camera pose

The Jacobian ${\mathbf{J}}_c$ for a pose can be decomposed as:
$$\begin{array}{}\text{(43)}& \begin{array}{rl}{\mathbf{J}}_c=\frac{\partial \mathbf{e}}{\partial \mathcal{T}}& =\frac{\partial }{\partial \mathcal{T}}(\mathbf{p}-\hat{\mathbf{p}})\\ & =\frac{\partial }{\partial \mathcal{T}}(\mathbf{p}-{\pi }_k({\pi }_h({\mathbf{T}}_i{\mathbf{X}}_j)))\\ & =\frac{\partial }{\partial \mathcal{T}}(-{\pi }_k({\pi }_h({\mathbf{T}}_i{\mathbf{X}}_j)))\end{array}\end{array}$$

 

By rearranging the above equation using the chain rule, we get:  At this time, for convenience, it is expressed as ${\mathbf{T}}_i{\mathbf{X}}_j\to {\mathbf{X}}^{\prime }$.

$$\begin{array}{}\text{(44)}& \begin{array}{rl}{\mathbf{J}}_c& =\frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}}\frac{\partial \tilde{\mathbf{p}}}{\partial {\mathbf{X}}^{\prime }}\frac{\partial {\mathbf{X}}^{\prime }}{\partial [\mathbf{w},\mathbf{t}]}\\ & ={\mathbb{R}}^{2\times 3}\cdot {\mathbb{R}}^{3\times 4}\cdot {\mathbb{R}}^{4\times 6}={\mathbb{R}}^{2\times 6}\end{array}\end{array}$$

 

At this time, the reason why the Jacobian $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{w}}$ for the angular velocity $\mathbf{w}$ instead of the Jacobian $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{R}}$ for the rotation matrix $\mathbf{R}$ will be explained in the next section. In addition, the sign of ${\mathbf{J}}_c$ is also changed depending on whether the error is defined as $\mathbf{p}-\hat{\mathbf{p}}$ or $\hat{\mathbf{p}}-\mathbf{p}$ , so this should be applied with care when implementing the actual code. In the data, the sign was considered and marked as $+$.

 

Assuming that undistortion has already been performed during the image input process, $\frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}}$ is as follows.
$$\begin{array}{}\text{(45)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}}& =\frac{\partial }{\partial \tilde{\mathbf{p}}}\tilde{\mathbf{K}}\tilde{\mathbf{p}}\\ & =\tilde{\mathbf{K}}\\ & =\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]\in {\mathbb{R}}^{2\times 3}\end{array}}}\end{array}$$

Next, $\frac{\partial \tilde{\mathbf{p}}}{\partial {\mathbf{X}}^{\prime }}$ is:
$$\begin{array}{}\text{(46)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial \tilde{\mathbf{p}}}{\partial {\mathbf{X}}^{\prime }}& =\frac{\partial [\tilde{u},\tilde{v},1]}{\partial [X^{\prime },Y^{\prime },Z^{\prime },1]}\\ & =\left[\begin{array}{cccc}\frac{1}{Z^{\prime }}& 0& \frac{-X^{\prime }}{Z^{\prime 2}}& 0\\ 0& \frac{1}{Z^{\prime }}& \frac{-Y^{\prime }}{Z^{\prime 2}}& 0\\ 0& 0& 0& 0\end{array}\right]\in {\mathbb{R}}^{3\times 4}\end{array}}}\end{array}$$

 

Next, we need to find $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{t}}$.  This can be obtained relatively simply as follows.
$$\begin{array}{}\text{(47)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{t}}& =\frac{\partial }{\partial [t_x,t_y,t_z]}\left[\begin{array}{c}\mathbf{R}\mathbf{X}+\mathbf{t}\\ 1\end{array}\right]\\ & =\frac{\partial }{\partial [t_x,t_y,t_z]}\left[\begin{array}{c}\mathbf{t}\\ 0\end{array}\right]\\ & =\frac{\partial }{\partial [t_x,t_y,t_z]}(\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\\ 0\end{array}\right])\\ & =\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]\in {\mathbb{R}}^{4\times 3}\end{array}}}\end{array}$$

 

3.1.2. Lie theory-based SO(3) optmization

Finally, we need to find $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{w}}$.  At this time, rotation-related parameters are expressed as angular velocity $\mathbf{w}$ rather than rotation matrix $\mathbf{R}$.  The rotation matrix $\mathbf{R}$ is over-parameterized because the number of parameters is 9, but the actual rotation is limited to 3 degrees of freedom.  Disadvantages of over-parameterized representation are as follows.

• Since redundant parameters must be calculated, the amount of computation increases during optimization.
• The additional degrees of freedom can lead to numerical instability problems.
• Whenever a parameter is updated, it must always be checked whether the constraint is satisfied.

 

Lie theory allows optimization to be free from constraints.  Therefore, by using lie algebra so(3) $\mathbf{w}$ instead of lie group SO(3) $\mathbf{R}$, parameters can be updated freely from constraints.

$$\begin{array}{}\text{(48)}& \begin{array}{r}{\mathbf{J}}_c=\frac{\partial \mathbf{e}}{\partial [\mathbf{R},\mathbf{t}]}\to \frac{\partial \mathbf{e}}{\partial [\mathbf{w},\mathbf{t}]}\end{array}\end{array}$$

 

However, since $\mathbf{w}$ is not immediately visible in ${\mathbf{X}}^{\prime }$, ${\mathbf{X}}^{\prime }$ must be expressed as lie algebra. 

At this time, since we need to find the Jacobian for the $\mathbf{w}$ term related to rotation, we translate the 3D point ${\mathbf{X}}_t$ by $\mathbf{t}$ and ${\mathbf{X}}^{\prime }$ Let be the point where the ${\mathbf{X}}_t$ at the same position is rotated by $\mathbf{R}$.

$$\begin{array}{}\text{(49)}& \begin{array}{rl}{\mathbf{X}}_t& =\mathbf{X}+\mathbf{t}\\ {\mathbf{X}}^{\prime }& =\mathbf{R}{\mathbf{X}}_t\\ & =\exp ([\mathbf{w}]\times ){\mathbf{X}}_t\end{array}\end{array}$$

 

$\exp ([\mathbf{w}{]}_{\times })\in SO(3)$ transforms the angular velocity $\mathbf{w}$ into a 3D rotation matrix $\mathbf{R}$ by exponential mapping refers to an operation that  For more information on exponential mapping, refer to this link.

$$\begin{array}{}\text{(50)}& \begin{array}{r}\exp ([\mathbf{w}{]}_{\times })=\mathbf{R}\end{array}\end{array}$$

 

 

At this time, there are two ways to update the small lie algebra increment $\mathrm{\Delta }\mathbf{w}$ to the existing $\exp ([\mathbf{w}{]}_{\times })$.  First of all, there is an update method using $[1]$ basic lie algebra.  Next, there is an update method using the $[2]$ perturbation model.

$$\begin{array}{}\text{(51)}& \begin{array}{rl}\exp ([\mathbf{w}{]}_{\times })& \leftarrow \exp ([\mathbf{w}+\mathrm{\Delta }\mathbf{w}{]}_{\times })\cdots \text{[1]}\\ \exp ([\mathbf{w}{]}_{\times })& \leftarrow \exp ([\mathrm{\Delta }\mathbf{w}{]}_{\times })\exp ([\mathbf{w}{]}_{\times })\cdots \text{[2]}\end{array}\end{array}$$

 

The relationship between the above two methods is as follows.  For details, refer to chapter 4.3.3 of this link.

$$\begin{array}{}\text{(52)}& \begin{array}{rl}& \exp ([\mathrm{\Delta }\mathbf{w}{]}_{\times })\exp ([\mathbf{w}{]}_{\times })=\exp ([\mathbf{w}+{\mathbf{J}}_l^{-1}\mathrm{\Delta }\mathbf{w}{]}_{\times })\\ & \exp ([\mathbf{w}+\mathrm{\Delta }\mathbf{w}{]}_{\times })=\exp ([{\mathbf{J}}_l\mathrm{\Delta }\mathbf{w}{]}_{\times })\exp ([\mathbf{w}{]}_{\times })\end{array}\end{array}$$

 

$[1]$ Lie algebra-based update: First of all, if you directly calculate $\frac{\partial \mathbf{R}{\mathbf{X}}_t}{\partial \mathbf{w}}$ using the $[1]$ method, you will get the following A complex diet is induced.

$$\begin{array}{}\text{(53)}& \begin{array}{rl}\frac{\partial \mathbf{R}{\mathbf{X}}_t}{\partial \mathbf{w}}& =\underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{\exp ([\mathbf{w}+\mathrm{\Delta }\mathbf{w}{]}_{\times }){\mathbf{X}}_t-\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t}{\mathrm{\Delta }\mathbf{w}}\\ & \approx \underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{\exp ([{\mathbf{J}}_l\mathrm{\Delta }\mathbf{w}{]}_{\times })(\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t-\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t}{\mathrm{\Delta }\mathbf{w}}\\ & \approx \underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{(\mathbf{I}+[{\mathbf{J}}_l\mathrm{\Delta }\mathbf{w}{]}_{\times })(\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t-\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t}{\mathrm{\Delta }\mathbf{w}}\\ & =\underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{[{\mathbf{J}}_l\mathrm{\Delta }\mathbf{w}{]}_{\times }\mathbf{R}{\mathbf{X}}_t}{\mathrm{\Delta }\mathbf{w}}(\because \exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t=\mathbf{R}{\mathbf{X}}_t)\\ & =\underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{-[\mathbf{R}{\mathbf{X}}_t{]}_{\times }{\mathbf{J}}_l\mathrm{\Delta }\mathbf{w}}{\mathrm{\Delta }\mathbf{w}}\\ & =-[\mathbf{R}{\mathbf{X}}_t{]}_{\times }{\mathbf{J}}_l\\ & =-[{\mathbf{X}}^{\prime }{]}_{\times }{\mathbf{J}}_l\end{array}\end{array}$$

 

In the above equation, the second row is a form in which the left Jacobian ${\mathbf{J}}_l$ is derived using the BCH approximation, and the third row is a form in which the first-order Taylor approximation is applied for a small rotation amount $\exp ([{\mathbf{J}}_l\mathrm{\Delta }\mathbf{w}{]}_{\times })$. For more information about ${\mathbf{J}}_l$, refer toChapter 4 of Introduction of Visual SLAM

 

To understand the approximation of the third row, given an arbitrary rotation vector $\mathbf{w}=[w_x,w_y,w_z{]}^{⊺}$, the rotation matrix is ​​exponential When developed in mapping form, it can be expressed as follows.

$$\begin{array}{}\text{(54)}& \begin{array}{r}\mathbf{R}=\exp ([\mathbf{w}{]}_{\times })=\mathbf{I}+[\mathbf{w}{]}_{\times }+\frac{1}{2}[\mathbf{w}{]}_{\times }^2+\frac{1}{3!}[\mathbf{w}{]}_{\times }^3+\frac{1}{4!}[\mathbf{w}{]}_{\times }^4+\cdots \end{array}\end{array}$$

For a small-sized rotation matrix $\mathrm{\Delta }\mathbf{R}$, it can be approximated as follows by ignoring higher-order terms of order 2 or higher.$$\begin{array}{}\text{(55)}& \begin{array}{r}\mathrm{\Delta }\mathbf{R}\approx \mathbf{I}+[\mathrm{\Delta }\mathbf{w}{]}_{\times }\end{array}\end{array}$$

 

$[2]$ Perturbation model-based update: In order to obtain a simpler Jacobian without using ${\mathbf{J}}_l$, the perturbation model of $[2]$ lie algebra so(3) is generally used.  The Jacobian $\frac{\partial \mathbf{R}{\mathbf{X}}_t}{\partial \mathrm{\Delta }\mathbf{w}}$ is obtained using the perturbation model as follows:

$$\begin{array}{}\text{(56)}& \begin{array}{rl}\frac{\partial \mathbf{R}{\mathbf{X}}_t}{\partial \mathrm{\Delta }\mathbf{w}}& =\underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{\exp ([\mathrm{\Delta }\mathbf{w}{]}_{\times })\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t-\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t}{\mathrm{\Delta }\mathbf{w}}\\ & \approx \underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{(\mathbf{I}+[\mathrm{\Delta }\mathbf{w}{]}_{\times })\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t-\exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t}{\mathrm{\Delta }\mathbf{w}}\\ & =\underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{[\mathrm{\Delta }\mathbf{w}{]}_{\times }\mathbf{R}{\mathbf{X}}_t}{\mathrm{\Delta }\mathbf{w}}(\because \exp ([\mathbf{w}{]}_{\times }){\mathbf{X}}_t=\mathbf{R}{\mathbf{X}}_t)\\ & =\underset{\mathrm{\Delta }\mathbf{w}\to 0}{lim}\frac{-[\mathbf{R}{\mathbf{X}}_t{]}_{\times }\mathrm{\Delta }\mathbf{w}}{\mathrm{\Delta }\mathbf{w}}\\ & =-[\mathbf{R}{\mathbf{X}}_t{]}_{\times }\\ & =-[{\mathbf{X}}^{\prime }{]}_{\times }\end{array}\end{array}$$

 

The above equation also uses approximate $\exp ([\mathrm{\Delta }\mathbf{w}{]}_{\times })\approx \mathbf{I}+[\mathrm{\Delta }\mathbf{w}{]}_{\times }$ for a small rotation matrix in the second row. Therefore, when using the $[2]$ perturbation model, there is an advantage in that the Jacobian can be obtained simply by using an skew-symmetric matrix of the points ${\mathbf{X}}^{\prime }$ in the 3D space.  In the case of reprojection error optimization, since most of the errors for the feature points of sequentially incoming images are optimized, the camera pose change is not large and the size of $\mathrm{\Delta }\mathbf{w}$ is not large, so the Jacobian above is generally used. Since the $[2]$ method is used, when updating the small increment $\mathrm{\Delta }\mathbf{w}$ to the existing rotation matrix $\mathbf{R}$, it is updated as ($\text{42}$).

$$\begin{array}{}\text{(57)}& \begin{array}{r}\mathbf{R}\leftarrow \mathrm{\Delta }{\mathbf{R}}^{\ast }\mathbf{R}\text{where, }\mathrm{\Delta }{\mathbf{R}}^{\ast }=\exp ([\mathrm{\Delta }{\mathbf{w}}^{\ast }{]}_{\times })\end{array}\end{array}$$

 

So the original Jacobian is $\frac{\partial {\mathbf{X}}^{\prime }}{\partial [\mathbf{w},\mathbf{t}]}$ to $\frac{\partial {\mathbf{X}}^{\prime }}{\partial [\mathrm{\Delta }\mathbf{w},\mathbf{t}]}$, which looks like this:

$$\begin{array}{}\text{(58)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial }{\partial [\mathrm{\Delta }\mathbf{w},\mathbf{t}]}\left[\begin{array}{c}\mathbf{RX}+\mathbf{t}\\ 1\end{array}\right]& =\left[\begin{array}{cccccc}0& Z^{\prime }& -Y^{\prime }& 1& 0& 0\\ -Z^{\prime }& 0& X^{\prime }& 0& 1& 0\\ Y^{\prime }& -X^{\prime }& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]\in {\mathbb{R}}^{4\times 6}\end{array}}}\end{array}$$

The Jacobian ${\mathbf{J}}_c$ for the final pose is
$$\begin{array}{}\text{(59)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{J}}_c& =\frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}}\frac{\partial \tilde{\mathbf{p}}}{\partial {\mathbf{X}}^{\prime }}\frac{\partial {\mathbf{X}}^{\prime }}{\partial [\mathrm{\Delta }\mathbf{w},\mathbf{t}]}\\ & =\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]\left[\begin{array}{cccc}\frac{1}{Z^{\prime }}& 0& \frac{-X^{\prime }}{Z^{\prime 2}}& 0\\ 0& \frac{1}{Z^{\prime }}& \frac{-Y^{\prime }}{Z^{\prime 2}}& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{cccccc}0& Z^{\prime }& -Y^{\prime }& 1& 0& 0\\ -Z^{\prime }& 0& X^{\prime }& 0& 1& 0\\ Y^{\prime }& -X^{\prime }& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]\\ & =\left[\begin{array}{cccc}\frac{f}{Z^{\prime }}& 0& -\frac{fX}{Z^{\prime 2}}& 0\\ 0& \frac{f}{Z^{\prime }}& -\frac{fY}{Z^{\prime 2}}& 0\end{array}\right]\left[\begin{array}{cccccc}0& Z^{\prime }& -Y^{\prime }& 1& 0& 0\\ -Z^{\prime }& 0& X^{\prime }& 0& 1& 0\\ Y^{\prime }& -X^{\prime }& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]\\ & =\left[\begin{array}{cccccc}-\frac{f{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f(1+X^{\prime 2})}{Z^{\prime 2}}& -\frac{f{Y}^{\prime }}{Z^{\prime }}& \frac{f}{Z^{\prime }}& 0& -\frac{f{X}^{\prime }}{Z^{\prime 2}}\\ -\frac{f(1+y^2)}{Z^{\prime 2}}& \frac{f{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f{X}^{\prime }}{Z^{\prime }}& 0& \frac{f}{Z^{\prime }}& -\frac{f{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\in {\mathbb{R}}^{2\times 6}\end{array}}}\end{array}$$

 

3.1.3. Code implementations

• g2o code: edge_project_xyz.cpp#L82 

 

3.2. Jacobian of Map Point

The Jacobian ${\mathbf{J}}_p$ for a 3D point $\mathbf{X}$ can be obtained as follows.

$$\begin{array}{}\text{(60)}& \begin{array}{rl}{\mathbf{J}}_p=\frac{\partial \mathbf{e}}{\partial \mathbf{X}}& =\frac{\partial }{\partial \mathbf{X}}(\mathbf{p}-\hat{\mathbf{p}})\\ & =\frac{\partial }{\partial \mathbf{X}}(\mathbf{p}-{\pi }_k({\pi }_h({\mathbf{T}}_i{\mathbf{X}}_j)))\\ & =\frac{\partial }{\partial \mathbf{X}}(-{\pi }_k({\pi }_h({\mathbf{T}}_i{\mathbf{X}}_j)))\end{array}\end{array}$$

By rearranging the above equation using the chain rule, we get:
$$\begin{array}{}\text{(61)}& \begin{array}{rl}{\mathbf{J}}_p& =\frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}}\frac{\partial \tilde{\mathbf{p}}}{\partial {\mathbf{X}}^{\prime }}\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{X}}\\ & ={\mathbb{R}}^{2\times 3}\cdot {\mathbb{R}}^{3\times 4}\cdot {\mathbb{R}}^{4\times 4}={\mathbb{R}}^{2\times 4}\end{array}\end{array}$$

Of these, $\frac{\partial \hat{\mathbf{p}}}{\partial \tilde{\mathbf{p}}}\frac{\partial \hat{\mathbf{p}}}{\partial {\mathbf{X}}^{\prime }}$ is the same as the Jacobian obtained earlier.  So we only need to calculate $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{X}}$.

$$\begin{array}{}\text{(62)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{X}}& =\frac{\partial }{\partial \mathbf{X}}\left[\begin{array}{c}\mathbf{R}\mathbf{X}+\mathbf{t}\\ 1\end{array}\right]\\ & =\left[\begin{array}{c}\mathbf{R}\\ 0\end{array}\right]\end{array}}}\end{array}$$

Therefore, ${\mathbf{J}}_p$ is
$$\begin{array}{}\text{(63)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{J}}_p& =\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]\left[\begin{array}{cccc}\frac{1}{Z^{\prime }}& 0& \frac{-X^{\prime }}{Z^{\prime 2}}& 0\\ 0& \frac{1}{Z^{\prime }}& \frac{-Y^{\prime }}{Z^{\prime 2}}& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\\ 0\end{array}\right]\\ & =\left[\begin{array}{cccc}\frac{f}{Z^{\prime }}& 0& -\frac{f{X}^{\prime }}{Z^{\prime 2}}& 0\\ 0& \frac{f}{Z^{\prime }}& -\frac{f{Y}^{\prime }}{Z^{\prime 2}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\\ 0\end{array}\right]\in {\mathbb{R}}^{2\times 4}\end{array}}}\end{array}$$

In general, since the last column of ${\mathbf{J}}_p$ is always 0, it is sometimes omitted to represent non-homogeneous form.

$$\begin{array}{}\text{(64)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{J}}_p& =\left[\begin{array}{ccc}\frac{f}{Z^{\prime }}& 0& -\frac{f{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f}{Z^{\prime }}& -\frac{f{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\mathbf{R}\in {\mathbb{R}}^{2\times 3}\end{array}}}\end{array}$$

 

3.2.1. Code implementations

•  g2o code: edge_project_xyz.cpp#L80 

 

4. Photometric error

Photometric error is an error mainly used in direct Visual SLAM.  It is mainly used when performing direct method-based visual odometry (VO) or bundle adjustment (BA).  For more information about direct methods, see [SLAM] Optical Flow와 Direct Method 개념 및 코드 리뷰 post.

 

NOMENCLATURE of photometric error

• ${\tilde{\mathbf{p}}}_2={\pi }_h(\cdot )$
  • A non-homogeneous transformation of the point ${\mathbf{X}}^{\prime }$ in 3-dimensional space for projection onto the image plane.
  • ${\tilde{\mathbf{p}}}_2:\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\\ 1\end{array}\right]\to \frac{1}{Z^{\prime }}{\tilde{\mathbf{X}}}^{\prime }=\left[\begin{array}{c}{X}^{\prime }/Z^{\prime }\\ Y^{\prime }/Z^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{c}{\tilde{u}}_2\\ {\tilde{v}}_2\\ 1\end{array}\right]$
• ${\mathbf{p}}_2={\pi }_k(\cdot )$
  • Projection onto the image plane after correcting for lens distortion. If distortion correction has already been performed at the input stage, then ${\pi }_k(\cdot )=\tilde{\mathbf{K}}(\cdot )$.
  • ${\mathbf{p}}_2=\tilde{\mathbf{K}}{\tilde{\mathbf{p}}}_2=\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]\left[\begin{array}{c}{\tilde{u}}_2\\ {\tilde{v}}_2\\ 1\end{array}\right]=\left[\begin{array}{c}f\tilde{u}+c_x\\ f\tilde{v}+c_y\end{array}\right]=\left[\begin{array}{c}{u}_2\\ v_2\end{array}\right]$
• $\mathbf{K}=\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\\ 0& 0& 1\end{array}\right]$: Camera intrinsic matrix
• $\tilde{\mathbf{K}}=\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]$: I omitted the last line of internal parameters to project to ${\mathbb{P}}^2\to {\mathbb{R}}^2$
• $\mathcal{P}$:  The set of all feature points in the image
• $\mathbf{e}(\mathbf{T})\to \mathbf{e}$: In general, it is sometimes indicated simply by omitting the notation.
• ${\mathbf{p}}_1^i,{\mathbf{p}}_2^i$: Pixel coordinates of the ith feature point in the first image and the second image
• $\oplus$ : operator for composition of two SE(3) group
• $\mathbf{J}=\frac{\partial \mathbf{e}}{\partial \mathbf{T}}=\frac{\partial \mathbf{e}}{\partial [\mathbf{R},\mathbf{t}]}$
• ${\mathbf{X}}^{\prime }=[X,Y,Z,1{]}^{⊺}=[\widetilde{{\mathbf{X}}^{\prime }},1{]}^{⊺}=\mathbf{TX}$
• $\mathbf{T}\mathbf{X}$: Transform 3D point $\mathbf{X}$ to camera coordinate system, $(\mathbf{T}\mathbf{X}=\left[\begin{array}{c}\mathbf{R}\mathbf{X}+\mathbf{t}\\ 1\end{array}\right]\in {\mathbb{R}}^{4\times 1})$
• ${\mathbf{X}}^{\prime }=[X^{\prime },Y^{\prime },Z^{\prime },1{]}^{⊺}=[{\tilde{\mathbf{X}}}^{\prime },1{]}^{⊺}$
• $\xi =[\mathbf{w},\mathbf{v}{]}^{⊺}=[w_x,w_y,w_z,v_x,v_y,v_z{]}^{⊺}$: A vector of three-dimensional angular velocity and velocity. It's called a twist.
• $[\xi {]}_{\times }=\left[\begin{array}{cc}[\mathbf{w}{]}_{\times }& \mathbf{v}\\ {\mathbf{0}}^{⊺}& 0\end{array}\right]\in \text{se}(3)$ : Twist's lie algebra with hat operator applied (4x4 matrix)
• ${\mathcal{J}}_l$: Jacobian for left multiplication. Since it is not used in actual calculations, it will not be introduced in detail.

 

In the figure above, the world coordinates of the 3D point $\mathbf{X}$ are $[X,Y,Z,1{]}^{⊺}\in {\mathbb{P}}^3$ and the corresponding two cameras The pixel coordinates on the image plane are ${\mathbf{p}}_1,{\mathbf{p}}_2\in {\mathbb{P}}^2$.  At this time, it is assumed that the internal parameters $\mathbf{K}$ of the two cameras $\{C_1\}$ and $\{C_2\}$ are the same. When camera $\{C_1\}$ is the origin($\mathbf{R}=\mathbf{I},\mathbf{t}=\mathbf{0}$) , if pixel coordinates ${\mathbf{p}}_1,{\mathbf{p}}_2$ are expressed through the 3D point $\mathbf{X}$, they are projected in the following order.

$$\begin{array}{}\text{(65)}& \begin{array}{r}\mathbf{p}=\pi (\mathbf{T},\mathbf{X})\end{array}\end{array}$$ 

$$\begin{array}{}\text{(66)}& \begin{array}{rl}{\mathbf{p}}_1=\left(\begin{array}{c}{u}_1\\ v_1\end{array}\right)& =\pi (\mathbf{I},\mathbf{X})={\pi }_k({\pi }_h(\mathbf{X}))\\ {\mathbf{p}}_2=\left(\begin{array}{c}{u}_2\\ v_2\end{array}\right)& =\pi (\mathbf{T},\mathbf{X})={\pi }_k({\pi }_h(\mathbf{TX}))\end{array}\end{array}$$ 

 

One of the characteristics of direct method is that unlike feature-based, there is no way to know which ${\mathbf{p}}_2$ matches ${\mathbf{p}}_1$.  So based on the current pose estimate, we find the position of ${\mathbf{p}}_2$. That is, ${\mathbf{p}}_2$ and ${\mathbf{p}}_1$ are made similar by optimizing the pose of the camera. At this time, the problem is solved by minimizing the photometric error.  The photometric error is:

$$\begin{array}{}\text{(67)}& \boxed{{\displaystyle \begin{array}{rl}\mathbf{e}(\mathbf{T})& ={\mathbf{I}}_1({\mathbf{p}}_1)-{\mathbf{I}}_2({\mathbf{p}}_2)\\ & ={\mathbf{I}}_1({\pi }_k({\pi }_h(\mathbf{X})))-{\mathbf{I}}_2({\pi }_k({\pi }_h(\mathbf{TX})))\end{array}}}\end{array}$$

 

The photometric error is based on the assumption of grayscale invariance and is scalar.  To solve non-linear least squares with photometric error, we can define the following error function $\mathbf{E}(\mathbf{T})$.

$$\begin{array}{}\text{(68)}& \begin{array}{rl}\mathbf{E}(\mathbf{T})& =\arg \underset{{\mathbf{T}}^{\ast }}{min}\sum _{i\in \mathcal{P}}{\Vert {\mathbf{e}}_i\Vert }^2\\ & =\arg \underset{{\mathbf{T}}^{\ast }}{min}\sum _{i\in \mathcal{P}}{\mathbf{e}}_i^{⊺}{\mathbf{e}}_i\\ & =\arg \underset{{\mathbf{T}}^{\ast }}{min}\sum _{i\in \mathcal{P}}({\mathbf{I}}_1({\mathbf{p}}_1^i)-{\mathbf{I}}_2({\mathbf{p}}_2^i){)}^{⊺}({\mathbf{I}}_1({\mathbf{p}}_1^i)-{\mathbf{I}}_2({\mathbf{p}}_2^i))\end{array}\end{array}$$

 

${\Vert \mathbf{e}({\mathbf{T}}^{\ast })\Vert }^2$ that satisfies $\mathbf{E}({\mathbf{T}}^{\ast })$ can be computed iteratively through non-linear least squares.  The optimal state is found by iteratively updating $\mathbf{T}$ in small increments $\mathrm{\Delta }\mathbf{T}$. 
$$\begin{array}{}\text{(69)}& \begin{array}{rl}\mathbf{E}(\mathbf{T}+\mathrm{\Delta }\mathbf{T})& =\arg \underset{{\mathbf{T}}^{\ast }}{min}\sum _{i\in \mathcal{P}}{\Vert {\mathbf{e}}_i(\mathbf{T}+\mathrm{\Delta }\mathbf{T})\Vert }^2\end{array}\end{array}$$

 

Strictly speaking, since the state increment $\mathrm{\Delta }\mathbf{T}$ is an SE(3) conversion matrix, it is correct to add it to the existing state $\mathbf{T}$ through the $\oplus$ operator, but for convenience of expression, The $+$ operator is used.

$$\begin{array}{}\text{(70)}& \begin{array}{r}\mathbf{T}\oplus \mathrm{\Delta }\mathbf{T}\to \mathbf{T}+\mathrm{\Delta }\mathbf{T}\end{array}\end{array}$$

This can be expressed as follows through the first-order Taylor approximation.
$$\begin{array}{}\text{(71)}& \begin{array}{rl}\mathbf{e}(\mathbf{T}+\mathrm{\Delta }\mathbf{T})& \approx {\mathbf{e}}_i(\mathbf{T})+\mathbf{J}\mathrm{\Delta }\mathbf{T}\\ & ={\mathbf{e}}_i(\mathbf{T})+\frac{\partial \mathbf{e}}{\partial \mathbf{T}}\mathrm{\Delta }\mathbf{T}\end{array}\end{array}$$

 

$$\begin{array}{}\text{(72)}& \begin{array}{rl}\mathbf{E}(\mathbf{T}+\mathrm{\Delta }\mathbf{T})& =\arg \underset{{\mathbf{T}}^{\ast }}{min}\sum _{i\in \mathcal{P}}{\Vert {\mathbf{e}}_i(\mathbf{T})+\mathbf{J}\mathrm{\Delta }\mathbf{T}\Vert }^2\end{array}\end{array}$$

 

By differentiating this, the optimal value of increment $\mathrm{\Delta }{\mathbf{T}}^{\ast }$ is obtained as follows.  The detailed derivation process is omitted in this section.  If you want to know more about the induction process, you can refer to the previous section.
$$\begin{array}{}\text{(73)}& \begin{array}{rl}& {\mathbf{J}}^{⊺}\mathbf{J}\mathrm{\Delta }{\mathbf{T}}^{\ast }=-{\mathbf{J}}^{⊺}\mathbf{e}\\ & \mathbf{H}\mathrm{\Delta }{\mathbf{T}}^{\ast }=-\mathbf{b}\end{array}\end{array}$$

Since the above equation is in the form of a linear system $\mathbf{Ax}=\mathbf{b}$, various linear algebra techniques such as schur complement and cholesky decomposition can be used to find $\mathrm{\Delta }{\mathbf{T}}^{\ast }$ .  The optimal increment obtained in this way is added to the current state. At this time, whether to update the pose seen in the local coordinate system (right) or the pose seen in the global coordinate system (left) depends on whether the existing $\mathbf{T}$ is multiplied to the right or to the left. will lose  Photometric errors update the transformation matrix in the global coordinate system, so the left multiplication method is generally used.
$$\begin{array}{}\text{(74)}& \begin{array}{r}\mathbf{T}\leftarrow \mathbf{T}\oplus \mathrm{\Delta }{\mathbf{T}}^{\ast }\end{array}\end{array}$$

 

The definition of the left multiplication $\oplus$ operation is as follows.

$$\begin{array}{}\text{(75)}& \begin{array}{rl}\mathbf{T}\oplus \mathrm{\Delta }{\mathbf{T}}^{\ast }& =\mathrm{\Delta }{\mathbf{T}}^{\ast }\mathbf{T}\\ & =\exp ([\mathrm{\Delta }{\xi }^{\ast }{]}_{\times })\mathbf{T}\cdots \text{ globally updated (left mult)}\end{array}\end{array}$$

 

4.1. Jacobian of the photometric error

To perform ($\text{73}$), we need to find the Jacobian $\mathbf{J}$ for the photometric error.  This can be expressed as:

$$\begin{array}{}\text{(76)}& \begin{array}{rl}\mathbf{J}& =\frac{\partial \mathbf{e}}{\partial \mathbf{T}}\\ & =\frac{\partial \mathbf{e}}{\partial [\mathbf{R},\mathbf{t}]}\end{array}\end{array}$$

 

Here's a closer look at this:

$$\begin{array}{}\text{(77)}& \begin{array}{rl}\mathbf{J}=\frac{\partial \mathbf{e}}{\partial \mathbf{T}}& =\frac{\partial }{\partial \mathbf{T}}({\mathbf{I}}_1({\mathbf{p}}_1)-{\mathbf{I}}_2({\mathbf{p}}_2))\\ & =\frac{\partial }{\partial \mathbf{T}}({\mathbf{I}}_1({\pi }_k({\pi }_h(\mathbf{X})))-{\mathbf{I}}_2({\pi }_k({\pi }_h(\mathbf{TX}))))\\ & =\frac{\partial }{\partial \mathbf{T}}(-{\mathbf{I}}_2({\pi }_k({\pi }_h(\mathbf{TX}))))\\ & =\frac{\partial }{\partial \mathbf{T}}(-{\mathbf{I}}_2({\pi }_k({\pi }_h({\mathbf{X}}^{\prime }))))\end{array}\end{array}$$

 

Re-expressing the above expression by applying the chain rule gives:

$$\begin{array}{}\text{(78)}& \begin{array}{rl}\frac{\partial \mathbf{e}}{\partial \xi }& =\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}}^{\prime }}\frac{\partial {\mathbf{X}}^{\prime }}{\partial \xi }\\ & ={\mathbb{R}}^{1\times 2}\cdot {\mathbb{R}}^{2\times 3}\cdot {\mathbb{R}}^{3\times 4}\cdot {\mathbb{R}}^{4\times 6}={\mathbb{R}}^{1\times 6}\end{array}\end{array}$$

 

 

At this time, the Jacobian $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{T}}$ for the transformation matrix $\mathbf{T}$ is not obtained, but for the twist $\xi$ The reason for finding the Jacobian $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \xi }$ is explained in the next section.  First of all, $\frac{\partial \mathbf{I}}{\partial {\mathbf{p}}_2}$ means the gradient of the image.

$$\begin{array}{}\text{(79)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial \mathbf{I}}{\partial {\mathbf{p}}_2}& =\left[\begin{array}{cc}\frac{\partial \mathbf{I}}{\partial u}& \frac{\partial \mathbf{I}}{\partial v}\end{array}\right]\\ & =\left[\begin{array}{cc}\mathrm{\nabla }{\mathbf{I}}_u& \mathrm{\nabla }{\mathbf{I}}_v\end{array}\right]\end{array}}}\end{array}$$

 

Assuming that undistortion has already been performed during the image input process, $\frac{\partial {\mathbf{p}}_2}{\partial {\tilde{\mathbf{p}}}_2}$ is as follows.
$$\begin{array}{}\text{(80)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial {\mathbf{p}}_2}{\partial {\tilde{\mathbf{p}}}_2}& =\frac{\partial }{\partial {\tilde{\mathbf{p}}}_2}\tilde{\mathbf{K}}{\tilde{\mathbf{p}}}_2\\ & =\tilde{\mathbf{K}}\\ & =\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]\in {\mathbb{R}}^{2\times 3}\end{array}}}\end{array}$$

Next, $\frac{\partial {\tilde{\mathbf{p}}}_2}{\partial {\mathbf{X}}^{\prime }}$ is:
$$\begin{array}{}\text{(81)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial {\tilde{\mathbf{p}}}_2}{\partial {\mathbf{X}}^{\prime }}& =\frac{\partial [{\tilde{u}}_2,{\tilde{v}}_2,1]}{\partial [X^{\prime },Y^{\prime },Z^{\prime },1]}\\ & =\left[\begin{array}{cccc}\frac{1}{Z^{\prime }}& 0& \frac{-X^{\prime }}{Z^{\prime 2}}& 0\\ 0& \frac{1}{Z^{\prime }}& \frac{-Y^{\prime }}{Z^{\prime 2}}& 0\\ 0& 0& 0& 0\end{array}\right]\in {\mathbb{R}}^{3\times 4}\end{array}}}\end{array}$$

 

4.1.1. Lie theory-based SE(3) optimization

Finally $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathbf{T}}=\frac{\partial {\mathbf{X}}^{\prime }}{\partial [\mathbf{R},\mathbf{t}]}$.  At this time, since the term $\mathbf{t}$ related to the position is a 3D vector and the size of the vector is equal to 3 degrees of freedom, which is the minimum degree of freedom for expressing the 3D position, a separate constraint is required when performing the optimization update. does not exist. On the other hand, the rotation matrix $\mathbf{R}$ has 9 parameters, which is more than 3 degrees of freedom, which is the minimum degree of freedom for expressing 3D rotation, so various constraints exist.  This is said to be over-parameterized.  Disadvantages of over-parameterized representation are as follows.

• Since redundant parameters must be calculated, the amount of computation increases during optimization.
• The additional degrees of freedom can lead to numerical instability problems.
• Whenever a parameter is updated, it must always be checked whether the constraint is satisfied.

 

Therefore, a lie theory-based optimization method, which is a minimal parameter expression free from constraints, is generally used.  Instead of calculating $\mathrm{\Delta }{\mathbf{T}}^{\ast }$, which includes a nonlinear rotation matrix, the lie group SE(3)-based optimization method includes $\mathbf{R}\to \mathbf{w}$, and position-related terms are changed to $\mathbf{t}\to \mathbf{v}$ to find the optimal lie algebra se(3) $\mathrm{\Delta }{\xi }^{\ast }$, and then exponential mapping Indicates how to update to SE(3).

$$\begin{array}{}\text{(82)}& \begin{array}{rl}\mathrm{\Delta }{\mathbf{T}}^{\ast }& \to \mathrm{\Delta }{\xi }^{\ast }\end{array}\end{array}$$

 

The Jacobian for $\xi$ is

$$\begin{array}{}\text{(83)}& \begin{array}{rlrl}\mathbf{J}& =\frac{\partial \mathbf{e}}{\partial [\mathbf{R},\mathbf{t}]}& & \to \frac{\partial \mathbf{e}}{\partial [\mathbf{w},\mathbf{v}]}\\ & & & \to \frac{\partial \mathbf{e}}{\partial \xi }\end{array}\end{array}$$

 

Through this, the existing expression is changed as follows.

$$\begin{array}{}\text{(84)}& \begin{array}{rl}\mathbf{e}(\mathbf{T})& \to \mathbf{e}(\xi )\\ \mathbf{E}(\mathbf{T})& \to \mathbf{E}(\xi )\\ \mathbf{e}(\mathbf{T})+{\mathbf{J}}^{\prime }\mathrm{\Delta }\mathbf{T}& \to \mathbf{e}(\xi )+\mathbf{J}\mathrm{\Delta }\xi \\ \mathbf{H}\mathrm{\Delta }{\mathbf{T}}^{\ast }=-\mathbf{b}& \to \mathbf{H}\mathrm{\Delta }{\xi }^{\ast }=-\mathbf{b}\\ \mathbf{T}\leftarrow \mathrm{\Delta }{\mathbf{T}}^{\ast }\mathbf{T}& \to \mathbf{T}\leftarrow \exp ([\mathrm{\Delta }{\xi }^{\ast }{]}_{\times })\mathbf{T}\end{array}\end{array}$$

- ${\mathbf{J}}^{\prime }=\frac{\partial \mathbf{e}}{\partial \mathbf{T}}$

- $\mathbf{J}=\frac{\partial \mathbf{e}}{\partial \xi }$

 

$\exp ([\xi {]}_{\times })\in \text{SE}(3)$ refers to an operation that transforms twist $\xi$ into a 3D pose by exponential mapping.  For more information on exponential mapping, refer to this link.

$$\begin{array}{}\text{(85)}& \begin{array}{r}\exp ([\mathrm{\Delta }\xi {]}_{\times })=\mathrm{\Delta }\mathbf{T}\end{array}\end{array}$$

 

So far the Jacobians have been easy to compute, whereas $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \xi }$ is because the parameter $\xi$ is not immediately visible in ${\mathbf{X}}^{\prime }$ ${\mathbf{X}}^{\prime }$ should be changed to terms related to lie algebra.

$$\begin{array}{}\text{(86)}& \begin{array}{r}{\mathbf{X}}^{\prime }\to \mathbf{TX}\to \exp ([\xi {]}_{\times })\mathbf{X}\end{array}\end{array}$$

 

At this time, there are two ways to update the small lie algebra increment $\mathrm{\Delta }\xi$ to the existing $\exp ([\xi {]}_{\times })$.  First of all, [1] There is an update method using basic lie algebra.  [2] Next, there is an update method using a perturbation model. 

$$\begin{array}{}\text{(87)}& \begin{array}{rl}\exp ([\xi {]}_{\times })& \leftarrow \exp ([\xi +\mathrm{\Delta }\xi {]}_{\times })\cdots \text{[1]}\\ \exp ([\xi {]}_{\times })& \leftarrow \exp ([\mathrm{\Delta }\xi {]}_{\times })\exp ([\xi {]}_{\times })\cdots \text{[2]}\end{array}\end{array}$$

 

Among the two methods above, the $[1]$ method adds the fine increment $\mathrm{\Delta }\xi$ to the existing $\xi$ and then performs exponential mapping to obtain the Jacobian. The $[2]$ method calculates the Jacobian. This method updates the existing state by multiplying the left side of $\xi$ by the perturbation model $\exp ([\mathrm{\Delta }\xi {]}_{\times })$.  The following conversion exists between the two methods, which is called the BCH approximation.  For details, see Chapter 4 of Introduction to Visual SLAM.

$$\begin{array}{}\text{(88)}& \begin{array}{rl}\exp ([\mathrm{\Delta }\xi {]}_{\times })\exp ([\xi {]}_{\times })& =\exp ([\xi +{\mathcal{J}}_l^{-1}\mathrm{\Delta }\xi {]}_{\times })\\ \exp ([\xi +\mathrm{\Delta }\xi {]}_{\times })& =\exp ([{\mathcal{J}}_l\mathrm{\Delta }\xi {]}_{\times })\exp ([\xi {]}_{\times })\end{array}\end{array}$$

 

Since using the $[1]$ method leads to very complex equations, this method is not often used, and the perturbation model of $[2]$ is mainly used.  Therefore, $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \xi }$ is transformed as follows.

$$\begin{array}{}\text{(89)}& \begin{array}{rl}\frac{\partial {\mathbf{X}}^{\prime }}{\partial \xi }& \to \frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathrm{\Delta }\xi }\end{array}\end{array}$$

 

The Jacobian for $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathrm{\Delta }\xi }$ can be calculated as

$$\begin{array}{}\text{(90)}& \boxed{{\displaystyle \begin{array}{rl}\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathrm{\Delta }\xi }& =\underset{\mathrm{\Delta }\xi \to 0}{lim}\frac{\exp ([\mathrm{\Delta }\xi {]}_{\times }){\mathbf{X}}^{\prime }-{\mathbf{X}}^{\prime }}{\mathrm{\Delta }\xi }\\ & \approx \underset{\mathrm{\Delta }\xi \to 0}{lim}\frac{(\mathbf{I}+[\mathrm{\Delta }\xi {]}_{\times }){\mathbf{X}}^{\prime }-{\mathbf{X}}^{\prime }}{\mathrm{\Delta }\xi }\\ & =\underset{\mathrm{\Delta }\xi \to 0}{lim}\frac{[\mathrm{\Delta }\xi {]}_{\times }{\mathbf{X}}^{\prime }}{\mathrm{\Delta }\xi }\\ & =\underset{\mathrm{\Delta }\xi \to 0}{lim}\frac{\left[\begin{array}{cc}[\mathrm{\Delta }\mathbf{w}{]}_{\times }& \mathrm{\Delta }\mathbf{v}\\ {\mathbf{0}}^{⊺}& 0\end{array}\right]\left[\begin{array}{c}{\tilde{\mathbf{X}}}^{\prime }\\ 1\end{array}\right]}{\mathrm{\Delta }\xi }\\ & =\underset{\mathrm{\Delta }\xi \to 0}{lim}\frac{\left[\begin{array}{c}[\mathrm{\Delta }\mathbf{w}{]}_{\times }{\tilde{\mathbf{X}}}^{\prime }+\mathrm{\Delta }\mathbf{v}\\ {\mathbf{0}}^{⊺}\end{array}\right]}{[\mathrm{\Delta }\mathbf{w},\mathrm{\Delta }\mathbf{v}{]}^{⊺}}=\left[\begin{array}{cc}-[{\tilde{\mathbf{X}}}^{\prime }{]}_{\times }& \mathbf{I}\\ {\mathbf{0}}^{⊺}& {\mathbf{0}}^{⊺}\end{array}\right]\in {\mathbb{R}}^{4\times 6}\end{array}}}\end{array}$$

 

Therefore, when using the $[2]$ perturbation model, there is an advantage in that the Jacobian can be obtained simply by using the skew-symmetric matrix of the points ${\mathbf{X}}^{\prime }$ in the 3D space.  In the case of photometric error optimization, since most of the errors for the brightness change of sequentially incoming images are optimized, the camera pose change is not large and the size of $\mathrm{\Delta }\xi$ is not large, so the above Jacobian is generally used. Since we use the $[2]$ perturbation model, the small increment $\mathrm{\Delta }{\xi }^{\ast }$ is updated as ($\text{75}$).

$$\begin{array}{}\text{(91)}& \begin{array}{r}\mathbf{T}\leftarrow \mathrm{\Delta }{\mathbf{T}}^{\ast }\mathbf{T}=\exp ([\mathrm{\Delta }{\xi }^{\ast }{]}_{\times })\mathbf{T}\end{array}\end{array}$$

 

In the above equation, the second row is the first-order Taylor approximation applied to the small twist increment $\exp ([\mathrm{\Delta }\xi {]}_{\times })$.  To understand the approximation of the second row, the transformation matrix $\mathbf{T}$ is exponential given an arbitrary twist $\xi =[\mathbf{w},\mathbf{v}{]}^{⊺}$ When developed in mapping form, it can be expressed as follows.

$$\begin{array}{}\text{(92)}& \begin{array}{rl}\mathbf{T}=\exp ([\xi {]}_{\times })& =\mathbf{I}+\left[\begin{array}{cc}[\mathbf{w}{]}_{\times }& \mathbf{v}\\ {\mathbf{0}}^{⊺}& 0\end{array}\right]+\frac{1}{2!}\left[\begin{array}{cc}[\mathbf{w}{]}_{\times }^2& [\mathbf{w}{]}_{\times }\mathbf{v}\\ {\mathbf{0}}^{⊺}& 0\end{array}\right]+\frac{1}{3!}\left[\begin{array}{cc}[\mathbf{w}{]}_{\times }^3& [\mathbf{w}{]}_{\times }^2\mathbf{v}\\ {\mathbf{0}}^{⊺}& 0\end{array}\right]+\cdots \\ & =\mathbf{I}+[\xi {]}_{\times }+\frac{1}{2!}[\xi {]}_{\times }^2+\frac{1}{3!}[\xi {]}_{\times }^3+\cdots \end{array}\end{array}$$

For a small twist increment $\mathrm{\Delta }\xi$, it can be approximated as follows by ignoring higher-order terms of the second or higher order.
$$\begin{array}{}\text{(93)}& \begin{array}{r}\exp ([\mathrm{\Delta }\xi {]}_{\times })\approx \mathbf{I}+[\mathrm{\Delta }\xi {]}_{\times }\end{array}\end{array}$$

 

Finally, The Jacobian $\mathbf{J}$ for the pose is:

$$\begin{array}{}\text{(94)}& \boxed{{\displaystyle \begin{array}{rl}\mathbf{J}=\frac{\partial \mathbf{e}}{\partial \mathrm{\Delta }\xi }& =\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}}^{\prime }}\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathrm{\Delta }\xi }\\ & =\left[\begin{array}{cc}\mathrm{\nabla }{\mathbf{I}}_u& \mathrm{\nabla }{\mathbf{I}}_v\end{array}\right]\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\end{array}\right]\left[\begin{array}{cccc}\frac{1}{Z^{\prime }}& 0& \frac{-X^{\prime }}{Z^{\prime 2}}& 0\\ 0& \frac{1}{Z^{\prime }}& \frac{-Y^{\prime }}{Z^{\prime 2}}& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{cc}-[{\tilde{\mathbf{X}}}^{\prime }{]}_{\times }& \mathbf{I}\\ {\mathbf{0}}^{⊺}& {\mathbf{0}}^{⊺}\end{array}\right]\\ & =\left[\begin{array}{cc}\mathrm{\nabla }{\mathbf{I}}_u& \mathrm{\nabla }{\mathbf{I}}_v\end{array}\right]\left[\begin{array}{cccc}\frac{f}{Z^{\prime }}& 0& -\frac{fX}{Z^{\prime 2}}& 0\\ 0& \frac{f}{Z^{\prime }}& -\frac{fY}{Z^{\prime 2}}& 0\end{array}\right]\left[\begin{array}{cccccc}0& Z^{\prime }& -Y^{\prime }& 1& 0& 0\\ -Z^{\prime }& 0& X^{\prime }& 0& 1& 0\\ Y^{\prime }& -X^{\prime }& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}\mathrm{\nabla }{\mathbf{I}}_u& \mathrm{\nabla }{\mathbf{I}}_v\end{array}\right]\left[\begin{array}{cccccc}-\frac{f{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f(1+X^{\prime 2})}{Z^{\prime 2}}& -\frac{f{Y}^{\prime }}{Z^{\prime }}& \frac{f}{Z^{\prime }}& 0& -\frac{f{X}^{\prime }}{Z^{\prime 2}}\\ -\frac{f(1+Y^{\prime 2})}{Z^{\prime 2}}& \frac{f{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f{X}^{\prime }}{Z^{\prime }}& 0& \frac{f}{Z^{\prime }}& -\frac{f{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\in {\mathbb{R}}^{1\times 6}\end{array}}}\end{array}$$

At this time, since the last line of $\frac{\partial {\mathbf{X}}^{\prime }}{\partial \mathrm{\Delta }\xi }$ is always 0, it is sometimes omitted and calculated.

 

4.1.2. Code implementations

• Code of Chapter 8 of Introduction to Visual SLAM: direct_sparse.cpp#L111
• DSO Code: CoarseInitializer.cpp#L430
• DSO Code2: CoarseTracker.cpp#L320

 

5. Relative pose error (PGO)

Relative pose error is an error mainly used in pose graph optimization (PGO).  For more information on PGO, refer to [SLAM] Pose Graph Optimization 개념 설명 및 예제 코드 분석 post.

 

NOMENCLATURE of relative pose error

• $\text{(Node) }{\mathbf{x}}_i=\left[\begin{array}{cc}{\mathbf{R}}_i& {\mathbf{t}}_i\\ {\mathbf{0}}^{⊺}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}$
• $\text{(Edge) }{\mathbf{z}}_{ij}=\left[\begin{array}{cc}{\mathbf{R}}_{ij}& {\mathbf{t}}_{ij}\\ {\mathbf{0}}^{⊺}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}$
• ${\hat{\mathbf{z}}}_{ij}={\mathbf{x}}_i^{-1}{\mathbf{x}}_j$ : predicted value
• ${\mathbf{z}}_{ij}$ : virtual measurement
• $\mathbf{x}=[{\mathbf{x}}_1,\cdots ,{\mathbf{x}}_n]$: All pose nodes in pose graph
• ${\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)\leftrightarrow {\mathbf{e}}_{ij}$:For convenience of expression, it is sometimes omitted.
• $\mathbf{J}=\frac{\partial \mathbf{e}}{\partial \mathbf{x}}$
• $\oplus$ : operator for composition of two SE(3) families
• $\text{Log}(\cdot )$: Operator that converts SE(3) to twist $\xi \in {\mathbb{R}}^6$.  For more information on logarithm mapping, refer to this post.

 

Given two nodes ${\mathbf{x}}_{i}and{\mathbf{x}}_j$ on the Pose graph, the relative pose (=observed value) ${\mathbf{z}}_{ij}$ newly calculated by the sensor data and the existing known The difference in relative pose (=predicted value) ${\hat{\mathbf{z}}}_{ij}$ is defined as the relative pose error. (figure: Freiburg univ. Robot Mapping Course).

$$\begin{array}{}\text{(95)}& \begin{array}{r}{\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)={\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij}={\mathbf{z}}_{ij}^{-1}{\mathbf{x}}_i^{-1}{\mathbf{x}}_j\end{array}\end{array}$$

 

The process of optimizing the relative pose error is called pose graph optimization (PGO) and is also called the back-end algorithm of graph-based SLAM. Nodes ${\mathbf{x}}_i,{\mathbf{x}}_{i+1},and\cdots$, which are sequentially calculated by front-end visual odometry (VO) or lidar odometry (LO), are observed Observed when an edge is connected between two non-sequential nodes ${\mathbf{x}}_i,{\mathbf{x}}_j$ because PGO is not performed because the values ​​and predicted values ​ are identical, but loop closing occurs PGO is performed because the difference between the value and the predicted value occurs.

 

That is, PGO is generally performed when a special situation such as loop closing occurs. When the robot revisits the same place while moving, the loop detection algorithm operates to determine the loop.  At this time, if a loop is detected, the existing node ${\mathbf{x}}_i$ and the node ${\mathbf{x}}_j$ created by revisiting are connected as loop edges, and various matching algorithms (GICP, NDT , etc...) to create observations. These observations are called virtual measurements because they are virtual observations created by a matching algorithm, not actual observations.

 

The relative pose error for all nodes on the pose graph can be defined as follows.

$$\begin{array}{}\text{(96)}& \begin{array}{rl}\mathbf{E}(\mathbf{x})& =\arg \underset{{\mathbf{x}}^{\ast }}{min}\sum _i\sum _j{\Vert {\mathbf{e}}_{ij}\Vert }^2\\ & =\arg \underset{{\mathbf{x}}^{\ast }}{min}\sum _i\sum _j{\mathbf{e}}_{ij}^{⊺}{\mathbf{e}}_{ij}\end{array}\end{array}$$

 

${\Vert \mathbf{e}({\mathbf{x}}^{\ast })\Vert }^2$ that satisfies $\mathbf{E}({\mathbf{x}}^{\ast })$ can be computed iteratively through non-linear least squares.  The optimal state is found by iteratively updating $\mathbf{x}$ in small increments $\mathrm{\Delta }\mathbf{x}$. 
$$\begin{array}{}\text{(97)}& \begin{array}{rl}\mathbf{E}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})& =\arg \underset{{\mathbf{x}}^{\ast }}{min}\sum _i\sum _j{\Vert {\mathbf{e}}_{ij}({\mathbf{x}}_i+\mathrm{\Delta }{\mathbf{x}}_i,{\mathbf{x}}_j+\mathrm{\Delta }{\mathbf{x}}_j)\Vert }^2\end{array}\end{array}$$

Strictly speaking, since the state increment $\mathrm{\Delta }\mathbf{x}$ is an SE(3) transformation matrix, it is correct to add it to the existing state $\mathbf{x}$ through the $\oplus$ operator, but for convenience of expression $+$ operator is used.

$$\begin{array}{}\text{(98)}& \begin{array}{r}{\mathbf{e}}_{ij}({\mathbf{x}}_i\oplus \mathrm{\Delta }{\mathbf{x}}_i,{\mathbf{x}}_j\oplus \mathrm{\Delta }{\mathbf{x}}_j)\to {\mathbf{e}}_{ij}({\mathbf{x}}_i+\mathrm{\Delta }{\mathbf{x}}_i,{\mathbf{x}}_j+\mathrm{\Delta }{\mathbf{x}}_j)\end{array}\end{array}$$

 

The above equation can be expressed as follows through Taylor's first-order approximation.
$$\begin{array}{}\text{(99)}& \begin{array}{rl}{\mathbf{e}}_{ij}({\mathbf{x}}_i+\mathrm{\Delta }{\mathbf{x}}_i,{\mathbf{x}}_j+\mathrm{\Delta }{\mathbf{x}}_j)& \approx {\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)+{\mathbf{J}}_{ij}\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{x}}_i\\ \mathrm{\Delta }{\mathbf{x}}_j\end{array}\right]\\ & ={\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)+{\mathbf{J}}_i\mathrm{\Delta }{\mathbf{x}}_i+{\mathbf{J}}_j\mathrm{\Delta }{\mathbf{x}}_j\\ & ={\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)+\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{x}}_i}\mathrm{\Delta }{\mathbf{x}}_i++\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{x}}_j}\mathrm{\Delta }{\mathbf{x}}_j\end{array}\end{array}$$

 

$$\begin{array}{}\text{(100)}& \begin{array}{rl}\mathbf{E}(\mathbf{x}+\mathrm{\Delta }\mathbf{x})& \approx \arg \underset{{\mathbf{x}}^{\ast }}{min}\sum _i\sum _j{\Vert {\mathbf{e}}_{ij}({\mathbf{x}}_i+\mathrm{\Delta }{\mathbf{x}}_i,{\mathbf{x}}_j+\mathrm{\Delta }{\mathbf{x}}_j)+{\mathbf{J}}_{ij}\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{x}}_i\\ \mathrm{\Delta }{\mathbf{x}}_j\end{array}\right]\Vert }^2\end{array}\end{array}$$

 

By differentiating this, the optimal value of increment $\mathrm{\Delta }{\mathbf{x}}^{\ast }$ for all nodes is obtained as follows.  The detailed derivation process is omitted in this section.  If you want to know more about the induction process, you can refer to the previous section.
$$\begin{array}{}\text{(101)}& \begin{array}{rl}& {\mathbf{J}}^{⊺}\mathbf{J}\mathrm{\Delta }{\mathbf{x}}^{\ast }=-{\mathbf{J}}^{⊺}\mathbf{e}\\ & \mathbf{H}\mathrm{\Delta }{\mathbf{x}}^{\ast }=-\mathbf{b}\end{array}\end{array}$$

 

Since the above equation is in the form of a linear system $\mathbf{Ax}=\mathbf{b}$, various linear algebra techniques such as schur complement and cholesky decomposition can be used to find $\mathrm{\Delta }{\mathbf{x}}^{\ast }$ .  The optimal increment obtained in this way is added to the current state. At this time, whether to update the pose seen in the local coordinate system (right) or the pose seen in the global coordinate system (left) depends on whether the existing $\mathbf{x}$ is multiplied to the right or to the left. will lose  Since the relative pose error is related to the relative pose of the two nodes, a right multiplication that updates in the local coordinate system is applied.
$$\begin{array}{}\text{(102)}& \begin{array}{r}\mathbf{x}\leftarrow \mathbf{x}\oplus \mathrm{\Delta }{\mathbf{x}}^{\ast }\end{array}\end{array}$$

 

The definition of the right multiplication $\oplus$ operation is as follows.

$$\begin{array}{}\text{(103)}& \begin{array}{rl}\mathbf{x}\oplus \mathrm{\Delta }{\mathbf{x}}^{\ast }& =\mathbf{x}\mathrm{\Delta }{\mathbf{x}}^{\ast }\\ & =\mathbf{x}\exp ([\mathrm{\Delta }{\xi }^{\ast }{]}_{\times })\cdots \text{ locally updated (right mult)}\end{array}\end{array}$$

 

 

5.1. Jacobian of relative pose error

To perform ($\text{101}$), the Jacobian $\mathbf{J}$ for the relative pose error must be obtained.  Given two non-sequential nodes ${\mathbf{x}}_i,{\mathbf{x}}_j$, the Jacobian ${\mathbf{J}}_{ij}$ can be expressed as there is.

$$\begin{array}{}\text{(104)}& \begin{array}{rl}{\mathbf{J}}_{ij}& =\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{x}}_{ij}}\\ & =\frac{\partial {\mathbf{e}}_{ij}}{\partial [{\mathbf{x}}_i,{\mathbf{x}}_j]}\\ & =[{\mathbf{J}}_i,{\mathbf{J}}_j]\end{array}\end{array}$$

 

Here's a closer look at this:

$$\begin{array}{}\text{(105)}& \begin{array}{rl}{\mathbf{J}}_{ij}=\frac{\partial {\mathbf{e}}_{ij}}{\partial [{\mathbf{x}}_i,{\mathbf{x}}_j]}& =\frac{\partial }{\partial [{\mathbf{x}}_i,{\mathbf{x}}_j]}({\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij})\\ & =\frac{\partial }{\partial [{\mathbf{R}}_i,{\mathbf{t}}_i,{\mathbf{R}}_j,{\mathbf{t}}_j]}({\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij})\end{array}\end{array}$$

 

5.1.1. Lie theory-based SE(3) optimization

When obtaining the above Jacobian, the position-related term $\mathbf{t}$ is a 3-dimensional vector and the magnitude of the vector is equal to 3 degrees of freedom, which is the minimum degree of freedom for expressing a 3-dimensional position. constraints do not exist. On the other hand, the rotation matrix $\mathbf{R}$ has 9 parameters, which is more than 3 degrees of freedom, which is the minimum degree of freedom for expressing 3D rotation, so various constraints exist.  This is said to be over-parameterized.  Disadvantages of over-parameterized representation are as follows.

• Since redundant parameters must be calculated, the amount of computation increases during optimization.
• The additional degrees of freedom can lead to numerical instability problems.
• Whenever a parameter is updated, it must always be checked whether the constraint is satisfied.

 

Therefore, a lie theory-based optimization method, which is a minimal parameter expression free from constraints, is generally used. Instead of calculating $\mathrm{\Delta }{\mathbf{T}}^{\ast }$, which includes a nonlinear rotation matrix, the lie group SE(3)-based optimization method includes $\mathbf{R}\to \mathbf{w}$, and position-related terms are changed to $\mathbf{t}\to \mathbf{v}$ to find the optimal lie algebra se(3) $\mathrm{\Delta }{\xi }^{\ast }$, and then exponential mapping Indicates how to update to SE(3).

$$\begin{array}{}\text{(106)}& \begin{array}{r}\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{x}}_i^{\ast },\mathrm{\Delta }{\mathbf{x}}_j^{\ast }\end{array}\right]\to [\mathrm{\Delta }{\xi }_i^{\ast },\mathrm{\Delta }{\xi }_j^{\ast }]\end{array}\end{array}$$

 

The Jacobian for $\xi$ is

$$\begin{array}{}\text{(107)}& \begin{array}{rlrl}{\mathbf{J}}_{ij}& =\frac{\partial {\mathbf{e}}_{ij}}{\partial [{\mathbf{x}}_i,{\mathbf{x}}_j]}& & \to \frac{\partial {\mathbf{e}}_{ij}}{\partial [{\xi }_i,{\xi }_j]}\end{array}\end{array}$$

 

Through this, the existing expression is changed as follows.

$$\begin{array}{}\text{(108)}& \begin{array}{rl}{\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)& \to {\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)\\ \mathbf{E}(\mathbf{x})& \to \mathbf{E}(\xi )\\ {\mathbf{e}}_{ij}({\mathbf{x}}_i,{\mathbf{x}}_j)+{\mathbf{J}}_i^{\prime }\mathrm{\Delta }{\mathbf{x}}_i+{\mathbf{J}}_j^{\prime }\mathrm{\Delta }{\mathbf{x}}_j& \to {\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)+{\mathbf{J}}_i\mathrm{\Delta }{\xi }_i+{\mathbf{J}}_j\mathrm{\Delta }{\xi }_j\\ \mathbf{H}\mathrm{\Delta }{\mathbf{x}}^{\ast }=-\mathbf{b}& \to \mathbf{H}\mathrm{\Delta }{\xi }^{\ast }=-\mathbf{b}\\ \mathbf{x}\leftarrow \mathrm{\Delta }{\mathbf{x}}^{\ast }\mathbf{x}& \to \mathbf{x}\leftarrow \exp ([\mathrm{\Delta }{\xi }^{\ast }{]}_{\times })\mathbf{x}\end{array}\end{array}$$

- ${\mathbf{J}}_{ij}^{\prime }=\frac{\partial \mathbf{e}}{\partial [{\mathbf{x}}_i,{\mathbf{x}}_j]}$

- ${\mathbf{J}}_{ij}=\frac{\partial \mathbf{e}}{\partial [{\xi }_i,{\xi }_j]}$

 

$\exp ([\xi {]}_{\times })\in \text{SE}(3)$ refers to an operation that transforms twist $\xi$ into a 3D pose by exponential mapping.  For more information on exponential mapping, refer to this link.

$$\begin{array}{}\text{(109)}& \begin{array}{r}\exp ([\mathrm{\Delta }\xi {]}_{\times })=\mathrm{\Delta }\mathbf{x}\end{array}\end{array}$$

 

$\frac{\partial }{\partial \xi }({\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij})$ is ${\mathbf{z}}_{ij}^{-1}Sinceitisnotimmediatelyvisiblein{\hat{\mathbf{z}}}_{ij}$, you need to change it to a term related to lie algebra.

$$\begin{array}{}\text{(110)}& \begin{array}{rl}{\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij}& \to \text{Log}({\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij})\end{array}\end{array}$$

 

At this time, $\text{Log}(\cdot )$ means logarithm mapping that changes from SE(3) to twist $\xi \in {\mathbb{R}}^6$.  For more information on logarithm mapping, refer to this post .  Therefore, the SE(3) version relative pose error ${\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)$ is changed as follows.

$$\begin{array}{}\text{(111)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)& =\text{Log}({\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij})\end{array}}}\end{array}$$

 

Here's how to break it down in detail:

$$\begin{array}{}\text{(112)}& \begin{array}{rl}{\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)& =\text{Log}({\mathbf{z}}_{ij}^{-1}{\hat{\mathbf{z}}}_{ij})\\ & =\text{Log}({\mathbf{z}}_{ij}^{-1}{\mathbf{x}}_i^{-1}{\mathbf{x}}_j)\\ & =\text{Log}(\exp ([-{\xi }_{ij}{]}_{\times })\exp ([-{\xi }_i{]}_{\times })\exp ([{\xi }_j{]}_{\times }))\end{array}\end{array}$$

 

Looking at the above expression, you can see that ${\xi }_i$ and ${\xi }_j$ parameters in ${\mathbf{z}}_{ij}$ are connected by exponential mapping.  Applying the left perturbation model to the formula in the second line of the above equation to express the incremental amount is as follows.

$$\begin{array}{}\text{(113)}& \begin{array}{rl}{\mathbf{e}}_{ij}({\xi }_i+\mathrm{\Delta }{\xi }_i,{\xi }_j+\mathrm{\Delta }{\xi }_j)& =\text{Log}({\hat{\mathbf{z}}}_{ij}^{-1}{\mathbf{x}}_i^{-1}\exp (-[\mathrm{\Delta }{\xi }_i{]}_{\times })\exp ([\mathrm{\Delta }{\xi }_j{]}_{\times }){\mathbf{x}}_j)\end{array}\end{array}$$

 

In the above equation, the terms are arranged in the form of $\mathbf{e}+\mathbf{J}\mathrm{\Delta }\xi$ by moving the increment term to the left or right.  To do this, we need to use the following property of the adjoint matrix of SE(3).  For more information on Adjoint martix, refer to the this post.

$$\begin{array}{}\text{(114)}& \begin{array}{r}\exp ([{\text{Ad}}_{\mathbf{T}}\cdot \xi {]}_{\times })=\mathbf{T}\cdot \exp ([\xi {]}_{\times })\cdot {\mathbf{T}}^{-1}\end{array}\end{array}$$

 

The expression above is transformed into an expression for $\mathbf{T}\to {\mathbf{T}}^{-1}$ as follows.

$$\begin{array}{}\text{(115)}& \begin{array}{r}\exp ([{\text{Ad}}_{{\mathbf{T}}^{-1}}\cdot \xi {]}_{\times })={\mathbf{T}}^{-1}\cdot \exp ([\xi {]}_{\times })\cdot \mathbf{T}\end{array}\end{array}$$

 

And rearranging, we get the following formula:

$$\begin{array}{}\text{(116)}& \begin{array}{r}\exp ([\xi {]}_{\times })\cdot \mathbf{T}=\mathbf{T}\exp ([{\text{Ad}}_{{\mathbf{T}}^{-1}}\cdot \xi {]}_{\times })\end{array}\end{array}$$

 

($\text{116}$) moves the $\exp (\cdot )\exp (\cdot )$ term in the middle of ($\text{113}$) to the right or left.  This post explains the process of moving to the right.  If this is expanded by $\mathrm{\Delta }{\xi }_i$ and $\mathrm{\Delta }{\xi }_j$ respectively, it is as follows.

$$\begin{array}{}\text{(117)}& \begin{array}{rl}{\mathbf{e}}_{ij}({\xi }_i+\mathrm{\Delta }{\xi }_i,{\xi }_j)& =\text{Log}({\hat{\mathbf{z}}}_{ij}^{-1}{\mathbf{x}}_i^{-1}\exp (-[\mathrm{\Delta }{\xi }_i{]}_{\times }){\mathbf{x}}_j)\\ & =\text{Log}({\mathbf{z}}_{ij}^{-1}{\mathbf{x}}_i^{-1}{\mathbf{x}}_j\exp ([-{\text{Ad}}_{{\mathbf{x}}_j^{-1}}\mathrm{\Delta }{\xi }_i{]}_{\times }))\cdots \text{[1]}\\ {\mathbf{e}}_{ij}({\xi }_i,{\xi }_j+\mathrm{\Delta }{\xi }_j)& =\text{Log}({\hat{\mathbf{z}}}_{ij}^{-1}{\mathbf{x}}_i^{-1}\exp ([\mathrm{\Delta }{\xi }_j{]}_{\times }){\mathbf{x}}_j)\\ & =\text{Log}({\mathbf{z}}_{ij}^{-1}{\mathbf{x}}_i^{-1}{\mathbf{x}}_j\exp ([{\text{Ad}}_{{\mathbf{x}}_j^{-1}}\mathrm{\Delta }{\xi }_j{]}_{\times }))\cdots \text{[2]}\end{array}\end{array}$$

 

In order to simplify this expression, $[1]$ and $[2]$ are respectively expressed as follows.

$$\begin{array}{}\text{(118)}& \begin{array}{r}\text{Log}(\exp ([\mathbf{a}{]}_{\times })\exp ([\mathbf{b}{]}_{\times }))\cdots \text{[1]}\\ \text{Log}(\exp ([\mathbf{a}{]}_{\times })\exp ([\mathbf{c}{]}_{\times }))\cdots \text{[2]}\end{array}\end{array}$$

• $\exp ([\mathbf{a}{]}_{\times })={\mathbf{z}}_{ij}^{-1}{\mathbf{x}}_i^{-1}{\mathbf{x}}_j$:Transformation matrix expressed in exponential terms.
  • According to the definition of ($\text{111}$) above, $\mathbf{a}={\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)$.
• $\mathbf{b}=-{\text{Ad}}_{{\mathbf{x}}_j^{-1}}\mathrm{\Delta }{\xi }_i$
• $\mathbf{c}={\text{Ad}}_{{\mathbf{x}}_j^{-1}}\mathrm{\Delta }{\xi }_j$

 

The above equation can be rearranged using the right-hand BCH approximation.  The right BCH approximation is

$$\begin{array}{}\text{(119)}& \begin{array}{rl}\exp ([\xi {]}_{\times })\exp ([\mathrm{\Delta }\xi {]}_{\times })& =\exp ([\xi +{\mathcal{J}}_r^{-1}\mathrm{\Delta }\xi {]}_{\times })\\ \exp ([\xi +\mathrm{\Delta }\xi {]}_{\times })& =\exp ([\xi {]}_{\times })\exp ([{\mathcal{J}}_r\mathrm{\Delta }\xi {]}_{\times })\end{array}\end{array}$$

 

For more information, please refer to Chapter 4 of Introduction to Visual SLAM.  By using the BCH approximation, ($\text{118}$) is summarized as follows.

$$\begin{array}{}\text{(120)}& \begin{array}{rl}\text{Log}(\exp ([\mathbf{a}{]}_{\times })\exp ([\mathbf{b}{]}_{\times }))& =\text{Log}(\exp ([\mathbf{a}+{\mathcal{J}}_r^{-1}\mathbf{b}{]}_{\times }))\\ & =\mathbf{a}+{\mathcal{J}}_r^{-1}\mathbf{b}\cdots \text{[1]}\\ \text{Log}(\exp ([\mathbf{a}{]}_{\times })\exp ([\mathbf{c}{]}_{\times }))& =\text{Log}(\exp ([\mathbf{a}+{\mathcal{J}}_r^{-1}\mathbf{c}{]}_{\times }))\\ & =\mathbf{a}+{\mathcal{J}}_r^{-1}\mathbf{c}\cdots \text{[2]}\end{array}\end{array}$$

 

Finally, after solving the substitution and rewriting the $\mathrm{\Delta }{\xi }_i$ and $\mathrm{\Delta }{\xi }_j$ expressions, the SE(3) version formula of ($\text{99}$) is obtained.

$$\begin{array}{}\text{(121)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{e}}_{ij}({\xi }_i+\mathrm{\Delta }{\xi }_i,{\xi }_j+\mathrm{\Delta }{\xi }_j)& =\mathbf{a}+{\mathcal{J}}_r^{-1}\mathbf{b}+{\mathcal{J}}_r^{-1}\mathbf{c}\\ & ={\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)-{\mathcal{J}}_r^{-1}{\text{Ad}}_{{\mathbf{x}}_j^{-1}}\mathrm{\Delta }{\xi }_i+{\mathcal{J}}_r^{-1}{\text{Ad}}_{{\mathbf{x}}_j^{-1}}\mathrm{\Delta }{\xi }_j\\ & ={\mathbf{e}}_{ij}({\xi }_i,{\xi }_j)+\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_i}\mathrm{\Delta }{\xi }_i+\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_j}\mathrm{\Delta }{\xi }_j\end{array}}}\end{array}$$

 

Therefore, the SE(3) version Jacobian of the final relative pose error is as follows.

$$\begin{array}{}\text{(122)}& \boxed{{\displaystyle \begin{array}{rl}& \frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_i}=-{\mathcal{J}}_r^{-1}{\text{Ad}}_{{\mathbf{x}}_j^{-1}}\in {\mathbb{R}}^{6\times 6}\\ & \frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_j}={\mathcal{J}}_r^{-1}{\text{Ad}}_{{\mathbf{x}}_j^{-1}}\in {\mathbb{R}}^{6\times 6}\end{array}}}\end{array}$$

 

At this time, since ${\mathcal{J}}_r^{-1}$ is a complicated expression, it is generally used by approximating it as follows, or it is used by setting it as ${\mathbf{I}}_6$ .

$$\begin{array}{}\text{(123)}& \begin{array}{r}{\mathcal{J}}_r^{-1}\approx {\mathbf{I}}_6+\frac{1}{2}\left[\begin{array}{cc}[\mathbf{w}{]}_{\times }& [\mathbf{v}{]}_{\times }\\ \mathbf{0}& [\mathbf{w}{]}_{\times }\end{array}\right]\in {\mathbb{R}}^{6\times 6}\end{array}\end{array}$$

 

If ${\mathcal{J}}_r^{-1}={\mathbf{I}}_6$ and optimization is performed, there is a reduction in the amount of computation, but the optimization performance is similar to the above Jacobian. The method used has a slight predominance.  For more information, please refer to Chapter 11 of Introduction to Visual SLAM.

 

5.1.2. Code implementations

• g2o code: edge_se3_expmap.cpp#L55  
  • In the g2o code above, the error is ${\mathbf{e}}_{ij}={\mathbf{x}}_j^{-1}{\mathbf{z}}_{ij}{\mathbf{x}}_i$ , which makes the Jacobian slightly different from the description above.
  • $\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_i}={\mathcal{J}}_l^{-1}{\text{Ad}}_{{\mathbf{x}}_j^{-1}{\mathbf{z}}_{ij}}$
  • $\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_j}=-{\mathcal{J}}_r^{-1}{\text{Ad}}_{{\mathbf{x}}_i^{-1}{\mathbf{z}}_{ij}^{-1}}$
  • This is the same format as in the ($\text{117}$) expression where $\mathrm{\Delta }{\xi }_i$  is arranged by passing terms to the left, and $\mathrm{\Delta }{\xi }_j$ is arranged by passing terms to the right and merged.
  • It also seems to be an approximate value for ${\mathcal{J}}_l^{-1}\approx {\mathbf{I}}_6,{\mathcal{J}}_r^{-1}\approx {\mathbf{I}}_6$.  Therefore, the actual implemented code is as follows.
    
    • $\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_i}\approx {\text{Ad}}_{{\mathbf{x}}_j^{-1}{\mathbf{z}}_{ij}}$
    • $\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathrm{\Delta }{\xi }_j}\approx -{\text{Ad}}_{{\mathbf{x}}_i^{-1}{\mathbf{z}}_{ij}^{-1}}$

 

 

6. Line reprojection error

Line reprojection error is an error used when optimizing a line in 3D space expressed in plücker coordinates.  For more information on Plücker coordinates, see the Plücker Coordinate 개념 정리 post.

 

 

NOMENCLATURE of line reprojection error

• ${\mathcal{T}}_{cw}\in {\mathbb{R}}^{6\times 6}$: Transformation matrix of Plücker lines
• ${\mathcal{K}}_L$: line intrinsic matrix
• $\mathbf{U}\in SO(3)$: Rotation matrix of a 3D line
• $\mathbf{W}\in SO(2)$: A matrix containing information on the distance a 3D line is from the origin
• $\mathit{\theta }\in {\mathbb{R}}^3$:  Parameter corresponding to rotation matrix SO(3) 
• $\theta \in \mathbb{R}$:  Parameter corresponding to rotation matrix SO(2)
• ${\mathbf{u}}_i$: $i$th column vector
• $\mathcal{X}=[{\delta }_{\mathit{\theta }},{\delta }_{\xi }]$: State variable
• ${\delta }_{\mathit{\theta }}=[{\mathit{\theta }}^{⊺},\theta ]\in {\mathbb{R}}^4$: State variables in orthonormal representation
• ${\delta }_{\xi }=[\delta \xi ]\in se(3)$: For the update method through Lie theory, refer to this link
• $\oplus$ : An operator that can update the state variable ${\delta }_{\mathit{\theta }},{\delta }_{\xi }$  in one step.
• $\mathbf{J}=\frac{\partial {\mathbf{e}}_l}{\partial \mathcal{X}}=\frac{\partial {\mathbf{e}}_l}{\partial [{\delta }_{\mathit{\theta }},{\delta }_{\xi }]}$

 

 

A line in 3D space can be expressed as a 6D column vector using Plücker coordinates.

$$\begin{array}{}\text{(124)}& \begin{array}{rl}\mathcal{L}& =[{\mathbf{m}}^{⊺}:{\mathbf{d}}^{⊺}{]}^{⊺}=[m_x:m_y:m_z:d_x:d_y:d_z{]}^{⊺}\end{array}\end{array}$$

 

Unlike the $[\mathbf{d}:\mathbf{m}]$ order described above, most papers using Plücker Coordinate use the $[\mathbf{m}:\mathbf{d}]$ order, so this section Also,  lines are expressed in the corresponding order. Since the linear expression method has scale ambiguity, it has 5 degrees of freedom, and even if $\mathbf{m}$ and $\mathbf{d}$ are not unit vectors, a line can be uniquely expressed by the ratio of the values ​​of the two vectors.

 

6.1. Line Transformation and projection

If ${\mathcal{L}}_w$ is a line viewed from the world coordinate system, it can be converted as follows when viewed from the camera coordinate system.

$$\begin{array}{}\text{(125)}& \begin{array}{rl}{\mathcal{L}}_c& =\left[\begin{array}{c}{\mathbf{m}}_c\\ {\mathbf{d}}_c\end{array}\right]={\mathcal{T}}_{cw}{\mathcal{L}}_w=\left[\begin{array}{cc}{\mathbf{R}}_{cw}& {\mathbf{t}}^{\wedge }{\mathbf{R}}_{cw}\\ 0& {\mathbf{R}}_{cw}\end{array}\right]\left[\begin{array}{c}{\mathbf{m}}_w\\ {\mathbf{d}}_w\end{array}\right]\end{array}\end{array}$$

- ${\mathcal{T}}_{cw}\in {\mathbb{R}}^{6\times 6}$:Transformation matrix for the  Plücker line 

 

Projecting the line onto the image plane gives:

$$\begin{array}{}\text{(126)}& \begin{array}{rl}{l}_c& =\left[\begin{array}{c}{l}_1\\ l_2\\ l_3\end{array}\right]={\mathcal{K}}_L{\mathbf{m}}_c=\left[\begin{array}{ccc}{f}_y& & \\ & f_x& \\ -f_y{c}_x& -f_x{c}_y& f_x{f}_y\end{array}\right]\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right]\end{array}\end{array}$$

- ${\mathcal{K}}_L$: 직선의 내부 파라미터 행렬(line intrinsic matrix)

 

${\mathcal{K}}_L$ means  $\mathcal{P}=[det(\mathbf{N}){\mathbf{N}}^{-⊺}|{\mathbf{n}}^{\wedge }\mathbf{N}]$  to $\mathbf{P}=K[\mathbf{I}|\mathbf{0}]$.  So $\mathcal{P}=[det(\mathbf{K}){\mathbf{K}}^{-⊺}|\mathbf{0}]$, so the $\mathbf{d}$ term of $\mathcal{L}$ is cleared to 0.  Therefore, when $\mathbf{K}=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right]$, the following expression is derived.

$$\begin{array}{}\text{(127)}& \begin{array}{r}{\mathcal{K}}_L=det(\mathbf{K}){\mathbf{K}}^{-⊺}=\left[\begin{array}{ccc}{f}_y& & \\ & f_x& \\ -f_y{c}_x& -f_x{c}_y& f_x{f}_y\end{array}\right]\in {\mathbb{R}}^{3\times 3}\end{array}\end{array}$$

 

6.2. Line reprojection error

The reprojection error ${\mathbf{e}}_l$ of a line can be expressed as follows.

$$\begin{array}{}\text{(128)}& \begin{array}{r}{\mathbf{e}}_l=\left[\begin{array}{c}{d}_s,d_e\end{array}\right]=\left[\begin{array}{cc}\frac{{\mathbf{x}}_s^{⊺}{l}_c}{\sqrt{l_1^2+l_2^2}},& \frac{{\mathbf{x}}_e^{⊺}{l}_c}{\sqrt{l_1^2+l_2^2}}\end{array}\right]\in {\mathbb{R}}^2\end{array}\end{array}$$

 

This can be expressed through the formula for the distance between a point and a line.  At this time, $\{{\mathbf{x}}_s$ and ${\mathbf{x}}_e\}$ mean the start and end points of the line extracted using line feature extracture (e.g., LSD), respectively. .  In other words, ${\mathcal{l}}_c$ is the predicted value obtained through modeling, and the line connecting ${\mathbf{x}}_s$ and ${\mathbf{x}}_e$ is measured through sensor data. becomes an observation.

 

6.3. Orthonormal representation

A problem arises if the Plücker Coordinate expression is used as it is when BA optimization is performed using ${\mathbf{e}}_l$ obtained above.  Since the Plücker Coordinate always has 5 degrees of freedom because it must satisfy the Klein quadric constraint of ${\mathbf{m}}^{⊺}\mathbf{d}=0$, the minimum number of parameters that can express a line is 4 It is over-parameterized.  The disadvantages of the over-parameterized expression method are as follows.

• Since redundant parameters must be calculated, the amount of computation increases during optimization.
• The additional degrees of freedom can lead to numerical instability problems.
• Whenever a parameter is updated, it must always be checked whether the constraint is satisfied.

 

Therefore, when optimizing a line, an orthonormal expression is generally used to change the minimum parameter to four degrees of freedom.  In other words, when expressing a line, Plücker Coordinate is used, but when performing optimization, it transforms into orthonormal expression, updates the optimal value, and returns to Plücker Coordinate.

 

Orthonormal expression is as follows.  A line in 3D space can always be expressed as:

$$\begin{array}{}\text{(129)}& \begin{array}{r}(\mathbf{U},\mathbf{W})\in SO(3)\times SO(2)\end{array}\end{array}$$

 

Any Plücker line $\mathcal{L}=[{\mathbf{m}}^{⊺}:{\mathbf{d}}^{⊺}{]}^{⊺}$ always has a one-to-one correspondence $(\mathbf{U},\mathbf{W})$, and this representation method is called orthonormal representation.  Given a line ${\mathcal{L}}_w=[{\mathbf{m}}_w^{⊺}:{\mathbf{d}}_w^{⊺}{]}^{⊺}$ in the world, $(\mathbf{U},\mathbf{W})$ can be obtained by performing QR decomposition of ${\mathcal{L}}_w$.

$$\begin{array}{}\text{(130)}& \begin{array}{r}\left[\begin{array}{c}{\mathbf{m}}_w|{\mathbf{d}}_w\end{array}\right]=\mathbf{U}\left[\begin{array}{cc}{w}_1& 0\\ 0& w_2\\ 0& 0\end{array}\right],\text{with set: }\mathbf{W}=\left[\begin{array}{cc}{w}_1& -w_2\\ w_2& w_1\end{array}\right]\end{array}\end{array}$$

 

At this time, the element $(1,2)$ of the upper triangle matrix $\mathbf{R}$ always becomes 0 due to the Plücker constraint (Klein quadric).  Since $\mathbf{U}$ and $\mathbf{W}$ mean 3D and 2D rotation matrices, respectively, $\mathbf{U}=\mathbf{R}(\mathit{\theta }),\mathbf{W}=\mathbf{R}(\theta )$.

$$\begin{array}{}\text{(131)}& \begin{array}{rl}\mathbf{R}(\mathit{\theta })& =\mathbf{U}=\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]=\left[\begin{array}{ccc}\frac{{\mathbf{m}}_w}{\Vert {\mathbf{m}}_w\Vert }& \frac{{\mathbf{d}}_w}{\Vert {\mathbf{d}}_w\Vert }& \frac{{\mathbf{m}}_w\times {\mathbf{d}}_w}{\Vert {\mathbf{m}}_w\times {\mathbf{d}}_w\Vert }\end{array}\right]\\ \mathbf{R}(\theta )& =\mathbf{W}=\left[\begin{array}{cc}{w}_1& -w_2\\ w_2& w_1\end{array}\right]=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\\ & =\frac{1}{\sqrt{{\Vert {\mathbf{m}}_w\Vert }^2+{\Vert {\mathbf{d}}_w\Vert }^2}}\left[\begin{array}{cc}\Vert {\mathbf{m}}_w\Vert & \Vert {\mathbf{d}}_w\Vert \\ -\Vert {\mathbf{d}}_w\Vert & \Vert {\mathbf{m}}_w\Vert \end{array}\right]\end{array}\end{array}$$

 

When performing real optimizations, they are updated like $\mathbf{U}\leftarrow \mathbf{U}\mathbf{R}(\mathit{\theta }),\mathbf{W}\leftarrow \mathbf{W}\mathbf{R}(\theta )$.  So the orthonormal representation is a  line in 3D space ${\delta }_{\mathit{\theta }}=[{\mathit{\theta }}^{⊺},\theta ]\in {\mathbb{R}}^4$ can be expressed with four degrees of freedom.  $[{\mathit{\theta }}^{⊺},\theta ]$ updated through optimization is converted to ${\mathcal{L}}_w$ as follows.

$$\begin{array}{}\text{(132)}& \begin{array}{r}{\mathcal{L}}_w=\left[\begin{array}{cc}{w}_1{\mathbf{u}}_1^{⊺}& w_2{\mathbf{u}}_2^{⊺}\end{array}\right]\end{array}\end{array}$$

 

6.4. Error function formulation

In order to optimize the reprojection error ${\mathbf{e}}_l$ for a line, iteratively using a nonlinear least squares method such as Gauss-Newton (GN) or Levenberg-Marquardt (LM) to find the optimal variable. need to update  The expression of the error function using the reprojection error is as follows.

$$\begin{array}{}\text{(133)}& \begin{array}{rl}{\mathbf{E}}_l(\mathcal{X})& =\arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\Vert {\mathbf{e}}_{l,ij}\Vert }^2\\ & =\arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\mathbf{e}}_{l,ij}^{⊺}{\mathbf{e}}_{l,ij}\end{array}\end{array}$$

 

The ${\Vert {\mathbf{e}}_l({\mathcal{X}}^{\ast })\Vert }^2$ satisfying ${\mathbf{E}}_l({\mathcal{X}}^{\ast })$ can be calculated iteratively through non-linear least squares. The optimal state is found by iteratively updating $\mathcal{X}$ in small increments $\mathrm{\Delta }\mathcal{X}$.
$$\begin{array}{}\text{(134)}& \begin{array}{rl}{\mathbf{E}}_l(\mathcal{X}+\mathrm{\Delta }\mathcal{X})& =\arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\Vert {\mathbf{e}}_l(\mathcal{X}+\mathrm{\Delta }\mathcal{X})\Vert }^2\end{array}\end{array}$$

Strictly speaking, since the state increment $\mathrm{\Delta }\mathcal{X}$ includes the SE(3) transformation matrix, it is correct to add it to the existing state $\mathcal{X}$ through the $\oplus$ operator, but for convenience of expression The $+$ operator is used.

$$\begin{array}{}\text{(135)}& \begin{array}{r}{\mathbf{e}}_l(\mathcal{X}\oplus \mathrm{\Delta }\mathcal{X})\to {\mathbf{e}}_l(\mathcal{X}+\mathrm{\Delta }\mathcal{X})\end{array}\end{array}$$

 

The above equation can be expressed as follows through Taylor's first-order approximation.
$$\begin{array}{}\text{(136)}& \begin{array}{rl}{\mathbf{e}}_l(\mathcal{X}+\mathrm{\Delta }\mathcal{X})& \approx {\mathbf{e}}_l(\mathcal{X})+\mathbf{J}\mathrm{\Delta }\mathcal{X}\\ & ={\mathbf{e}}_l(\mathcal{X})+{\mathbf{J}}_{\mathit{\theta }}\mathrm{\Delta }{\delta }_{\mathit{\theta }}+{\mathbf{J}}_{\xi }\mathrm{\Delta }{\delta }_{\xi }\\ & ={\mathbf{e}}_l(\mathcal{X})+\frac{\partial {\mathbf{e}}_l}{\partial {\delta }_{\mathit{\theta }}}\mathrm{\Delta }{\delta }_{\mathit{\theta }}+\frac{\partial {\mathbf{e}}_l}{\partial {\delta }_{\xi }}\mathrm{\Delta }{\delta }_{\xi }\end{array}\end{array}$$

 

$$\begin{array}{}\text{(137)}& \begin{array}{rl}{\mathbf{E}}_l(\mathcal{X}+\mathrm{\Delta }\mathcal{X})& \approx \arg \underset{{\mathcal{X}}^{\ast }}{min}\sum _i\sum _j{\Vert {\mathbf{e}}_l(\mathcal{X})+\mathbf{J}\mathrm{\Delta }\mathcal{X}\Vert }^2\end{array}\end{array}$$

 

By differentiating this, the optimal value of increment $\mathrm{\Delta }{\mathcal{X}}^{\ast }$ is obtained as follows.  The detailed derivation process is omitted in this section.  If you want to know more about the induction process, you can refer to the previous section.

$$\begin{array}{}\text{(138)}& \begin{array}{rl}& {\mathbf{J}}^{⊺}\mathbf{J}\mathrm{\Delta }{\mathcal{X}}^{\ast }=-{\mathbf{J}}^{⊺}\mathbf{e}\\ & \mathbf{H}\mathrm{\Delta }{\mathcal{X}}^{\ast }=-\mathbf{b}\end{array}\end{array}$$

 

6.4.1. The analytical jacobian of 3d line

As explained in the previous section, to perform nonlinear optimization we need to compute $\mathbf{J}$.  $\mathbf{J}$ consists of:

$$\begin{array}{}\text{(139)}& \begin{array}{rl}& \mathbf{J}=[{\mathbf{J}}_{\mathit{\theta }},{\mathbf{J}}_{\xi }]\end{array}\end{array}$$

 

$[{\mathbf{J}}_{\mathit{\theta }},{\mathbf{J}}_{\xi }]$ can be expanded as follows:

$$\begin{array}{}\text{(140)}& \begin{array}{rl}& {\mathbf{J}}_{\mathit{\theta }}=\frac{\partial {\mathbf{e}}_l}{\partial {\delta }_{\mathit{\theta }}}=\frac{\partial {\mathbf{e}}_l}{\partial l}\frac{\partial l}{\partial {\mathcal{L}}_c}\frac{\partial {\mathcal{L}}_c}{\partial {\mathcal{L}}_w}\frac{\partial {\mathcal{L}}_w}{\partial {\delta }_{\mathit{\theta }}}\\ & {\mathbf{J}}_{\xi }=\frac{\partial {\mathbf{e}}_l}{\partial {\delta }_{\xi }}=\frac{\partial {\mathbf{e}}_l}{\partial l}\frac{\partial l}{\partial {\mathcal{L}}_c}\frac{\partial {\mathcal{L}}_c}{\partial {\delta }_{\xi }}\end{array}\end{array}$$

 

$\frac{\partial {\mathbf{e}}_l}{\partial l}$ can be obtained as follows.  At this time, note that $l$ is a vector and $l_i$ is a scalar.

$$\begin{array}{}\text{(141)}& \boxed{{\displaystyle \begin{array}{r}\frac{\partial {\mathbf{e}}_l}{\partial l}=\frac{1}{\sqrt{l_1^2+l_2^2}}\left[\begin{array}{ccc}{x}_s-\frac{l_1{\mathbf{x}}_{s}l}{\sqrt{l_1^2+l_2^2}}& y_s-\frac{l_2{\mathbf{x}}_{s}l}{\sqrt{l_1^2+l_2^2}}& 1\\ x_e-\frac{l_1{\mathbf{x}}_{e}l}{\sqrt{l_1^2+l_2^2}}& y_e-\frac{l_2{\mathbf{x}}_{e}l}{\sqrt{l_1^2+l_2^2}}& 1\end{array}\right]\in {\mathbb{R}}^{2\times 3}\end{array}}}\end{array}$$

 

$\frac{\partial l}{\partial {\mathcal{L}}_c}$ can be obtained as follows.

$$\begin{array}{}\text{(142)}& \boxed{{\displaystyle \begin{array}{r}\frac{\partial l}{\partial {\mathcal{L}}_c}=\frac{\partial {\mathcal{K}}_L{\mathbf{m}}_c}{\partial {\mathcal{L}}_c}=\left[\begin{array}{cc}{\mathcal{K}}_L& {\mathbf{0}}_{3\times 3}\end{array}\right]=\left[\begin{array}{cccccc}{f}_y& & & 0& 0& 0\\ & f_x& & 0& 0& 0\\ -f_y{c}_x& -f_x{c}_y& f_x{f}_y& 0& 0& 0\end{array}\right]\in {\mathbb{R}}^{3\times 6}\end{array}}}\end{array}$$

 

$\frac{\partial {\mathcal{L}}_c}{\partial {\mathcal{L}}_w}$ can be obtained as follows.

$$\begin{array}{}\text{(143)}& \boxed{{\displaystyle \begin{array}{r}\frac{\partial {\mathcal{L}}_c}{\partial {\mathcal{L}}_w}=\frac{\partial {\mathcal{T}}_{cw}{\mathcal{L}}_w}{\partial {\mathcal{L}}_w}={\mathcal{T}}_{cw}=\left[\begin{array}{cc}{\mathbf{R}}_{cw}& {\mathbf{t}}^{\wedge }{\mathbf{R}}_{cw}\\ 0& {\mathbf{R}}_{cw}\end{array}\right]\in {\mathbb{R}}^{6\times 6}\end{array}}}\end{array}$$

 

The Jacobian $\frac{\partial {\mathcal{L}}_w}{\partial {\delta }_{\mathit{\theta }}}$ for the orthonormal representation can be obtained as follows:

$$\begin{array}{}\text{(144)}& \boxed{{\displaystyle \begin{array}{r}\frac{\partial {\mathcal{L}}_w}{\partial {\delta }_{\mathit{\theta }}}=\left[\begin{array}{cccc}{\mathbf{0}}_{3\times 1}& -w_1{\mathbf{u}}_3& w_1{\mathbf{u}}_2& -w_2{\mathbf{u}}_1\\ w_2{\mathbf{u}}_3& {\mathbf{0}}_{3\times 1}& -w_2{\mathbf{u}}_1& w_1{\mathbf{u}}_2\end{array}\right]\in {\mathbb{R}}^{6\times 4}\end{array}}}\end{array}$$

 

The Jacobian $\frac{\partial {\mathcal{L}}_c}{\partial {\delta }_{\xi }}$ for the camera pose can be obtained as follows:

$$\begin{array}{}\text{(145)}& \boxed{{\displaystyle \begin{array}{r}\frac{\partial {\mathcal{L}}_c}{\partial {\delta }_{\xi }}=\left[\begin{array}{cc}-(\mathbf{R}\mathbf{m}{)}^{\wedge }-({\mathbf{t}}^{\wedge }\mathbf{Rd}{)}^{\wedge }& -(\mathbf{Rd}{)}^{\wedge }\\ -(\mathbf{Rd}{)}^{\wedge }& {\mathbf{0}}_{3\times 3}\end{array}\right]\in {\mathbb{R}}^{6\times 6}\end{array}}}\end{array}$$

 

6.4.2. Code implementations

• Structure PLP SLAM code: g2o/se3/pose_opt_edge_line3d_orthonormal.h#L62
• Structure PLP SLAM code2: g2o/se3/pose_opt_edge_line3d_orthonormal.h#L81

 

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