wrong spatial alignment for visual inertial poses (T_W_E) and laser poses (T_B_H)

[JzHuai0108]

Dear author,

Thank you for open sourcing the hand eye calibration algorithm.
Following the instructions at README.md, I built the hand eye calibration package in Ubuntu 16.04 + ROS kinetic. The data was collected with a lidar (laser range finder) and a camera and an IMU, the three mounted rigidly to a moving platform.

Then the poses of the camera(eye) expressed in a world frame (the camera frame at the start), T_W_E, was obtained by a visual inertial odometry method with loop closure. And the poses of the lidar (hand) expressed in a base frame (the lidar frame at the start), T_B_H, was computed with a 2d lidar package. The data files are in the input directory of the attached zip file.

mydata.zip

In the end, the below command was executed.

rosrun hand_eye_calibration compute_complete_handeye_calibration.sh \
poses_B_H.csv poses_W_E.csv

And the resulting log, figures, and data files are in the output directory of the attached zip file.
It appears that the temporal calibration is done well. However, the spatial calibration is wildly wrong.

Can you please look into the issue? Am I missing certain points?

[smauq]

The problem is 2d lasers provide constraints only in xy motion and yaw, which causes the optimization problem to be degenerate and not converge. To solve this you would have to change the initial estimate and pass the appropriate priors to the optimization algorithm.

[JzHuai0108]

Do you mean the dual-quaternion-based hand-eye calibration requires the sensor to rotate about at least two axes?

As you suggested, the initial estimates contained in calibration.json was updated to the below values:

{
    "delay": -18.388532638549805, 
    "rotation": {
       "i": -0.0364413, 
       "j": -0.705874, 
       "k": 0.706455, 
       "w": 0.0365489
    }, 
    "translation": {
       "x": -0.374, 
       "y": -0.657, 
       "z": -0.61
    }
}

Then the below command was executed:

rosrun hand_eye_calibration_batch_estimation batch_estimator \
   --v 1 \
   --pose1_csv poses_B_H.csv \
   --pose2_csv poses_W_E.csv \
   --init_guess_file calibration.json \
   --output_file calibration_optimized.json

The resultant calibration_optimized.json has below content:

{
    "delay": "-18.364190020999999",
    "rotation": {
        "i": "0.08702350670296348",
        "j": "-0.70107073682332255",
        "k": "0.70236266525664703",
        "w": "-0.087258117345047473"
    },
    "translation": {
        "x": "-0.30239376833050313",
        "y": "-0.56651254374484339",
        "z": "-0.61006163744095709"
    }
}

These values looks reasonable. For this test, the optimization algorithm indeed handles the degenerate case well.

[smauq]

Yes, sounds reasonable. The idea of the hand eye calibration is to calibrate 2 fully 6DoF tracks, meaning it would require motion in all of the degrees of freedom. For the degenerate case indeed the first problem is coming up with an initial estimate.

The optimizer could then theoretically handle the case. Did it in the end converge to some visually correct looking result? To have a guarantee of it working would require giving the optimizer some knowledge about the priors (for example an initial height estimate with a covariance). This does not mean that you could not obtain a result in some cases. But since without full 6DoF movement some of the variables are not fully constrained and there could be multiple solutions (or even an infinity), you have no clear guarantees.

[JzHuai0108]

I agree.

IMHO, the optimized T_H_E results in visually correct alignment. Below are the figures before and after alignment drawn by hand_eye_calibration.

The visuals before alignment:
original size:

zoom in:

Using the manually measured T_H_E, the alignment result looks like:

Using the optimized T_H_E initialized with the manually measured value, the alignment result looks like:
and

[smauq]

Don't think the alignment is correct. They are both on the same z plane indeed, but the thin red and green lines between the frames which represent the trajectories are not overlain. I still see about a 90 degree rotation offset between the aligned trajectories.

[JzHuai0108]

I appreciate that you pointed out my misinterpretation of the figure. I used Horn's method implemented in https://vision.in.tum.de/data/datasets/rgbd-dataset/tools to get an initial rough value for T_H_E, and implemented a correlation approach to estimate the initial time offset, and finally updated calibration.json to

{
    "delay": -18.217288, 
    "rotation": {
       "i": -0.50038157, 
       "j": 0.49597917, 
       "k": -0.50092414, 
       "w": 0.50269053
    }, 
    "translation": {
       "x": 0.186525, 
       "y": 0.436177, 
       "z": 0.118397
    }
}

With the updated calibration.json, after running the batch_estimator, the calibration_optimized.json looks like:

{
    "delay": "-18.215783967",
    "rotation": {
        "i": "-0.50868042643872924",
        "j": "0.48512137503036812",
        "k": "-0.49618751775785747",
        "w": "0.5096069447368512"
    },
    "translation": {
        "x": "0.19301951643356816",
        "y": "0.069235089106293432",
        "z": "0.11977924556405914"
    }
}

And the results are visualized like follows.
The figure showing results of evaluate_alignment which used estimates from calibration_optimized.json.

The figure showing the 3D poses after alignment by using estimates from calibration_optimized.json.

And the aligned poses from another perspective.

I believe these results look fine. FYI, the log for batch_estimator is also attached.
batch_estimator.log

One comment about the initial time alignment in this hand_eye_calibration repository.
I have tried several approaches to estimate the time offset. All the estimates fall into [-18.22, -18.21]. And your batch_estimator also gave such an estimate, -18.2158. However, the compute_aligned_poses modules gave an exceptional estimate, -18.3885. Running batch_estimator initialized with this exceptional time offset estimate took much longer than with an estimate in range [-18.22, -18.21].