search

UZ-SLAMLab / ORB_SLAM3

ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
GNU General Public License v3.0
6.44k stars 2.53k forks source link

Relative orientation between Stereo camera and IMU #522

Open ymei opened 2 years ago

ymei commented 2 years ago

We encountered an interesting phenomenon that is dependent on the relative orientation between the Stereo camera and IMU. We have two ZED2i cameras, labeled A and B here, fixed on a rigid chassis. The Stereo camera of A is used for vision, and the IMU of B is used for inertial. In the default coordinate system of ZED2i in ROS, the IMU coordinate system is two 90deg rotations away from the camera's coordinate system, and Kalibr recovers this using calibration data well, and the distance in the x-direction is very close to reality.

IMU.T_b_c1: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [-0.00328153, -0.99999454, -0.00039314, -0.77762852,
          0.0009982,   0.00038986, -0.99999943, -0.00126859,
          0.99999412, -0.00328192,  0.00099692, -0.04008871,
          0.,          0.,          0.,          1.        ]

When this matrix T_b_c1 is used in Stereo-Inertial SLAM, the tracking fails. The failure is particularly pronounced when the vehicle makes a turn, where the SLAM reconstructed track diverges drastically from the ground truth. (Stereo alone, without IMU, reproduces the track reasonably OK)

However, if we add code to manually rotate IMU data such that the IMU coordinate system is oriented the same way as that of the camera, the Stereo-Inertial SLAM works well, with better accuracy than Stereo vision alone. In this case, the Kalibr generated matrix becomes

IMU.T_b_c1: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [ 0.99999658, -0.00122573,  0.00231222, -0.77736065,
          0.00122848,  0.99999854, -0.00118624, -0.0008075,
         -0.00231076,  0.00118908,  0.99999662, -0.0448897,
          0.     ,     0.   ,       0.      ,    1.        ]

The two cases used the same recorded dataset. The question is, why would manually aligning the coordinate systems between the camera and the IMU yield a good result while having the needed rotation encoded in T_b_c1 gives a bad one?

B-Leslie commented 2 years ago

The matrix that will transform a point in the IMU frame to be in the camera frame would be written T_c1_b in ORB_SLAM3. Are you sure you're using the correct matrix? Have you tried using its inverse?

ymei commented 2 years ago

The matrix that will transform a point in the IMU frame to be in the camera frame would be written T_c1_b in ORB_SLAM3. Are you sure you're using the correct matrix? Have you tried using its inverse?

Yes, we've tried using its inverse, it still diverges wildly.

mahammadirfan commented 2 years ago

hi @ymei , I am having the same issue. I am using stereo-inertial slam, I did the cam-imu calibration using kalibr and put the resulted matrix into the yaml file, stereo works good but stereo-inertial don't.

Could you please tell me more about how to change the cam and IMU orientation ? I am using an external IMU with a stereo camera. I didn't changed the orientation and just did the kalibr. I guess this is the reason stereo-inertial is not working in my case.

Looking forward to hearing from you. Thanks