Ground truth data of RINS-W

[boundles]

Hi, first thanks for your great work. I have a question about how to generate ground truth data from Complex Urban LiDAR Data Set? Looking for your reply, thanks!

[mbrossar]

This dataset contains accurate pose estimates that uses a GPS-RTK, SLAM lidar, and an highly expansive gyro. If you need to compute velocity, you just have to differentiate position.

[Iqun1314]

Hi, first thanks for your great work. I have a question about how to generate ground truth data from Complex Urban LiDAR Data Set? Looking for your reply, thanks!

Hi,Wang Yang: I want to know how to extract the data from Complex Urban LiDAR Data Set. Do you have some ideas? We can use We-Chat to contact. Hope for your reply, thanks.

[mbrossar]

I extract ground truth for this dataset based on the following function.

def read_data(self, data_dir):
r"""Read the data from the dataset"""

print("Start read_data, be patient please")
def set_path(seq):
    path_imu = os.path.join(data_dir, seq, "sensor_data",
        "xsens_imu.csv")
    path_gt = os.path.join(data_dir, seq, "global_pose.csv")
    # path_odo = os.path.join(data_dir, seq, "encoder.csv")
    return path_imu, path_gt

time_factor = 1e9  # ns -> s

def interpolate(x, t, t_int, angle=False):
    """
    Interpolate ground truth with sensorsS
    """
    x_int = np.zeros((t_int.shape[0], x.shape[1]))
    for i in range(x.shape[1]):
        if angle:
            x[:, i] = np.unwrap(x[:, i])
        x_int[:, i] = np.interp(t_int, t, x[:, i])
    return x_int

sequences = os.listdir(data_dir)
# read each sequence
for sequence in sequences:
    print("\nSequence name: " + sequence)
    path_imu, path_gt = set_path(sequence)
    imu = np.genfromtxt(path_imu, delimiter=",")

    # Urban00-05 and campus00 have only quaternion and Euler data
    if not imu.shape[1] > 10:
        cprint("No IMU data for dataset " + sequence, 'yellow')
        continue
    gt = np.genfromtxt(path_gt, delimiter=",")

    # time synchronization between IMU and ground truth
    t0 = np.max([gt[0, 0], imu[0, 0]])
    t_end = np.min([gt[-1, 0], imu[-1, 0]])

    # start index
    idx0_imu = np.searchsorted(imu[:, 0], t0)
    idx0_gt = np.searchsorted(gt[:, 0], t0)

    # end index
    idx_end_imu = np.searchsorted(imu[:, 0], t_end, 'right')
    idx_end_gt = np.searchsorted(gt[:, 0], t_end, 'right')

    # subsample
    imu = imu[idx0_imu: idx_end_imu]
    gt = gt[idx0_gt: idx_end_gt]
    t_gt = gt[:, 0]
    t = imu[:, 0]

    # take ground truth position
    p_gt = gt[:, [4, 8, 12]]
    p_gt = p_gt - p_gt[0]

    # take ground matrix pose
    Rot_gt = torch.Tensor(gt.shape[0], 3, 3)
    for j in range(3):
        Rot_gt[:, j] = torch.Tensor(gt[:, 1 + 4 * j: 1 + 4 * j + 3])

    # convert to angle orientation
    rpys = SO3.to_rpy(Rot_gt)

    # interpolate ground-truth
    p_gt = interpolate(p_gt, t_gt, t)
    rpys = interpolate(rpys.numpy(), t_gt, t, angle=True)

    # convert from numpy
    ts = (t - t0)/time_factor
    p_gt = torch.Tensor(p_gt)
    rpys = torch.Tensor(rpys).float()
    Rot_gt = SO3.from_rpy(rpys[:, 0], rpys[:, 1], rpys[:, 2])
    q_gt = SO3.to_quaternion(Rot_gt)

    # take IMU gyro and accelerometer and magnetometer
    us = torch.Tensor(imu[:, 8:17])

    dt = ts[1:] - ts[:-1]
    # compute speed ground truth (apply smoothing)
    v_gt = torch.zeros(p_gt.shape[0], 3)
    for j in range(3):
        p_gt_smooth = savgol_filter(p_gt[:, j], 11, 1)
        v_j = (p_gt_smooth[1:] - p_gt_smooth[:-1]) / dt
        v_j_smooth = savgol_filter(v_j, 11, 0)
        v_gt[1:, j] = torch.Tensor(v_j_smooth)

    mondict = {
        'ts': ts,
        'qs': q_gt.float(),
        'vs': v_gt.float(),
        'ps': p_gt.float(),
    }
    pdump(mondict, self.predata_dir, sequence + "_gt.p")

[Iqun1314]

I extract ground truth for this dataset based on the following function.

def read_data(self, data_dir):
r"""Read the data from the dataset"""

print("Start read_data, be patient please")
def set_path(seq):
    path_imu = os.path.join(data_dir, seq, "sensor_data",
        "xsens_imu.csv")
    path_gt = os.path.join(data_dir, seq, "global_pose.csv")
    # path_odo = os.path.join(data_dir, seq, "encoder.csv")
    return path_imu, path_gt

time_factor = 1e9  # ns -> s

def interpolate(x, t, t_int, angle=False):
    """
    Interpolate ground truth with sensorsS
    """
    x_int = np.zeros((t_int.shape[0], x.shape[1]))
    for i in range(x.shape[1]):
        if angle:
            x[:, i] = np.unwrap(x[:, i])
        x_int[:, i] = np.interp(t_int, t, x[:, i])
    return x_int

sequences = os.listdir(data_dir)
# read each sequence
for sequence in sequences:
    print("\nSequence name: " + sequence)
    path_imu, path_gt = set_path(sequence)
    imu = np.genfromtxt(path_imu, delimiter=",")

    # Urban00-05 and campus00 have only quaternion and Euler data
    if not imu.shape[1] > 10:
        cprint("No IMU data for dataset " + sequence, 'yellow')
        continue
    gt = np.genfromtxt(path_gt, delimiter=",")

    # time synchronization between IMU and ground truth
    t0 = np.max([gt[0, 0], imu[0, 0]])
    t_end = np.min([gt[-1, 0], imu[-1, 0]])

    # start index
    idx0_imu = np.searchsorted(imu[:, 0], t0)
    idx0_gt = np.searchsorted(gt[:, 0], t0)

    # end index
    idx_end_imu = np.searchsorted(imu[:, 0], t_end, 'right')
    idx_end_gt = np.searchsorted(gt[:, 0], t_end, 'right')

    # subsample
    imu = imu[idx0_imu: idx_end_imu]
    gt = gt[idx0_gt: idx_end_gt]
    t_gt = gt[:, 0]
    t = imu[:, 0]

    # take ground truth position
    p_gt = gt[:, [4, 8, 12]]
    p_gt = p_gt - p_gt[0]

    # take ground matrix pose
    Rot_gt = torch.Tensor(gt.shape[0], 3, 3)
    for j in range(3):
        Rot_gt[:, j] = torch.Tensor(gt[:, 1 + 4 * j: 1 + 4 * j + 3])

    # convert to angle orientation
    rpys = SO3.to_rpy(Rot_gt)

    # interpolate ground-truth
    p_gt = interpolate(p_gt, t_gt, t)
    rpys = interpolate(rpys.numpy(), t_gt, t, angle=True)

    # convert from numpy
    ts = (t - t0)/time_factor
    p_gt = torch.Tensor(p_gt)
    rpys = torch.Tensor(rpys).float()
    Rot_gt = SO3.from_rpy(rpys[:, 0], rpys[:, 1], rpys[:, 2])
    q_gt = SO3.to_quaternion(Rot_gt)

    # take IMU gyro and accelerometer and magnetometer
    us = torch.Tensor(imu[:, 8:17])

    dt = ts[1:] - ts[:-1]
    # compute speed ground truth (apply smoothing)
    v_gt = torch.zeros(p_gt.shape[0], 3)
    for j in range(3):
        p_gt_smooth = savgol_filter(p_gt[:, j], 11, 1)
        v_j = (p_gt_smooth[1:] - p_gt_smooth[:-1]) / dt
        v_j_smooth = savgol_filter(v_j, 11, 0)
        v_gt[1:, j] = torch.Tensor(v_j_smooth)

    mondict = {
        'ts': ts,
        'qs': q_gt.float(),
        'vs': v_gt.float(),
        'ps': p_gt.float(),
    }
    pdump(mondict, self.predata_dir, sequence + "_gt.p")

Thank you very much! I will read carefully!

[Iqun1314]

hello , I run the code , but there has an error: NameError: name 'SO3' is not defined

Can you tell me how to solve this problem. Thank you ! Hope your reply.

[mbrossar]

Sorry, you need to download the code present in the src folder of this repo (take a look at dataset.py) and them import it

from lie_algebra import SO3

[Iqun1314]

Thank you so much!