MSCKF_VIO Update

[borongyuan]

Now msckf_vio is ready to use. Tested with OAK cameras. You can refer to this patch or my fork for changes. I made some changes to the initial frame. Now the body frame and IMU frame have the same reference frame, avoiding the inconvenience of KumarRobotics/msckf_vio#39 (comment). After initialization, the body frame is located at the origin of the world frame, and its orientation is aligned with gravity. This is consistent with the initialization of the odometry of RTAB-Map. Some of the previous hardcoded parameters seem to be for the euroc dataset. After modification it should be suitable for other hardwares.

[youngjae41]

@borongyuan hello

I am also trying to implement rtabmap msckf using oak-d. All installations are complete, and when you run it, a message saying that camera sync is different is displayed. Is there any way?

[borongyuan]

Hi @youngjae41

The exact sync based on SequenceNum was added in previous commits. And for msckf_vio, publishInterIMU should be enabled.

[matlabbe]

First time trying MSCKF live other than euroc dataset, it works pretty well! Super fast VIO when you can play live with it! Thanks for the PR!

[youngjae41]

@borongyuan
I'm using the latest version of rtabmap with Stereo enabled, not RGB-D in the source. Does it work if I set it to RGB-D?
The IMU is using an external IMU, not a camera IMU

[borongyuan]

MSCKF_VIO is a stereo version of msckf. So only stereo input is supported. If you use external IMU, you need to handle the synchronization yourself. There are some pitfalls and tricks. I'm currently following msckf because of its performance and robustness. Other improvements provided by OpenVINS also make msckf have good accuracy. In addition, its image preprocessing parts such as feature detection and KLT tracking can be run on OAK camera. So I may write a hardware accelerated implementation later.

[youngjae41]

@borongyuan Thank you for answer. I am using noetic, can you provide me with depthai-ros executable of oak-d?

The problem I'm having has nothing to do with imu. It's just that right-left synchronization isn't accurate when using stereo.

[borongyuan]

For ROS use, I am not using depthhai-core. Instead, I wrote another ROS package, which only depend on depthhai-core. The official ROS package from depthhai still has many issues that have not been resolved. There are also some PRs that I have submitted have not been processed. I heard from their partners in China that Luxonis' software team is busy and short-handed. Their main focus now is on adding new features and RVC3.
Sorry that I can't provide you with the ROS package I wrote. But I can give you some implementation hints. First, using image_transport you can publish image and camera_info at the same time. You can use getCvFrame() in depthai-core to get cv::Mat, and then use CvImage() in cv_bridge to convert it into a ROS message. You need to be careful with timestamps, as ROS's clock may have time shift issue. My suggestion is to use the steady_clock on OAK device, and you may find the realtime_clock in realtime_tools helpful. Even though the two cameras share the same trigger signal, they may have different exposures and thus produce slightly different timestamps. Therefore, exact sync can only be achieved using SequenceNum. DepthAI only calibrates the camera system. For IMU you may need to measure Allan variance yourself to improve the accuracy of VIO algorithms. If you are using the built-in IMU, you can refer to my measurement data here.

[youngjae41]

@borongyuan Thanks for sharing even if you don't provide. I was wondering if it could work with the existing depthai_core, and this was a sufficient answer.

Currently, rtabmap_msckf_vio creates fixed values ​​for cam and imu rather than a cfg file. Will this affect the performance of mskf_vio?

[borongyuan]

For many VIO algorithms, a set of calibration parameters is needed, usually in kalibr like format. Many stereo camera products have been well calibrated before shipment. So I prefer to directly parse these built-in calibration parameters for the convenience of users. OAK cameras only lack IMU-Camera extrinsics. So I measured it directly from the PCB file. This should be accurate enough. As most algorithms are not sensitive to IMU-Camera extrinsics. You may also notice that when parsing parameters with depth-core, some of them are spec values by default. luxonis/depthai-core#847

[borongyuan]

Just noticed that the chi-squared confidence level of s-msckf needs to be corrected (KumarRobotics/msckf_vio#124). I updated the patch file. No new commit is required here.