Handwriting VIO from scratch (1)

---

Handwriting VIO from scratch the first lecture homework
This is my own summary of the mind map of the first lecture: Handwriting VIO from scratch the first lecture mind map

---

Most of the previous review questions refer to the blog of the big guy https://blog.csdn.net/learning_tortosie/article/details/102408738, (lazy cancer committed again), this blog is set to be reprinted as a respect.
The formula derivation part is slightly different from the big guy blog. If there is an error, I hope to correct it.

1. What are the advantages of the integration of vision and IMU?

To talk about the advantages of the integration of vision and IMU, we must first understand the limitations of vision and IMU:

(1) Although the IMU can measure acceleration and angular velocity, there are obvious drifts in these quantities, which makes the pose data obtained by integration twice very unreliable. For example, if we put the IMU on the table without moving, the posture obtained by integrating its readings will drift out of the distance. However, for fast movements in a short period of time, IMU can provide some better estimates. This is the weakness of the camera. When the movement is too fast, the camera (rolling shutter) will have motion blur, or the overlap area between the two frames is too small for feature matching, so pure visual SLAM is very afraid of fast movement. With IMU, even during the period of time when the camera data is invalid, we can still maintain a good pose estimation, which is impossible for pure visual SLAM.

(2) Compared with the IMU, the camera data basically does not drift. If the camera is placed in place and fixed, then (in a static scene) the pose estimation of visual SLAM is also fixed. Therefore, the camera data can effectively estimate and correct the drift in the IMU reading, so that the pose estimation after slow motion is still valid.

(3) When the image changes, essentially we cannot know whether the camera itself has moved or the external conditions have changed, so pure visual SLAM is difficult to deal with dynamic obstacles. The IMU can feel its own motion information, to some extent reduce the impact of dynamic objects.

(4) For monocular vision SLAM, there is scale uncertainty, and the scale can be restored after fusing the IMU.

(5) Pure visual SLAM is susceptible to weak texture scenes and lighting changes. When positioning fails, IMU can be used for short-term positioning.

In summary, the fusion of vision and IMU will make up for their respective disadvantages. Visual positioning information can be used to estimate the zero offset of the IMU and reduce the divergence and cumulative error caused by the zero offset of the IMU; IMU can provide vision for fast-moving positioning, and When positioning fails due to some factors (fewer feature points in the scene, greater lighting changes, etc.).

2. What are the common vision + IMU fusion solutions? Are there any examples of industrial applications?

(1) Common vision + IMU fusion scheme

MSCKF
OKVIS
ROVIO
VIORB
VINS-Mono, VINS-Mobile, VINS-Fusion and the
above solutions are basically open source on Github.

(2) Industrial applications

Google ： Tango ， ARCore
Apple ： ARKit
Microsoft ： HoloLens
： ： DuMix AR

3. In academia, what are the new developments in VIO research? Are there any examples of applying learning methods to VIO?

To find new progress in academic research, you can go to Google Scholar to search through keywords (VIO, Visual-Inertial Odometry) and a limited time (such as since 2019). You can also search in robotics, computer vision and other top conferences (such as IROS, ICRA) , CVPR). Retrieval is the first step. The next step is to read the title and abstract, screen for meaningful research, and then selectively read the paper.

At present, I pay less attention to academic papers. Today, I just saw someone publish the new progress of VIO on Zhihu, so I simply moved it here.

(1) New progress in traditional methods

[1] Usenko V , Demmel N , Schubert D , et al. Visual-Inertial Mapping with Non-Linear Factor Recovery[J]. 2019.
[2] Shao W , Vijayarangan S , Li C , et al. Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping[J]. 2019.

(2) Examples based on learning methods

[1] Clark R, Wang S, Wen H, et al. VINet: Visual-Inertial Odometry as a Sequence-to-Sequence Learning Problem[C]//AAAI. 2017: 3995-4001.
[2] Chen, Changhao, et al. “Selective Sensor Fusion for Neural Visual-Inertial Odometry.” arXiv preprint arXiv:1903.01534 (2019).
[3] Shamwell, E. Jared, et al. “Unsupervised Deep Visual-Inertial Odometry with Online Error Correction for RGB-D Imagery.” IEEE transactions on pattern analysis and machine intelligence (2019).
[4] Lee, Hongyun, Matthew McCrink, and James W. Gregory. “Visual-Inertial Odometry for Unmanned Aerial Vehicle using Deep Learning.” AIAA Scitech 2019 Forum. 2019.
[5] Wang, Chengze, Yuan Yuan, and Qi Wang. “Learning by Inertia: Self-supervised Monocular Visual Odometry for Road Vehicles.” ICASSP 2019-2019 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP). IEEE, 2019.

4. Quaternion and Lie Algebra Update

1. Code:

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"

using namespace std; 

int main( int argc, char** argv )
{
    
    
    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    
    cout << "R * R^t ?= I"<< endl << R * R.transpose() <<endl;

    Sophus::SO3 SO3_R(R);               // 从旋转矩阵构造Sophus::SO(3)
    Eigen::Quaterniond q(R);            // 从旋转矩阵构造四元数

    // 李代数更新
    Eigen::Vector3d update_so3(0.01, 0.02, 0.03); //更新量
    Sophus::SO3 SO3_updated = SO3_R * Sophus::SO3::exp(update_so3);
    cout<<"SO3 updated = "<< endl << SO3_updated.matrix() <<endl;
    
    //四元数更新
    Eigen::Quaterniond q_update(1, update_so3(0)/2, update_so3(1)/2, update_so3(2)/2); 
    Eigen::Quaterniond q_updated = (q * q_update).normalized(); //四元数归一化
    cout<<"q2R = "<< endl << q_updated.toRotationMatrix() <<endl;

    return 0;
}

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(class1)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加eigen
include_directories("/usr/include/eigen3/")

# 添加sophus
find_package(Sophus REQUIRED)
include_directories(${
    
    Sophus_INCLUDE_DIRS})

# 添加可执行程序
add_executable(question2 question2.cpp)
target_link_libraries(question2
    ${
    
    Sophus_LIBRARIES}
    )

2. Output result
   
   From the result, the effect of updating through Lie algebra and updating through quaternion is not great.

5. Derivation

Derivation 1:

Derivation 2:

This is slightly different from the result of the referenced boss, but the result should not be wrong.