Zed2 running vins-mono preliminary test

Last blog , Already completed ZED2 Calibration of , Obtain relevant calibration parameters , On this basis VINS Relevant modifications to , Implementations use ZED2 function VINS-MONO

One VINS-MONO Environment building

Because the system environment is from ubuntu18.04 Change to ubuntu20.04, Therefore, the environment needs to be rebuilt , The data set test environment can be used .
The details can be based on VINS-MONO practice

1）ceres Compiler error error: ‘integer_sequence’ is not a member of ‘std’
It is used in compiling some ceres An error will be reported when the project is completed error: ‘integer_sequence’ is not a member of ‘std’, This is because in the newer version ceres Yes c++ The version requires

In the report of the wrong item CMakeList Inside
set(CMAKE_CXX_FLAGS “-std=c++11”)
Change to
set(CMAKE_CXX_STANDARD 14)

2）error: ‘CV_RGB2GRAY’ was not declared in this scope
Add... To the error header file

#include <opencv2/imgproc/types_c.h>

3） An error occurred during compilation error: ‘CV_FONT_HERSHEY_SIMPLEX’ was not declared in this scope
The CV_FONT_HERSHEY_SIMPLEX The parameter is changed to cv::FONT_HERSHEY_SIMPLEX

4） take camera_model Package changed to compatible opencv4

stay camera_model Header file in package Chessboard.h Add
#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d_c.h>
stay CameraCalibration.h Add
#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgproc/imgproc_c.h>

For other questions, please refer to https://zhuanlan.zhihu.com/p/432167383

After successful compilation , function roslaunch vins_estimator vins_rviz.launch when , Report errors

ERROR: cannot launch node of type [rviz/rviz]: rviz
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/sjj/Vins-mono/src
ROS path [2]=/opt/ros/noetic/share

install rviz that will do

sudo apt-get install ros-noetic-rviz

Open the four terminals and run the following commands respectively ：

roscore
roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play MH_01_easy.bag

The dataset was run successfully ：

Two modify VINS The configuration file

modify realsense_color_config.yaml file

1) subscribe topics modify

imu_topic: "/zed2/zed_node/imu/data_raw"
image_topic: "/zed2/zed_node/left/image_rect_color"

2) Left eye camera internal parameter modification

model_type: PINHOLE
camera_name: camera
image_width: 1280
image_height: 720
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 414.926283
   fy: 416.695084
   cx: 653.478486
   cy: 365.478913

The corrected image is used here , Therefore, the distortion coefficients are set to 0;

3)IMU to cam The transformation matrix of , Parameter is modified as 2, Use online calibration （ Set to 0, It is also possible to use the existing calibration parameters ）：

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

here IMU and camera The external parameter matrix between... Is recommended Kalibr The tool is calibrated offline , It can also be changed to 1 perhaps 2 Let the estimator calibrate and optimize itself .

4)IMU Parameters , Use VINS-mono Parameters given in

#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80       # gravity magnitude

5) There is no need to estimate the synchronization time difference online

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

6) The camera changes to global exposure

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

3、 ... and function VINS-MONO

roslaunch zed_wrapper zed2.launch
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

Report errors ：

terminate called after throwing an instance of 'cv::Exception'
  what():  OpenCV(4.2.0) ../modules/core/src/matrix.cpp:423: error: (-215:Assertion failed) 0 <= _rowRange.start && _rowRange.start <= _rowRange.end && _rowRange.end <= m.rows in function 'Mat'

[feature_tracker-1] process has died [pid 4090, exit code -6, cmd /home/sjj/Vins-mono/devel/lib/feature_tracker/feature_tracker __name:=feature_tracker __log:=/home/sjj/.ros/log/ee50516c-4c0a-11ec-86e6-c7de7ab2b409/feature_tracker-1.log].
log file: /home/sjj/.ros/log/ee50516c-4c0a-11ec-86e6-c7de7ab2b409/feature_tracker-1*.log

The reason is the size of the camera picture , That is, the problem of high and wide scale , You can view the relevant topic see .

After modification, it can run successfully , But once you can't track it in real time , The position begins to drift

The suspect reason is that if the image tracking fails suddenly and there is no relocation , The track will go straight ahead , This is because only IMU Data time ,IMU The displacement of the integral 、 The velocity and angle values must drift . Yes IMU Parameter calibration and appropriate modification of the value of gravity acceleration will reduce the degree of offset, but can not be removed .

Try to lower IMU Frequency of release , Better position tracking , However, the matching accuracy and system robustness need to be submitted from the perspective of algorithm .