Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

kablir gives an irregular result for calibrating RGB and IMU of D455 #2568

Closed
Hypothesis-Z opened this issue Dec 5, 2022 · 12 comments
Closed
Labels

Comments

@Hypothesis-Z
Copy link

I tried to calibrate RGB and IMU of RealSense D455 using kablir but failed.

librealsense version: 2.41.0
realsense_ros version: 2.2.21
platform: nvidia jetson nx
Initial value of transform between RGB and IMU is searched from TF tree: camera_color_optical_frame and camera_imu_optical_frame.
The imu noise was estimated by Allan Variance.

Here is kalibr output:

rosrun kalibr kalibr_calibrate_imu_camera --target april_6x6.yaml --cams camchain.yaml --imu imu_bmi055.yaml --bag awsome.bag
importing libraries
Initializing IMUs:
  Update rate: 100.0
  Accelerometer:
    Noise density: 0.0153202936582 
    Noise density (discrete): 0.153202936582 
    Random walk: 0.000251690375031
  Gyroscope:
    Noise density: 0.00240175485095
    Noise density (discrete): 0.0240175485095 
    Random walk: 1.35378590854e-05
Initializing imu rosbag dataset reader:
	Dataset:          awsome.bag
	Topic:            /imu0
	Number of messages: 8069
Reading IMU data (/imu0)
  Read 8069 imu readings over 78.6 seconds                   
Initializing calibration target:
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.088 [m]
    Spacing 0.0264 [m]
Initializing camera chain:
Camera chain - cam0:
  Camera model: pinhole
  Focal length: [382.4713439941406, 382.0862121582031]
  Principal point: [322.09954833984375, 239.96194458007812]
  Distortion model: radtan
  Distortion coefficients: [-0.05718875676393509, 0.0715477243065834, -0.0008046463481150568, 0.00048176205018535256]
  baseline: no data available
Initializing camera rosbag dataset reader:
	Dataset:          awsome.bag
	Topic:            /cam0/image_raw
	Number of images: 1454
Extracting calibration target corners
  Extracted corners for 1435 images (of 1454 images)                              

Building the problem
	Spline order: 6
	Pose knots per second: 100
	Do pose motion regularization: False
		xddot translation variance: 1000000.000000
		xddot rotation variance: 100000.000000
	Bias knots per second: 50
	Do bias motion regularization: True
	Blake-Zisserman on reprojection errors -1
	Acceleration Huber width (sigma): -1.000000
	Gyroscope Huber width (sigma): -1.000000
	Do time calibration: True
	Max iterations: 30
	Time offset padding: 0.030000
Estimating time shift camera to imu:

Initializing a pose spline with 7856 knots (100.000000 knots per second over 78.555476 seconds)
  Time shift camera to imu (t_imu = t_cam + shift):
0.594118983071

Estimating imu-camera rotation prior

Initializing a pose spline with 7856 knots (100.000000 knots per second over 78.555476 seconds)
Gravity was intialized to [-0.05837476 -9.80178548  0.30002771] [m/s^2]
  Orientation prior camera-imu found as: (T_i_c)
[[ 0.99997033  0.00104116 -0.00763302]
 [-0.00097154  0.99995794  0.00911976]
 [ 0.00764219 -0.00911208  0.99992928]]
  Gyro bias prior found as: (b_gyro)
[-0.00173105  0.00105153 -0.00140897]

Initializing a pose spline with 7868 knots (100.000000 knots per second over 78.675476 seconds)

Initializing the bias splines with 3934 knots

Adding camera error terms (/cam0/image_raw)
  Added 1435 camera error terms                              

Adding accelerometer error terms (/imu0)
  Added 8069 of 8069 accelerometer error terms (skipped 0 out-of-bounds measurements)

Adding gyroscope error terms (/imu0)
  Added 8069 of 8069 gyroscope error terms (skipped 0 out-of-bounds measurements)

Before Optimization
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 1.51293447152, median 1.22090444886, std: 1.22749140483
Gyroscope error (imu0):        mean 4.59831692296, median 3.92815759511, std: 3.25002549482
Accelerometer error (imu0):    mean 3.8554000838, median 3.24488329379, std: 2.70277828539

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 1.51293447152, median 1.22090444886, std: 1.22749140483
Gyroscope error (imu0) [rad/s]:     mean 0.110440299759, median 0.0943447155938, std: 0.0780576449791
Accelerometer error (imu0) [m/s^2]: mean 0.590658614536, median 0.497125649473, std: 0.414073570251

Optimizing...
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Initializing
Optimization problem initialized with 15755 design variables and 183450 error terms
The Jacobian matrix is 383036 x 70881
[0.0]: J: 1.06979e+06
[1]: J: 783023, dJ: 286765, deltaX: 0.590529, LM - lambda:10 mu:2
[2]: J: 532584, dJ: 250438, deltaX: 0.826636, LM - lambda:10.8544 mu:2
Last step was a regression. Reverting
[3]: J: 1.94153e+06, dJ: -1.40895e+06, deltaX: 0.493894, LM - lambda:11.2358 mu:2
[4]: J: 196412, dJ: 336173, deltaX: 0.0955236, LM - lambda:44.9431 mu:4
[5]: J: 50894.4, dJ: 145517, deltaX: 0.230494, LM - lambda:42.7472 mu:2
Last step was a regression. Reverting
[6]: J: 60960.3, dJ: -10065.9, deltaX: 0.284167, LM - lambda:14.2491 mu:2
[7]: J: 43595.3, dJ: 7299.16, deltaX: 0.0436881, LM - lambda:56.9963 mu:4
[8]: J: 42976.4, dJ: 618.865, deltaX: 0.100316, LM - lambda:26.865 mu:2
[9]: J: 42891.4, dJ: 85.0516, deltaX: 0.0833367, LM - lambda:23.269 mu:2
[10]: J: 42495.1, dJ: 396.293, deltaX: 0.06903, LM - lambda:27.3106 mu:2
[11]: J: 42234, dJ: 261.067, deltaX: 0.0941264, LM - lambda:16.7083 mu:2
[12]: J: 41534.1, dJ: 699.897, deltaX: 0.229517, LM - lambda:9.5773 mu:2
Last step was a regression. Reverting
[13]: J: 57786.9, dJ: -16252.8, deltaX: 1.01848, LM - lambda:3.19243 mu:2
[14]: J: 41033.7, dJ: 500.378, deltaX: 0.110576, LM - lambda:12.7697 mu:4
[15]: J: 40630.5, dJ: 403.17, deltaX: 0.397158, LM - lambda:5.30167 mu:2
Last step was a regression. Reverting
[16]: J: 41419.2, dJ: -788.671, deltaX: 0.430296, LM - lambda:5.5663 mu:2
[17]: J: 39200.8, dJ: 1429.78, deltaX: 0.0390144, LM - lambda:22.2652 mu:4
[18]: J: 38676.6, dJ: 524.214, deltaX: 0.193053, LM - lambda:7.42173 mu:2
Last step was a regression. Reverting
[19]: J: 49650, dJ: -10973.4, deltaX: 0.974712, LM - lambda:2.47391 mu:2
[20]: J: 38372, dJ: 304.6, deltaX: 0.124208, LM - lambda:9.89565 mu:4
[21]: J: 38051, dJ: 320.962, deltaX: 0.570627, LM - lambda:3.29855 mu:2
[22]: J: 37966.9, dJ: 84.0441, deltaX: 0.447443, LM - lambda:3.51 mu:2
[23]: J: 36386.2, dJ: 1580.78, deltaX: 0.17314, LM - lambda:5.97629 mu:2
Last step was a regression. Reverting
[24]: J: 39852.3, dJ: -3466.1, deltaX: 0.814627, LM - lambda:2.34506 mu:2
[25]: J: 36192.9, dJ: 193.309, deltaX: 0.0631653, LM - lambda:9.38025 mu:4
[26]: J: 36077.1, dJ: 115.801, deltaX: 0.237071, LM - lambda:4.22605 mu:2
[27]: J: 35983.6, dJ: 93.4327, deltaX: 0.277643, LM - lambda:4.0807 mu:2
[28]: J: 35790.9, dJ: 192.711, deltaX: 0.229176, LM - lambda:4.11213 mu:2
[29]: J: 35730.9, dJ: 59.9746, deltaX: 0.299759, LM - lambda:3.74734 mu:2
[30]: J: 35600.7, dJ: 130.185, deltaX: 0.234193, LM - lambda:3.87596 mu:2

After Optimization (Results)
==================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.26381749927, median 0.194441248209, std: 0.2329223014
Gyroscope error (imu0):        mean 0.628025382007, median 0.216819005846, std: 1.05619641396
Accelerometer error (imu0):    mean 0.275233274591, median 0.101739616749, std: 0.508157293276

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.26381749927, median 0.194441248209, std: 0.2329223014
Gyroscope error (imu0) [rad/s]:     mean 0.0150836300776, median 0.0052074609907, std: 0.0253672486079
Accelerometer error (imu0) [m/s^2]: mean 0.0421665459123, median 0.0155868080527, std: 0.0778511895754

Transformation T_cam0_imu0 (imu0 to cam0, T_ci): 
[[ 0.99986547  0.01615346  0.00284803  0.03763731]
 [-0.01617964  0.9998247   0.00942316 -0.05021891]
 [-0.00269532 -0.00946797  0.99995155 -0.41248928]
 [ 0.          0.          0.          1.        ]]

cam0 to imu0 time: [s] (t_imu = t_cam + shift)
0.595493544549 

IMU0:
 ----------------------------
  Model: calibrated
  Update rate: 100.0
  Accelerometer:
    Noise density: 0.0153202936582 
    Noise density (discrete): 0.153202936582 
    Random walk: 0.000251690375031
  Gyroscope:
    Noise density: 0.00240175485095
    Noise density (discrete): 0.0240175485095 
    Random walk: 1.35378590854e-05
  T_i_b
    [[ 1.  0.  0.  0.]
     [ 0.  1.  0.  0.]
     [ 0.  0.  1.  0.]
     [ 0.  0.  0.  1.]]
  time offset with respect to IMU0: 0.0 [s]

  Saving camera chain calibration to file: awsome-camchain-imucam.yaml

  Saving imu calibration to file: awsome-imu.yaml
  Detailed results written to file: awsome-results-imucam.txt
Generating result report...

They should not have a 41cm offset on z-axis. Becides, time offset is also very large though I have set enable_sync. I wonder what could I do to acquire a good calibration result.

Another problem is IMU lost frames for a long time (sometimes for 1 second), why?
image

Looking forward a response...
Thanks.

@MartyG-RealSense
Copy link
Collaborator

Hi @Hypothesis-Z I would recommend using official RealSense calibration tools to calibrate RGB and the IMU module. Non-official tools are not designed specially for RealSense cameras and cannot save the calibration to the camera hardware.

RGB can be calibrated using the official Dynamic Calibration tool. Instructions for installing on Ubuntu can be found on page 14 onwards of the user guide for the tool.

https://www.intel.com/content/www/us/en/support/articles/000026723/emerging-technologies/intel-realsense-technology.html

The IMU can be calibrated with a Python tool.

https://github.com/IntelRealSense/librealsense/tree/master/tools/rs-imu-calibration

https://dev.intelrealsense.com/docs/imu-calibration-tool-for-intel-realsense-depth-camera

In regard to lost frames, tracking may be lost if the camera moves quickly or makes a sharp turn. This is described in Intel's D435i SLAM guide.

https://github.com/IntelRealSense/realsense-ros/wiki/SLAM-with-D435i


The built-in IMU can only keep track for a very short time. Moving or turning too quickly will break the sequence of successful point cloud matches and will result in the system losing track. It could happen that the system will recover immediately if stopped moving but if not, the longer the time passed since the break, the farther away it will drift from the correct position. The odds for recovery get very slim, very quickly. The parameters set in the launch file are most likely not ideal but this is a good starting point for calibrating.


In regard to IMU time offset, you could try disabling the global time setting by including global_time_enabled:=false in your launch instruction to see whether it improves the results.

@Hypothesis-Z
Copy link
Author

@MartyG-RealSense Thank you for quick response. I will try the calibraion tools and settings global_time_enabled:=false later. In regard to lost frames of IMU, I indeed moved the cameras slowly (about 0.2m/s) without running any SLAM program. I was just collecting data.

@MartyG-RealSense
Copy link
Collaborator

@Hypothesis-Z Thanks very much for the update. I look forward to hearing about your test results from the calibraion tools and the setting global_time_enabled:=false. Good luck!

@MartyG-RealSense
Copy link
Collaborator

Hi @Hypothesis-Z Do you require further assistance with this case, please? Thanks!

@Hypothesis-Z
Copy link
Author

@MartyG-RealSense Yes, thank you. I did IMU calibration with official tools recently. According to the last section of documentation, the script wrote the prameters into the camera. Does it mean that the data of /rscamera/gyro/imu_info and /rscamera/accel/imu_info are changed but it makes no difference to /rscamera/imu, /rscamera/gyro/sample and rscamera/accel/sample?

@MartyG-RealSense
Copy link
Collaborator

A key benefit of IMU calibration is that it optimizes the accuracy of the gravity acceleration values. So it is /camera/accel/sample that it is likely to have the most effect on.

@MartyG-RealSense
Copy link
Collaborator

Hi @Hypothesis-Z Do you have an update about this case that you can provide, please? Thanks!

@MartyG-RealSense
Copy link
Collaborator

Hi @Hypothesis-Z Do you require further assistance with this case, please? Thanks!

@Hypothesis-Z
Copy link
Author

Hi! @MartyG-RealSense I am confused with global_time_enabled:=false. If I set it false, the timestamp of images will be generated by realsense itself. But I have two realsense and other sensors in use. How could I synchronize them by setting global_time_enabled:=false? Thanks!

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented Jan 12, 2023

If global_time_enabled is set to True then the SDK will attempt to use a common timestamp for all cameras and does this by comparing camera hardware time with the computer's clock time, as described at IntelRealSense/librealsense#3909

In the RealSense ROS wrapper though, enabling global time has a less significant effect than it does in the librealsense SDK because ROS already corrects for timestamp drift anyway.

It is possible to synchronize devices as 'slaves' to a 'master' trigger so that the slave cameras attempt to follow the master's timestamp timing by using the inter_cam_sync_mode hardware sync system.

https://dev.intelrealsense.com/docs/multiple-depth-cameras-configuration

@MartyG-RealSense
Copy link
Collaborator

Hi @Hypothesis-Z Do you require further assistance with this case, please? Thanks!

@MartyG-RealSense
Copy link
Collaborator

Case closed due to no further comments received.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

2 participants