Add accumulated noise to odometry estimation.

AddNoiseToOdometry.py

def odom_add_noise(poses_poor):
    ts_list = []
    pose_list = []
    for key, pose in sorted(poses_poor.items()):
        ts_list.append(key)
        pose_list.append(pose)

    velocity_integration = []
    for idx in range(1, len(pose_list)):
        velocity_integration.append(U.INV(pose_list[idx - 1]) @ pose_list[idx])

    # 积分全部的velocity
    # 这里就类似IMU预积分的场景
    pose_add_noise_list = []
    pose_add_noise_list.append(pose_list[0])
    for vi in velocity_integration:
        if len(pose_add_noise_list) == len(pose_list) // 3:
            noise = U.create_se3_matrix_from_xyzrpy(0, 0, 0, 0, 0, 0.05)
        else:
            noise = U.create_se3_matrix_from_xyzrpy(
                np.random.rand(1) * 1e-4,
                np.random.rand(1) * 1e-4,
                np.random.rand(1) * 1e-3,
                np.random.rand(1) * 1e-8,
                np.random.rand(1) * 1e-8,
                np.random.rand(1) * 1e-6
            )
        pose_add_noise_list.append(pose_add_noise_list[-1] @ vi @ noise)
    assert len(pose_list) == len(pose_add_noise_list)

    return {k: v for k, v in zip(ts_list, pose_add_noise_list)}