Home

Welcome to the LF-VIO wiki! Our wiki is constantly updated......

Camera Model:

For large FoV cameras, there are already many models. The MEI camera model and the Scaramuzza camera model are currently supported in our program.

Scaramuzza camera model calibration tool: OCamCalib

MEI camera model calibration tool：Omnidirectional Calibration Toolbox

...

Dataset:

Indoors datasets are open sourced as follows:

ID01, ID06, ID10: Google Drive

ID01~ID10: Baidu Yun, Code: d7wq

IDL01~IDL02: Baidu Yun, Code: khw2

Initialization for Large FoV camera

* Epipolar geometry problem

1、Essential matrix E Acquisition

Problem formulation:

$\mathbf{x}_2^T\left( \left[\mathbf{t}^{O_2}_{O_1}\right]^\wedge \mathbf{R}^{O_2}_{O_1} \right)\mathbf{x}_1=0$

$\mathbf{x}_1=\begin{bmatrix} \alpha_1 \\\beta_1 \\\gamma_1\end{bmatrix},\mathbf{x}_2=\begin{bmatrix} \alpha_2 \\\beta_2 \\\gamma_2\end{bmatrix}$

$ \mathbf{E}^{O_2}_{O_1}=\left[\mathbf{t}^{O_2}_{O_1}\right]^\wedge \mathbf{R}^{O_2}_{O_1}$

let $\mathbf{E}^{O_2}_{O_1}=\begin{bmatrix}E_{11},E_{12},E_{13}\\E_{21},E_{22},E_{23}\\E_{31},E_{32},E_{33}\end{bmatrix}$

$\begin{equation} \begin{bmatrix}\alpha_1\alpha_2&\alpha_1\beta_2 &\alpha_1\gamma_2&\beta_1\alpha_2&\beta_1\beta_2&\beta_1\gamma_2&\gamma_1\alpha_2&\gamma_1\beta_2 &\gamma_1\gamma_2\end{bmatrix}\begin{bmatrix}E_{11}\\E_{12}\\E_{13}\\E_{21}\\E_{22}\\E_{23}\\E_{31}\\E_{32}\\E_{33}\\\end{bmatrix}=0. \end{equation}$

One pair of points corresponds to one equation.

Due to the lack of scale constraints, at least eight pairs of points are required. We use the SVD method to solve this problem.

Code:

Eigen::Matrix3d InitialEXRotation::compute_E_21(vector<Eigen::Vector3d> &bearings_1, vector<Eigen::Vector3d> &bearings_2)

Input: bearings_1 and bearings_2

Output: best_E

2、Essential matrix E decomposition

Solve $\mathbf{R}^{O_2}_{O_1}$:

Since $[\mathbf{t}^{O_2}_{O_1}]^\wedge$ is a skew-symmetric matrix, $[\mathbf{t}^{O_2}_{O_1}]^\wedge=k\mathbf{U}diag\left( 1,1,0\right)\mathbf{W}\mathbf{U}^T$, where $\mathbf{W} =\begin{bmatrix} 0&-1&0\\1&0 &0\\0 & 0&1\end{bmatrix}$, $\mathbf{U}$ is orthogonal matrix, and $k$ is constant.

Then, we have $\mathbf{E}^{O_2}_{O_1}=\left[\mathbf{t}^{O_2}_{O_1}\right]^\wedge \mathbf{R}^{O_2}_{O_1}=\mathbf{U}diag\left( k,k,0\right)\mathbf{W}\mathbf{U}^T\mathbf{R}^{O_2}_{O_1}=\mathbf{U}\Sigma\mathbf{V}^T$.

If $k>0$, $\Sigma=diag\left( k,k,0\right)$, $\mathbf{R}^{O_2}_{O_1}=\mathbf{U}\mathbf{W}^T\mathbf{V}^T$,

and if $k<0$, $\Sigma=diag\left(-k,-k,0\right)$, $\mathbf{R}^{O_2}_{O_1}=\mathbf{U}\mathbf{W}\mathbf{V}^T$.

At this point, $\mathbf{R}^{O_2}_{O_1}$ has two solutions.

Solve $\mathbf{t}^{O_2}_{O_1}$:

$\mathbf{E}^{O_2}_{O_1}\mathbf{t}^{O_2}_{O_1}=0$ We use the SVD method to solve this problem to promise $||\mathbf{t}^{O_2}_{O_1}||=1$.

At this point, $\mathbf{t}^{O_2}_{O_1}$ has two solutions.

In summary, a total of four corresponding situations are shown in the above figure.

In our paper, yo select the correct solution, we use a method that chooses the $\mathbf{R}^{O_2}_{O_1}$ and $\mathbf{t}^{O_2}_{O_1}$ that maximize the number of feature point pairs while satisfying the condition that the dot product between the landmarks and the feature point is always positive.

$\begin{equation} \overrightarrow{O_1P_1}\cdot \overrightarrow{\mathbf{x_1}}>0\ \&\&\ \overrightarrow{O_2P_1}\cdot \overrightarrow{\mathbf{x_2}}>0. \end{equation}$

Code:

int MotionEstimator::recoverPose(Eigen::Matrix3d E, vector<Eigen::Vector3d> _points1, vector<Eigen::Vector3d> _points2,
 Matrix3d _R, Vector3d _t, vector<uchar> _mask)

Input: E, _points1, _points2

Output: _R, _t, mask(1:inlier 0:outlier)

* Triangulation problem

$\lambda\begin{bmatrix} \alpha \\ \beta \\ \gamma\ \end{bmatrix}=\left[ \mathbf{R}|\mathbf{t} \right]\begin{bmatrix} x\\y\\z\\1 \end{bmatrix}$

let $\textbf{u}=\begin{bmatrix} \alpha \\ \beta \\ \gamma\ \end{bmatrix}$,$\textbf{X}=\begin{bmatrix} x\\y\\z\\1 \end{bmatrix}$

$\lambda \textbf{u}=\textbf{P}\textbf{X}$ ,the left and right sides of the formula are cross-multiplied by $\textbf{u}$

$\left[ \textbf{u} \right]_\times\textbf{P}\textbf{X}=0 \Rightarrow \begin{bmatrix} 0&-\gamma &\beta\\ \gamma&0&-\alpha\\-\beta&\alpha&0 \end{bmatrix}\begin{bmatrix}\textbf{P}_1\\\textbf{P}_2\\\textbf{P}_3\end{bmatrix}\textbf{X}=0 $

$\left\{\begin{matrix}\left(\beta\textbf{P}_3-\gamma\textbf{P}_2 \right)\textbf{X}=0\\ \left(\gamma\textbf{P}_1-\alpha\textbf{P}_3 \right)\textbf{X}=0\\ \left(\alpha\textbf{P}_2-\gamma\textbf{P}_1 \right)\textbf{X}=0 \end{matrix}\right.$ Since the first two formulas can lead to the third formula, a point has two equations.

Therefore, there are four equations in the two-point triangulation process, and we use the SVD method to solve the three-dimensional coordinates to solve the $\textbf{A}\textbf{X}=0$ problem.

Code:

void MotionEstimator::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
                                       const Vector3d &point0, const Vector3d &point1, Vector3d &point_3d)

Input: Pose0, Pose1, point0, point1

Output: point_3d

* Perspective-n-Point(PnP) problem

$\lambda\begin{bmatrix} \alpha \\\beta \\\gamma \end{bmatrix}=\left[ \mathbf{R}|\mathbf{t} \right]\begin{bmatrix} x\\y\\z\\1 \end{bmatrix}\\ $

Input: $N$ pairs of $\begin{bmatrix} \alpha \\\beta \\\gamma \end{bmatrix}$ and $\begin{bmatrix} x\\y\\z\\1 \end{bmatrix}$

Output: $\mathbf{R}$ and $\mathbf{t}$

In our paper, we use EPnP method to slove PnP problem.

Code:

double PnpSolver::compute_pose(Eigen::Matrix3d &R, Eigen::Vector3d &T)

* Solve gyroscope bias

Problem formulation:

$\min_{\delta b_w}{\sum_{k\in all\_frame}{||{q^{c_{0}}_{b_{k+1}}}^{-1}\otimes q^{c_{0}}_{b_{k}}\otimes \gamma^{b_{k}}_{b_{k+1}}-\begin{bmatrix} 1\\0\\0\\0 \end{bmatrix}||}}^2$

Code:

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)

* Vision only SFM problem

$\begin{equation} \underset{\mathbf{R}_{c_i}^{w},\mathbf{t}_{c_i}^{w}}{min}\left\{ {{\sum}_{i=1}^M}{{\sum}_{j=1}^N}||\begin{bmatrix} \alpha_i\\\beta_i\\\gamma_i\end{bmatrix}-\frac{ \mathbf{R}_{c_j}^{c_i}\mathbf{p}_j+\mathbf{t}_{c_j}^{c_i}}{||\mathbf{R}_{c_j}^{c_i} \mathbf{p}_j+\mathbf{t}_{c_j}^{c_i}||_2}||^2\right\}, \end{equation}$

Code:

bool GlobalSFM::construct(int frame_num, Quaterniond *q, Vector3d *T, int l,
						  const Matrix3d relative_R, const Vector3d relative_T,
						  vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)

Optimization after initialization for Large FoV camera

* Visual measurement residual

$\begin{equation} \textbf{r}_c\left( \hat{\mathbf{z}}_l^{c_j},\chi \right)=\left[ \begin{array}{cc} \textbf{b}_1 & \textbf{b}_2 \end{array} \right]^T \cdot\left( \hat{\bar{\mathcal{P}}}_l^{c_j}-\frac{\mathcal{P}_l^{c_j}}{||\mathcal{P}_l^{c_j}||}\right), \end{equation}$

$\begin{equation} \begin{aligned} {\mathcal{P}}_l^{c_j} = & \textbf{R}_b^c\bigg( \textbf{R}_w^{b_j}\bigg( \textbf{R}_{b_i}^w \bigg( \textbf{R}_{c}^{b} \frac{1}{\lambda_d}\pi^{-1}_s\bigg({\begin{bmatrix} \hat{\bar{x}}_l^{c_i}\\\hat{\bar{y}}_l^{c_i} \end{bmatrix}}\bigg)\\ & + {\textbf{p}_{c}^b} \bigg)+{\textbf{p}_{b_i}^w}-{\textbf{p}_{b_j}^w} \bigg)-{\textbf{p}_c^b} \bigg), \end{aligned} \end{equation}$

Code without $t_d$ optimization::

ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j)

Code with $t_d$ optimization:

ProjectionTdFactor::ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j,
                                       const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j,
                                       const double _td_i, const double _td_j, const double _row_i, const double _row_j, int _track_size) : pts_i(_pts_i), pts_j(_pts_j),
                                                                                                                                            td_i(_td_i), td_j(_td_j), track_size(_track_size)

* Inertial Measurement Unit(IMU) residual

Code:

 IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
          linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
            jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
          sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}

* Marginalized residual

Code:

void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)