Mono_Inertial mode does not work properly with RealSense D455 for image and IMU data acquisition

[liangchen97]

Mono_Inertial mode does not work properly with RealSense D455 for image and IMU data acquisition

I made sure that I had ORB-SLAM3 installed correctly and that the Euroc dataset was running properly.Now I need to use RealSense D455 to run ORB-SLAM3's MONO mode and MONO_INERTIAL mode We calibrate the D455 camera parameters, modify the topic in the code, and edit the YAML file

%YAML:1.0

Camera Parameters. Adjust them! Camera.type: "PinHole"

Camera calibration and distortion parameters (OpenCV) Camera.fx: 638.1018216271331 Camera.fy: 638.3300728549654 Camera.cx: 629.4728680757129 Camera.cy: 375.5517788016263

Camera.k1: -0.05529716280253562 Camera.k2: 0.04869113994652115 Camera.p1: -0.0012081319997317495 Camera.p2: 0.000948771411746312

Camera.width: 1280 Camera.height: 720

Camera frames per second Camera.fps: 20.0

Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1

ORB Parameters

ORB Extractor: Number of features per image ORBextractor.nFeatures: 1000

ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2 ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8 ORB Extractor: Fast threshold Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20 ORBextractor.minThFAST: 7

Viewer Parameters Viewer.KeyFrameSize: 0.05 Viewer.KeyFrameLineWidth: 1 Viewer.GraphLineWidth: 0.9 Viewer.PointSize: 2 Viewer.CameraSize: 0.08 Viewer.CameraLineWidth: 3 Viewer.ViewpointX: 0 Viewer.ViewpointY: -0.7 Viewer.ViewpointZ: -1.8 Viewer.ViewpointF: 500

I can run Mono mode as normal.So I also calibrated the IMU foreign parameters using kalibr and edited the YAML file as follows:

%YAML:1.0 Camera Parameters. Adjust them! Camera.type: "PinHole"

Camera calibration and distortion parameters (OpenCV) Camera.fx: 638.1018216271331 Camera.fy: 638.3300728549654 Camera.cx: 629.4728680757129 Camera.cy: 375.5517788016263

Camera.k1: -0.05529716280253562 Camera.k2: 0.04869113994652115 Camera.p1: -0.0012081319997317495 Camera.p2: 0.000948771411746312

Camera resolution Camera.width: 1280 Camera.height: 720

Camera frames per second Camera.fps: 20.0

Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1

Transformation from camera to body-frame (imu) Tbc: !!opencv-matrix rows: 4 cols: 4 dt: f data: [0.99999368, -0.00211361, -0.00285705, 0.02947189, 0.00208701, 0.99995474, -0.00928254, -0.00822922, 0.00287654, 0.00927652, 0.99995283, 0.02332212, 0.0, 0.0, 0.0, 1.0]

IMU noise IMU.NoiseGyro: 0.00201078376608 #1.6968e-04 IMU.NoiseAcc: 0.0170676352326 #2.0e-3 IMU.GyroWalk: 1.33363959029e-05 IMU.AccWalk: 0.000377325109439 # 3e-03 IMU.Frequency: 200

ORB Parameters

ORB Extractor: Number of features per image ORBextractor.nFeatures: 1000 # 1000 ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2

ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8

ORB Extractor: Fast threshold Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20 ORBextractor.minThFAST: 7

Viewer Parameters

Viewer.KeyFrameSize: 0.05 Viewer.KeyFrameLineWidth: 1 Viewer.GraphLineWidth: 0.9 Viewer.PointSize:2 Viewer.CameraSize: 0.08 Viewer.CameraLineWidth: 3 Viewer.ViewpointX: 0 Viewer.ViewpointY: -0.7 Viewer.ViewpointZ: -3.5 # -1.8 Viewer.ViewpointF: 500

The command line results are as follows:

nvidia@nvidia-desktop:~/catkin_ws/src/ORB_SLAM3$ rosrun ORB_SLAM3 Mono_Inertial /home/nvidia/catkin_ws/src/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/nvidia/catkin_ws/src/ORB_SLAM3/Examples/Monocular-Inertial/d455.yaml

ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. This program comes with ABSOLUTELY NO WARRANTY; This is free software, and you are welcome to redistribute it under certain conditions. See LICENSE.txt.

Input sensor was set to: Monocular-Inertial

Loading ORB Vocabulary. This could take a while... Vocabulary loaded!

Creation of new map with id: 0 Creation of new map with last KF id: 0 Seq. Name:

Camera Parameters:

• Camera: Pinhole
• fx: 638.102
• fy: 638.33
• cx: 629.473
• cy: 375.552
• k1: -0.0552972
• k2: 0.0486911
• p1: -0.00120813
• p2: 0.000948771
• fps: 20
• color order: RGB (ignored if grayscale)

ORB Extractor Parameters:

• Number of Features: 1000
• Scale Levels: 8
• Scale Factor: 1.2
• Initial Fast Threshold: 20
• Minimum Fast Threshold: 7

Left camera to Imu Transform (Tbc): [0.99999368, -0.0020870101, -0.0028765399, 0.029521611; 0.00211361, 0.99995476, -0.0092765205, -0.0080747996; 0.0028570499, 0.0092825396, 0.99995285, 0.0233132; 0, 0, 0, 1]

IMU frequency: 200 Hz IMU gyro noise: 0.00201078 rad/s/sqrt(Hz) IMU gyro walk: 1.33364e-05 rad/s^2/sqrt(Hz) IMU accelerometer noise: 0.0170676 m/s^2/sqrt(Hz) IMU accelerometer walk: 0.000377325 m/s^3/sqrt(Hz)

---

First KF:0; Map init KF:0 New Map created with 101 points Point distribution in KeyFrame: left-> 101 --- right-> 0 build optimization graph start optimization end optimization IMU in Map 0 is initialized RESETING FRAME!!! Fail to track local map! IMU is not or recently initialized. Reseting active map... SYSTEM-> Reseting active map in monocular case LM: Active map reset recieved LM: Active map reset, waiting... LM: Reseting current map in Local Mapping... LM: End reseting Local Mapping... LM: Reset free the mutex LM: Active map reset, Done!!! mnFirstFrameId = 101 mnInitialFrameId = 0 23 Frames set to lost First KF:13; Map init KF:0 New Map created with 89 points Point distribution in KeyFrame: left-> 89 --- right-> 0 build optimization graph start optimization end optimization IMU in Map 0 is initialized Fail to track local map! IMU is not or recently initialized. Reseting active map... SYSTEM-> Reseting active map in monocular case LM: Active map reset recieved LM: Active map reset, waiting... LM: Reseting current map in Local Mapping... LM: End reseting Local Mapping... LM: Reset free the mutex LM: Active map reset, Done!!! mnFirstFrameId = 180 mnInitialFrameId = 123 43 Frames set to lost First KF:24; Map init KF:13 New Map created with 101 points Point distribution in KeyFrame: left-> 101 --- right-> 0 build optimization graph start optimization end optimization IMU in Map 0 is initialized Fail to track local map! IMU is not or recently initialized. Reseting active map... SYSTEM-> Reseting active map in monocular case LM: Active map reset recieved LM: Active map reset, waiting... LM: Reseting current map in Local Mapping... LM: End reseting Local Mapping... LM: Reset free the mutex LM: Active map reset, Done!!! mnFirstFrameId = 222 mnInitialFrameId = 199 67 Frames set to lost First KF:35; Map init KF:24 New Map created with 77 points Point distribution in KeyFrame: left-> 77 --- right-> 0 Fail to track local map! IMU is not or recently initialized. Reseting active map... SYSTEM-> Reseting active map in monocular case LM: Active map reset recieved LM: Active map reset, waiting... LM: Reseting current map in Local Mapping... LM: End reseting Local Mapping... LM: Reset free the mutex LM: Active map reset, Done!!! mnFirstFrameId = 250 mnInitialFrameId = 245 69 Frames set to lost

It seems that ORB-SLAM3 has been unable to detect feature points and occasionally initializes soon after detecting and calculating the path, thus losing the result

[Ador2]

notice IMU is not or recently initialized. Reseting active map... you may need to excite the IMU more while keeping detected features.

[hellovuong]

It could be due to Tbc matrix is not correct, in my case, I use this matrix provide by librealsense and it works: Tbc: !!opencv-matrix rows: 4 cols: 4 dt: f data: [ 0.999999,-0.00128063,0.000919103,-0.0289604812860489, 0.00127786,0.999995,0.0030171,-0.00753192370757461, -0.000922961,-0.00301592,0.999995,-0.0156808998435736, 0.0, 0.0, 0.0, 1.0] Moreover, your IMU noises parameters should be increased. Also please, change text properties in your question, so that it is easier to look at.

[liangchen97]

Thank you for your answer. I will try to use librealsense to get the Tbc matrix. Besides, do I need to increase the noise of imu? These parameters of mine are calculated with kalibr. Can I refer to your yaml file? @hellovuong

[hellovuong]

hi @liangchen97, about IMU noise, I think yes you need to increase it. To get calibration (Intrinsic/Extrinsic) information, you should run this command after attaching the camera to your computer: rs-enumerate-devices -c Here is my config file for your reference D455.txt. It is Stereo-Intertial config, so just change it a little to work with Mono-Inertial case. Also, my config file is used with a D455 camera resolution of 640x360, so don't use my camera parameter if you use different resolutions.

[liangchen97]

hi @hellovuong Thank you again for providing the file. I have made the following modifications to my YAML file.

1. Modify the external parameters of IMU according to rs-enumerate-devices -c
2. Increase IMU internal parameters by 10 times Things seem to be getting a little better, but they're still not good.Now feature points are easily lost and the predicted trajectory is erratic

Maybe my understanding is wrong.Can I ask you a few questions?

1. Why does "Transformation from camera to body-frame (IMU)" reflect Extrinsic from "Gyro" to "Color"
2. How to get the IMU noise data? The IMU noise obtained by using rs-enumerate-devices -c is all 0
3. I noticed that IMU.Frequency is 400, default is 200.Do I need to change it here
4. Have you tried Mono mode? My result is always that Mono-inertial is not as stable as Mono

[hellovuong]

Good to know that. Sorry in the file I sent I used an Infrared camera not Color so the Tbc in the file is Extrinsic from "Gyro" To "Infrared1". I forgot to change that line.

1. This would surprise you that either Extrinsic from "Gyro" to "Infrared 1" or Extrinsic from "Infrared 1" to "Gyro" works in my experiments (Stereo +IMU). I could also confirm that Extrinsic from "Gyro" to "Color" or Extrinsic from "Color" to "Gyro" works in my experiments (Monocular using Color camera +IMU). It is bothering me too, but I have no time to investigate it yet.
2. Exactly, they did not provide IMU noise. I used this repo to compute it instead. Follow the instruction, you will get IMU noise then increase it by 10 or 20 times or follow TUM-VI on how they increased their IMU noise to inflating value.
3. Yes, I used IMU fps at 400. It would be no different if you use 200 (T265 used 200Hz) but I still prefer 400 and of course, you need to config it in your camera also.
4. I tested all cased: Mono only, Stereo Only, Mono/Stereo + IMU with both D435i, D455 and T265. Camera + IMU as you said is "not stable", it is obvious due to the nature of IMU, but when you config it correctly it works very well especially in the RECENTLY_LOST case.

I want to inform that how I recorded my datasets. Realsense provides accel and gyro separately, so you need to enable Online linear interpolation to merge that into 1 topic. That may cause the problem, once I tested with my rosbag format datasets with IMU online linear interpolation and the result is not good as I remember correctly, it is been while ago. So recently, I recorded the dataset in rosbag format with separated accel and gyro topics. Then, I parsed that rosbag file into a dataset in TUM-VI or EuRoC format with images and gyro/accel data in txt files. I wrote my own interpolation code to merge them into the IMU data based on gyro timestamps. I used that interpolated IMU data and the config file that I sent above and It works very well on ORB_SLAM3. I am not quite familiar with ROS so that all my testing datasets are in EuRoC format, it is easier to debug. So, I may recommend you to do the same procedures on your dataset to see how the result. If It works, the problem will be at linear interpolation in librealsence as I assumed. Otherwise, we need to investigate further. Moreover, in the first 30s of your dataset, you should provide enough motion for your camera, including moving along x-y-z and roll-pitch-yaw with NO constant velocity, so that the initialized process has a good estimation of IMU bias and velocity. I also found a modified librealsense repo that changed some stuff related to IMU interpolation to make it works with maplab (V-I system). I have not time to test it yet but you may want to take a look at it. My config file for D455_Color D455_Color.txt for your reference

Hope this can help you.

[liangchen97]

@hellovuong
Thank you for your willingness to take the time to answer my question. Based on the answers above, I have conducted further tests. I found that ORB-SLAM3 does not have strict requirements for external and internal parameters. Even if the imu noise of euroc.yaml is all set to 0, it can run with better results. So it is likely to be a problem with enable Online linear interpolation. Since I am new to this aspect, could you please explain in detail how to use the recorded rosbag containing images, accelerometer and angular velocity to generate a EuroC format rosbag that can be used？

[hellovuong]

Hi, Please refer to this Associate_datasets

[liangchen97]

Thank you, I will try this project

[liangchen97]

@hellovuong Hello, I ran it according to the above method, but the result is still not getting better. I think it may be the problem of orbslam3, it may be that the configuration file of realsense d455 is not configured properly. As far as I am concerned, I only configure acc and gyr to be turned on, and merge them, the others are the default formats. Is this configuration sufficient? Or some insights in other directions above?

[Pallav1299]

@hellovuong, thanks for your insights. I am working on an application which requires Monocular+Inertial SLAM. So, I came across ORB-SLAM3 and started testing it. It worked well with Euroc dataset. Then I tried using a Realsense D435i, but didn't achieve good results with the same even after doing external calibration using kalibr and imu_utils. I am witnessing the following behaviour:

Input sensor was set to: Monocular-Inertial

Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!

Creation of new map with id: 0
Creation of new map with last KF id: 0
Seq. Name: 

Camera Parameters: 
- Camera: Pinhole
- fx: 628.724
- fy: 626.18
- cx: 271.739
- cy: 218.344
- k1: 0.180974
- k2: -0.268076
- p1: -0.0105464
- p2: -0.0283861
- fps: 30
- color order: RGB (ignored if grayscale)

ORB Extractor Parameters: 
- Number of Features: 2000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7

Left camera to Imu Transform (Tbc): 
[0.99755955, 0.013658711, 0.068472087, 0.030867778;
 -0.016439233, 0.99905598, 0.040210493, 0.0017487752;
 -0.067858227, -0.041237988, 0.99684238, -0.0033510376;
 0, 0, 0, 1]

IMU frequency: 400 Hz
IMU gyro noise: 0.00212992 rad/s/sqrt(Hz)
IMU gyro walk: 1.67862e-05 rad/s^2/sqrt(Hz)
IMU accelerometer noise: 0.0244312 m/s^2/sqrt(Hz)
IMU accelerometer walk: 0.000506753 m/s^3/sqrt(Hz)
First KF:0; Map init KF:0
New Map created with 95 points
Point distribution in KeyFrame: left-> 95 --- right-> 0
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 117
mnInitialFrameId = 0
1270 Frames set to lost
First KF:107; Map init KF:0
New Map created with 161 points
Point distribution in KeyFrame: left-> 161 --- right-> 0
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 1515
mnInitialFrameId = 1386
1589 Frames set to lost
First KF:138; Map init KF:107
New Map created with 128 points
Point distribution in KeyFrame: left-> 128 --- right-> 0
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 1860
mnInitialFrameId = 1833
1616 Frames set to lost
First KF:143; Map init KF:138
New Map created with 136 points
Point distribution in KeyFrame: left-> 136 --- right-> 0
build optimization graph
start optimization
end optimization
scale too small
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
start VIBA 1
build optimization graph
start optimization
end optimization
end VIBA 1
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 1925
mnInitialFrameId = 1886
1836 Frames set to lost
First KF:172; Map init KF:143
New Map created with 183 points
Point distribution in KeyFrame: left-> 183 --- right-> 0
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 2152
mnInitialFrameId = 2144
1855 Frames set to lost
First KF:176; Map init KF:172
New Map created with 120 points
Point distribution in KeyFrame: left-> 120 --- right-> 0
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 2178
mnInitialFrameId = 2170
1875 Frames set to lost
First KF:180; Map init KF:176
New Map created with 146 points
Point distribution in KeyFrame: left-> 146 --- right-> 0
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 2218
mnInitialFrameId = 2197
2065 Frames set to lost
First KF:210; Map init KF:180
New Map created with 128 points
Point distribution in KeyFrame: left-> 128 --- right-> 0
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 2435
mnInitialFrameId = 2407
2124 Frames set to lost
First KF:219; Map init KF:210
New Map created with 123 points
Point distribution in KeyFrame: left-> 123 --- right-> 0
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 2575
mnInitialFrameId = 2493
2304 Frames set to lost
First KF:243; Map init KF:219
New Map created with 109 points
Point distribution in KeyFrame: left-> 109 --- right-> 0
build optimization graph
start optimization
end optimization
scale too small
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
start VIBA 1
build optimization graph
start optimization
end optimization
end VIBA 1
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 2801
mnInitialFrameId = 2754
2565 Frames set to lost
First KF:284; Map init KF:243
New Map created with 121 points
Point distribution in KeyFrame: left-> 121 --- right-> 0
build optimization graph
start optimization
end optimization
scale too small
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 3181
mnInitialFrameId = 3061
2643 Frames set to lost
First KF:295; Map init KF:284
New Map created with 150 points
Point distribution in KeyFrame: left-> 150 --- right-> 0
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 3296
mnInitialFrameId = 3258
2649 Frames set to lost
First KF:297; Map init KF:295
New Map created with 198 points
Point distribution in KeyFrame: left-> 198 --- right-> 0
build optimization graph
start optimization
end optimization
IMU in Map 0 is initialized
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
SYSTEM-> Reseting active map in monocular case
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 3456
mnInitialFrameId = 3301
2903 Frames set to lost

I want to inform that how I recorded my datasets. Realsense provides accel and gyro separately, so you need to enable Online linear interpolation to merge that into 1 topic. That may cause the problem, once I tested with my rosbag format datasets with IMU online linear interpolation and the result is not good as I remember correctly, it is been while ago. So recently, I recorded the dataset in rosbag format with separated accel and gyro topics. Then, I parsed that rosbag file into a dataset in TUM-VI or EuRoC format with images and gyro/accel data in txt files. I wrote my own interpolation code to merge them into the IMU data based on gyro timestamps. I used that interpolated IMU data and the config file that I sent above and It works very well on ORB_SLAM3. I am not quite familiar with ROS so that all my testing datasets are in EuRoC format, it is easier to debug. So, I may recommend you to do the same procedures on your dataset to see how the result. If It works, the problem will be at linear interpolation in librealsence as I assumed. Otherwise, we need to investigate further.

After going through your discussion it seemed to be an IMU related issue in D435i. Following which I tried converting my collected Rosbag to Euroc format with different time interpolation using https://github.com/hellovuong/Associate_datasets but still didn't achieve good results.

I also found a modified librealsense repo that changed some stuff related to IMU interpolation to make it works with maplab (V-I system). I have not time to test it yet but you may want to take a look at it.

I even tried using the modified librealsense repo. The results were still the same.

Few questions that I wanted to ask:

• What is the primary odometry source in ORB-SLAM3 (visual or inertial)?
• Will it work in featureless environments by just using an IMU? Any constraints?

I also tried using D435i with Vins Fusion (Mono+Inertial) where I could achieve stable tracking as compared to ORB-SLAM3(Mono+Inertial). Tracking using IMU was also working but degrades after sometime in featureless environments where Visual odometry is not available.

[hellovuong]

@liangchen97 hi sorry, I didn't see your reply. Actually, it is wired that the preprocessed dataset is not working. I did not set to merge acc and gyro at the time I recorded the dataset, it is two separate topics only, merge them by the repo that I shared

[hellovuong]

@Pallav1299 hi, have you try the OpenLoris dataset on ORB_SLAM3, they are using D435i also. Along with the data, they provided config files including Tbc. May you want to try that dataset first? then if it works well, you can try their configuration with your camera, pay attention on Tbc matrix.

[Pallav1299]

Thanks for your suggestion @hellovuong. I tried ORB-SLAM3 with the cafe sequence in OpenLoris dataset by converting the rosbag data to Euroc dataset format using https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/Associate_datasets for IMU interpolation.

I was able to achieve satisfactory results with the dataset. But I am still not able to achieve similar results on my setup although my extrinsic calibration was almost the same as the on in the dataset.

Can you help me in understanding the suspects for such behaviour? I suspect my calibration for the same. What do you think? If so, can you help me out with the exact procedure for calibrating Realsense D435i?

I believe the following things to be causing trouble:

• noisy IMU
• large reprojection error in Kalbr results
• improper IMU intrinsic calibration.

Just for clarification, I am using the RGB color camera(rolling shutter) and IMU data from D435i.

[HuangTY96]

I am using D435i to do mono-inertial slam. When I use IMU (200Hz) and color (20Hz), it will show same problem, but now I change them to IMU(400Hz) and color(30Hz), and the result is nice.

[Pallav1299]

I was able to get it working after proper calibration. Thanks.

[yaelbenoren]

could you please explain how you required the proper calibration? I have calibrated using Kalibr and am getting very bad results

[HuangTY96]

could you please explain how you required the proper calibration? I have calibrated using Kalibr and am getting very bad results

I just use Kalibr, but now I use IMU(400Hz) and color(30Hz), you can also try this parameter.

[yaelbenoren]

This won't help in my case since I am using IMU in 416Hz and camera in 10Hz (and can not change this).
@Pallav1299 used Kalibr too but modified something in the calibration to get good results.

[Pallav1299]

@yaelbenoren, ORB-SLAM3 mono-inertial won't work well for the RGB sensor(rolling shutter) + IMU on D435i. Also mentioned on this ISSUE. I used Kalibr for extrinsic calibration and I was able to achieve decent results. The odometry degrades badly in some cases and is not able to recover in such scenarios. You can use the Camera-IMU extrinsics from OpenLoris as is. What remains is the camera intrinsics and IMU intrinsics. Please mind the camera model that you choose for its calibration. I uses the pinhole-radtan model while using Kalibr. For IMU intrinsics I used imu_utils.

Now I am using the left IR camera(global shutter on D435i) as input to ORB-SLAM3 in place of the RGB sensor. You can use the following modified Realsense ros-wrapper for removing the projector dots from the IR cameras.

I am still sceptical about the failure detection and managing on ORB-SLAM3. Open to suggestion for the same.

[mateosss]

I was having similar logs to the ones presented in this issue with the D455, the most notorious symptom was the map resetting every 10 keyframes. In my case, I was able to solve it by checking different things. The bottom line is: double-check that all the timestamps that you are sending to TrackMonocular or TrackStereo make sense.

That includes:

• Making sure that all timestamps (frames, accelerometer, gyroscope) are in the same "timestamp domain" (a concept from the RealSense SDK). In my case, I was unable to install the DKMS kernel patch required by the RealSense API and this made the gyroscope stream work in a different timestamp domain which was about ~30ms off. (A quick solution for this is to use the accelerometer timestamps at 250hz and just copy the values from the last gyroscope sample at 200hz)
• Making sure you don't block execution in the RealSense threads when receiving a new sample. Make the receiving of RealSense samples lightweight, just push to a threadsafe queue (or even better, a single-writer single-consumer queue like this) and process the samples in a separate thread.
• I didn't need to implement IMU interpolation, but be sure to save for later IMU samples that have a timestamp greater than the image frame you are sending, else ORB-SLAM3 discards them.

[RoZhong]

color

May I ask what your color is? I don't know what this 20Hz is

[makolele12]

@yaelbenoren, ORB-SLAM3 mono-inertial won't work well for the RGB sensor(rolling shutter) + IMU on D435i. Also mentioned on this ISSUE. I used Kalibr for extrinsic calibration and I was able to achieve decent results. The odometry degrades badly in some cases and is not able to recover in such scenarios. You can use the Camera-IMU extrinsics from OpenLoris as is. What remains is the camera intrinsics and IMU intrinsics. Please mind the camera model that you choose for its calibration. I uses the pinhole-radtan model while using Kalibr. For IMU intrinsics I used imu_utils.

Now I am using the left IR camera(global shutter on D435i) as input to ORB-SLAM3 in place of the RGB sensor. You can use the following modified Realsense ros-wrapper for removing the projector dots from the IR cameras.

I am still sceptical about the failure detection and managing on ORB-SLAM3. Open to suggestion for the same.

Hi @Pallav1299 , I have literally the same setup as you (RealSense D435i) and same problems. You mention you got decent results with the RGB sensor after a proper calibration. Could you please elaborate it a bit more about what you exactly did?

I got the right extrinsics from OPENLORIS as you said. I calibrated the camera with Kalibr and performed IMU calibration with the allan-variance-ros package though. But I still cannot get passed through the initialization.

• Did you do something else aside from that?

• Did you modify something from the realsense-ros-wrapper?

• When using imu-utils, did you use the same noise parameters as outputted? or did you perform some extra calculations on them?

Thanks in advance!

[songxuekun]

Have you solved the problem yet

[songxuekun]

was able to get it working after proper calibration. Thanks could you tell me how do you solve the problem?

[wwendy233]

@yaelbenoren, ORB-SLAM3 mono-inertial won't work well for the RGB sensor(rolling shutter) + IMU on D435i. Also mentioned on this ISSUE. I used Kalibr for extrinsic calibration and I was able to achieve decent results. The odometry degrades badly in some cases and is not able to recover in such scenarios. You can use the Camera-IMU extrinsics from OpenLoris as is. What remains is the camera intrinsics and IMU intrinsics. Please mind the camera model that you choose for its calibration. I uses the pinhole-radtan model while using Kalibr. For IMU intrinsics I used imu_utils.

Now I am using the left IR camera(global shutter on D435i) as input to ORB-SLAM3 in place of the RGB sensor. You can use the following modified Realsense ros-wrapper for removing the projector dots from the IR cameras.

I am still sceptical about the failure detection and managing on ORB-SLAM3. Open to suggestion for the same.

What is the reprojection error after you calibrate the D345i with kalibr? The reprojection error I calibrated is about 0.1. T_ic (cam0 to imu0) generated by kalibr is uesd to run monocular_inertia, but the map still resets every 10 keyframes. I wonder if it's because of the calibration accuracy is not enough.