Learning VINS-Mono/Fusion source code from scratch (5): VIO initialization

This section analyzes the VIO initialization part

VINS-Mono/Fusion code learning series:
Learning VINS-Mono/Fusion source code from scratch (1): Main function
learning VINS-Mono/Fusion source code from scratch (2): Front-end image tracking
learning VINS-Mono/Fusion from scratch Source code (3): IMU pre-integration formula derivation and
learning VINS-Mono/Fusion from scratch source code (4): Error Kalman filter
learning VINS-Mono/Fusion from scratch source code (5): VIO initialization
learning VINS- from scratch Mono/Fusion source code (6): back-end optimization

1 Why do we need to initialize?

Provide initial values ​​(good initial values ​​can prevent the system from falling into a local minimum during the optimization process, reduce the number of iterations, and reduce the amount of calculations)

Initialized variables:
pose, velocity, zero offset, hardware external parameters, map points.

Among them, in the absence of a priori conditions, the accelerometer zero offset and translation external parameters are relatively difficult to solve , and the accelerometer bias is interfered by gravity. , difficult to separate.
Therefore, the translational external parameters are generally measured in advance, and the accelerometer zero bias is set to 0, which is processed in subsequent optimization.

2 Initialization process

Find the main function main() in estimator_node.cpp. The main function is similar to the optical flow part. It defines nodes, sets parameters, and receives topics.

int main(int argc, char **argv)
{
    
    
    ros::init(argc, argv, "vins_estimator");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    readParameters(n);
    estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    registerPub(n);

    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);//前端光流结果
    ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);//接收前端重启命令
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);//回环检测fast_relocalization响应

    std::thread measurement_process{
    
    process};
    ros::spin();
 
    return 0;
}

2.1 Parameter reading

Use readParameters(n) to read the estimator iterative calculation parameters, IMU parameters, rotation and translation external parameters, and time delay parameters in the yaml file.

estimator.setParameter() The estimator external parameter preset
is essentially a process of assigning external parameters, and then setting the confidence of the feature point. By default, under the virtual camera, the coordinate difference after projection of the feature point is 1.5 pixels.

void Estimator::setParameter()
{
    
    
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
    
    
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);

	//特征点置信度
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

2.2 Callback function

IMU_callback()
The IMU callback function mainly completes two tasks:

• Store imu messages in buffer
• Predict and send the pose based on the IMU frequency, and increase the odometer frequency (corresponding to the system block diagram propagation)

Process locks are used in the process of storing messages into the buffer to avoid access conflicts when data is put in and taken out. For specific concepts, see:
[c++11] Multi-threaded Programming (6) - Condition Variable (Condition Variable)

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    
    
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
    
    
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();

    last_imu_t = imu_msg->header.stamp.toSec();

    {
    
    
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg);
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

feature_callback is to throw the front-end optical flow data into the buffer
restart_callback does a state device reset

2.3 Measurement_process{process} - vio processing thread

2.3.1 Data preprocessing

• getMeasurements() completes the time synchronization of imu image frames (this getMeasurements() essentially groups image frames and imu data. The imu data between the previous frame k and the current frame k+1 is composed of the current image frame k+1 A group);
• After that, send imu to estimator.processIMU for pre-integration, and update the state quantity Ps Vs Rs in the sliding window to provide better initial values ​​for nonlinear optimization;
• The optical flow results (pixel coordinates, normalized coordinates, velocity) are reorganized into the 7x1 vector xyz_uv_velocity.

2.3.2 VIO initialization task

The VIO initialization and back-end optimization solution are completed in the estimator.processImage function. This section only discusses the VIO initialization task:

1. Feature point management, check whether it is a keyframe addFeatureCheckParallax
2. Rotation external parameter initialization CalibrationExRotation
3. VIO initialization initialStructure
   

3 VIO initialization implementation

3.1 Feature point management, check whether it is a key frame addFeatureCheckParallax

Send the feature point information of the current frame to f_manager for maintenance, and determine key frames through disparity.

VINS feature point maintenance method:
In the FeatureManager class, there is a list<FeaturePerId> feature, which is used to maintain the attributes corresponding to each feature point ID in the figure below.

Keyframe filter conditions:

• Is frame 0 or frame 1 (frame_count<2)
• The number of feature points tracked in the previous frame is less than 20 (last_track_num<20)
• Calculate the parallax between the second to last frame and the third to last frame (the essence is to determine whether the second to last frame is a key frame) compensatedParallax2
• There are no common view feature points with the previous frame (parallax_num==0)

If the penultimate frame is a key frame, remove the oldest frame; if the penultimate frame is not a key frame, remove it.

3.2 Rotation external parameter initialization CalibrationExRotation

Only for ESTIMATE_EXTRINSIC=2, it is necessary to calculate the rotation external parameters in the initialization stage.
The idea is to use the rotation relationship between two adjacent frames from k to k+n time to construct an overdetermined equation to solve.
There are two rotation relationships There are two ways to obtain it: the first is IMU pre-integration, and the second is feature point epipolar constraints.

Taking the time from k to k+1 as an example, the external parameter $q_{cb}$As a bridge, the following equation can be constructed:

$q_{cb}\otimes q_{b_k{b}_{k+1}}=q_{c_k{c}_{k+1}}\otimes q_{cb}$
${\left[q_{b_k{b}_{k+1}}\right]}_R{q}_{cb}={\left[q_{c_k{c}_{k+1}}\right]}_L{q}_{cb}$

For time k to k+n:

$\left(R_k-L_k\right)q_{cb}=0$
$\left(R_{k+1}-L_{k+1}\right)q_{cb}=0$
$⋮$
$\left(R_{k+n}-L_{k+n}\right)q_{cb}=0$

Then a problem like Ax=0 was constructed and solved through SVD singular value decomposition.

3.3 VIO initialization initialStructure

Tasks completed in the initialStructure function: check imu visibility, SFM pure visual three-dimensional reconstruction, pnp for all frames, visual inertia alignment

3.3.1 SFM part

Assume that there are 11 frames in the sliding window. First find a pivot frame (assuming the 4th frame) and require it to be as far away from the last frame as possible (the purpose is to avoid pure rotation and facilitate the solution of t). Solve the pivot frame and the sum through epipolar constraints . The relative pose between the last frames ; at the same time, too far a distance will result in a small number of feature points observed in both frames, so there is a trade-off.

Then, based on the obtained pose, the 3D world coordinates of the observed feature points are restored through triangulation .

Using the restored 3D points, use pnp (3D-2D) to restore the relative pose of the image frames of frames 5-9 . In this process, more 3D points can also be triangulated.

Similarly, use pnp to solve the image frame pose of frame 3-0.

Then, do a global BA to adjust these poses and 3D points.

3.3.2 Visual inertial alignment

• solveGyroscopeBias solves the gyro zero bias
  

  Theoretically, it is a unit quaternion. Take out the imaginary part to construct the Ax=b problem and solve $\delta b_w$：

• LinearAlignment solves for speed, scale, and gravity direction

  The quantity to be solved pointed out in the paper:
  
  the constructed observation equation:
  
  deduce how this equation comes from:
  first replace equation (5) with $c_0$Tie it down (that is, the pivot frame), and then bring it into equation (14).
  

  ①The translation pre-integrated component constitutes the equation:

  $R_{c_0}^{b_k}{p}_{b_{k+1}}^{c_0}=R_{c_0}^{b_k}(p_{b_k}^{c_0}+v_{b_k}^{c_0}\Delta t_k-\frac{1}{2}{g}^{c_0}\Delta t_k^2)+a$
  $R_{c_0}^{b_k}{p}_{b_{k+1}}^{c_0}=R_{c_0}^{b_k}(s{p}_{c_k}^{c_0}-R_{b_k}^{c_0}{p}_b^c+v_{b_k}^{c_0}\Delta t_k-\frac{1}{2}{g}^{c_0}\Delta t_k^2)+a$
  $R_{c_0}^{b_k}(s{p}_{c_{k+1}}^{c_0}-R_{b_{k+1}}^{c_0}{p}_b^c)=R_{c_0}^{b_k}(s{p}_{c_k}^{c_0}-R_{b_k}^{c_0}{p}_b^c+v_{b_k}^{c_0}\Delta t_k-\frac{1}{2}{g}^{c_0}\Delta t_k^2)+a$
  $a-R_{c_0}^{b_k}{R}_{b_k}^{c_0}{p}_b^c+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_b^c=R_{c_0}^{b_k}(s{p}_{c_{k+1}}^{c_0}-s{p}_{c_k}^{c_0}-v_{b_k}^{c_0}\Delta t_k+\frac{1}{2}{g}^{c_0}\Delta t_k^2)$
  $a-p_b^c+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_b^c=R_{c_0}^{b_k}(s{p}_{c_{k+1}}^{c_0}-s{p}_{c_k}^{c_0}-v_{b_k}^{c_0}\Delta t_k+\frac{1}{2}{g}^{c_0}\Delta t_k^2)$

  ②The rotation pre-integrated component constitutes the equation:

  $R_{c_0}^{b_k}{v}_{b_{k+1}}^{c_0}=R_{c_0}^{b_k}(v_{b_k}^{c_0}-g^{c_0}\Delta t_k^{})+b$
  $R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{v}^{b_{k+1}}=R_{c_0}^{b_k}(R_{b_k}^{c_0}{v}^{b_k}-g^{c_0}\Delta t_k^{})+b$
  $b=R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{v}^{b_{k+1}}-v^{b_k}+R_{c_0}^{b_k}{g}^{c_0}\Delta t_k^{}$