Error in evaluation.py

[zerolover]

1. Mix different Quaternion definition in script The format of Estimated Trajectories is # timestamp_s tx ty tz qx qy qz qw from_quat use (x, y, z, w) format PoseTrajectory3D use (w, x, y, z) format
2. Error transformation in script The Estimated Trajectories is T_w_imu, so we should use T_w_imu * T_imu_prism to get the traj_est however the code seems strange...
3. Different extrinsic in rosbag the extrinsic seems different in rosbag, for example Basement_1 LAB_Survey_2 the result of rosrun tf tf_echo imu pole_tip is different

[bedaberner]

1. This is true, the Posetrajectory3d currently uses the wrong format. Because we only evaluate position error, this doesn't have any influence on the results, I will however still fix it for correctness sake.
2. There was indeed a sign error in the application of the calibration, I'll fix that. I verified with Rviz that the new version is indeed correct.
3. Datasets that were captured very early (Basement_1 was the first dataset) might have a slightly different value for the transform to prism and pole_tip. Since the submission is in imu frame and the values in the evaluation script will be used for the final evaluation this will not affect the results.

[zerolover]

@bedaberner Thanks. It was confusing using tf tf_echo /imu /pole_tip. Actually you will get T_imu_pole, namely a transformation transform point in pole Frame to IMU frame. You can see the source code. So the script the values need to be invert is wrong...

For clarification, I will post my script for reference

    apply_pole_tip_calibration = True
    # apply poletip calibration
    if apply_pole_tip_calibration == True:
        calibration_type = ref_file.split('_')[-1].lower()
        if calibration_type == 'pole.txt':
            T_imu_ref = np.array([[  0.176566,   -0.984288, 0.00121622, -0.00938425],
                                  [ -0.984256,    -0.17655, 0.00837907,  -0.0148401],
                                  [-0.0080327, -0.00267653,  -0.999964,     1.66722],
                                  [         0,           0,          0,         1.0]])
        elif calibration_type == 'prism.txt':
            T_imu_ref = np.array([[  0.176566,   -0.984288, 0.00121622, -0.00594496],
                                  [ -0.984256,    -0.17655, 0.00837907, -0.00721288],
                                  [-0.0080327, -0.00267653,  -0.999964,    0.272943],
                                  [         0,           0,          0,         1.0]])
        elif calibration_type == 'imu.txt':
            T_imu_ref = np.identity(4)
        else:
            print("reference file has non supported calibration type. Please don't change the reference file names.")
            exit()
        data = np.genfromtxt(est_file, delimiter=' ', skip_header=False)
        # stamps = data[:, 0]  # n x 1
        # xyz = data[:, 1:4]  # n x 3
        # quat = data[:, 4:]  # n x 4, xyzw
        for i in range(data.shape[0]):
            rot_mat = R.from_quat([data[i, 4:8]]).as_matrix().reshape([3, 3]) # from_quat(), xyzw
            transl = data[i, 1:4].reshape([3, 1])
            homogeneous_transform = np.vstack([np.hstack([rot_mat, transl]), np.array([0, 0, 0, 1])])
            result = homogeneous_transform @ T_imu_ref
            data[i, 1:4] = result[0:3,3].reshape([1, 3])
            data[i, 4:] = R.from_matrix(result[0:3,0:3]).as_quat()

        stamps = data[:, 0]  # n x 1
        xyz = data[:, 1:4]  # n x 3
        quat_wxyz = data[:, [7,4,5,6]] # n x 4
        traj_est = PoseTrajectory3D(xyz, quat_wxyz, stamps) # PoseTrajectory3D(), wxyz

    else:
        traj_est = file_interface.read_tum_trajectory_file(est_file)

[hemi86]

thank you for pointing out this error. I uploaded a new version of the evaluation script.