Skip to content

[feat] Find calibration with external trajectory

Julia Sanchez requested to merge feat/FindCalibWithExtTraj into master

Goal : be able to compute the calibration transform between the SLAM trajectory tracking base frame and an external trajectory tracking another frame. This external trajectory can also be represented in another reference frame compared to the SLAM poses.

  • Basic idea is to minimize the sum of the residuals defined by a pair of poses (one from the current SLAM S, the other from the uploaded trajectory Ext) as :
residual_i =  Ext_i*Calib^-1-S_i
  • To make it independent from the global representation frame, we chose to use relative motion since first frames. W being the global representation frame of the SLAM and F being the global representation frame of the external trajectory, we get
residual_i = (F*Ext_0*Calib^-1)^-1 * F*Ext_i*Calib^-1 - (W*S_0)^-1 * W*S_i  -> residual_i = Calib * Ext_0^-1 * Ext_i * Calib^-1 - S_0^-1 * S_i

  • In ParaView, when loading an external pose trajectory, if the calibration is not found as a file in the same directory as the external pose data file, it is estimated with the above method.

  • In ROS, new commands are added to save the current trajectory on disk (tracking base frame or the LiDAR frame). Another command is added to compute the calibration between the current base frame and an external pose sensor using a file with a frame ID. After estimating the calibration, a static TF is published between the frames to send the calibration. The visualization plugin for rviz has also been updated to allow to save the trajectory, to save the maps and to calibrate the Lidar using an external file of poses. Below is the aspect of the new interface.

ROS_interface

Here is a demo when loading external sensor data without calibration file after having performed a small trajectory with pure SLAM.

Edited by Julia Sanchez

Merge request reports