SLAM轨迹按时间插值

介绍

最近在想，怎么把通过SLAM框架获得低频率轨迹，插值到高频率？例如获得雷达轨迹（10Hz），在同一时间系下，可以通过插值获得与相机频率下（30Hz）的轨迹（依然是雷达轨迹，当然知道雷达相机外参可变换成相机轨迹）。
思路是把旋转和平移分开插值再合起来，其中平移插值较简单，线性插值就行；旋转插值需要转成旋转向量进行插值，具体原理见代码和参考。

插值脚本

# 两个输入参数，分别为lidar轨迹`lidar_trajectory.txt`和待插值的时间戳`camera_timestamps.txt`
# 输出文件`lidar_trajectory_interpolation.txt`
python3 interpolation.py --input_path_traj lidar_trajectory.txt --input_path_timestamps camera_timestamps.txt

参数分析

lidar_trajectory.txt的格式为TUM格式，即每一行为time tx ty tz qx qy qz qw，举例如下：

1638071388.334760 0.00295208 0.000131266 -5.32433e-05 5.26652e-05 0.000110696 -0.000142421 1
1638071395.628481 0.08583 -0.00324124 1.6325e-06 0.000150509 0.00107514 0.00344029 0.999993
1638071396.651781 0.326695 -0.0111444 3.00087e-06 0.000977325 0.000722375 0.0100748 0.999949
1638071397.122600 0.456406 0.00337339 3.87732e-06 0.000402137 -0.000385426 0.0118958 0.999929
1638071398.425759 0.919398 0.0465742 4.34299e-06 0.00118325 -0.00351136 0.0400358 0.999191
.
.
.
1638071404.225555 2.37347 0.462837 -6.45353e-07 0.00305258 0.00564018 0.215082 0.976575

camera_timestamps.txt文本就是高频率的时间戳，举例如下：

1638071388.275513
1638071388.308870
1638071388.342227
1638071388.375584
1638071388.408941
1638071388.442298
.
.
.
1638071388.809231

脚本内容

interpolation.py脚本内容如下：

import argparse
from scipy.spatial.transform import Rotation as RR
import numpy as np
import math

def main(input_path_traj, input_path_timestamps):
    lidar_path_name = input_path_traj.split('/')
    file_name = lidar_path_name[-1].split('.')
    time1 = time2 = time_inter = 0.0
    time_stamps = []
    q1 = q2 = RR.from_quat([0.0, 0.0, 0.0, 1.0])
    trans1 = trans2 = np.array([0.0, 0.0, 0.0])
    is_initial = True
    with open(input_path_timestamps, 'r') as f_cam:
        for line in f_cam.readlines():
            time_stamps.append(float(line))
    
    time_stamps = iter(time_stamps)

    with open(input_path_traj, 'r') as f, open(file_name[0] + '_interpolation.txt', 'w+') as new_file:
        for line in f.readlines():
            tmp_txt = str.split(line.strip())
            
            if is_initial:
                while time_inter < float(tmp_txt[0]):
                    time_inter = next(time_stamps)
                time1 = float(tmp_txt[0])
                q1 = RR.from_quat([float(tmp_txt[4]), float(tmp_txt[5]), float(tmp_txt[6]), float(tmp_txt[7])])
                trans1 = np.array([float(tmp_txt[1]), float(tmp_txt[2]), float(tmp_txt[3])])
                is_initial = False
                
            time2 = float(tmp_txt[0])
            q2 = RR.from_quat([float(tmp_txt[4]), float(tmp_txt[5]), float(tmp_txt[6]), float(tmp_txt[7])])
            trans2 = np.array([float(tmp_txt[1]), float(tmp_txt[2]), float(tmp_txt[3])])
            
            while (time1 < time_inter) & (time2 > time_inter):
                print("time1 = {0}, time2 = {1}, time_inter = {2}".format(time1, time2, time_inter))
                q_inter = interpolate_rotation(time1, q1, time2, q2, time_inter)
                trans_inter = interpolate_translation(time1, trans1, time2, trans2, time_inter) 
                write_txt = "{0} {1} {2} {3} {4} {5} {6} {7}\n".format(time_inter, 
                    trans_inter[0], trans_inter[1], trans_inter[2], 
                    q_inter.as_quat()[0], q_inter.as_quat()[1], q_inter.as_quat()[2], q_inter.as_quat()[3])
                new_file.writelines(write_txt)
                
                time_inter = next(time_stamps, None)
                if time_inter is None:
                    return
            
            time1 = time2
            q1 = q2
            trans1 = trans2

def interpolate_translation(t1, trans1, t2, trans2, t_inter):
    return trans1 + (trans2 - trans1) / (t2 - t1) * (t_inter - t1)

def interpolate_rotation(t1, q1, t2, q2, t_inter):
    theta = (t_inter - t1) / (t2 - t1)
    q1_2 = RR.from_quat([-q1.as_quat()[0], -q1.as_quat()[1], -q1.as_quat()[2], q1.as_quat()[3]]) * q2
    q_inter = q1 * exp_quat(theta * log_quat(q1_2))
    return q_inter
    # return q1

# Method of implementing this function that is accurate to numerical precision from
# Grassia, F. S. (1998). Practical parameterization of rotations using the exponential map. journal of graphics, gpu, and game tools, 3(3):29–48.
def exp_quat(dx):
    theta = np.linalg.norm(dx)
    # na is 1/theta sin(theta/2)
    na = 0
    if is_less_then_epsilon_4th_root(theta):
        one_over_48 = 1.0 / 48.0
        na = 0.5 + (theta * theta) * one_over_48
    else:
        na = math.sin(theta * 0.5) / theta
    ct = math.cos(theta * 0.5)
    return RR.from_quat([dx[0]*na, dx[1]*na, dx[2]*na, ct])

def log_quat(q):
    q_imagi = q.as_quat()[:3]
    na = np.linalg.norm(q_imagi)
    eta = q.as_quat()[3]
    scale = 0.0
    # use eta because it is more precise than na to calculate the scale. No singularities here.
    if abs(eta) < na: 
    	# check sign of eta so that we can be sure that log(-q) = log(q)
        if eta >= 0:
            scale = math.acos(eta) / na
        else:
            scale = -math.acos(-eta) / na
    else:
     ###
     # In this case more precision is in na than in eta so lets use na only to calculate the scale:
     #
     # assume first eta > 0 and 1 > na > 0.
     #               u = asin (na) / na  (this implies u in [1, pi/2], because na i in [0, 1]
     #    sin (u * na) = na
     #  sin^2 (u * na) = na^2
     #  cos^2 (u * na) = 1 - na^2
     #                              (1 = ||q|| = eta^2 + na^2)
     #    cos^2 (u * na) = eta^2
     #                              (eta > 0,  u * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 )
     #      cos (u * na) = eta
     #                              (u * na in [ 0, pi/2] )
     #                 u = acos (eta) / na
     #
     # So the for eta > 0 it is acos(eta) / na == asin(na) / na.
     # From some geometric considerations (mirror the setting at the hyper plane q==0) it follows for eta < 0 that (pi - asin(na)) / na = acos(eta) / na.
     ###
        if eta > 0:
            scale = arc_sin_x_over_x(na)
        else:
            scale = -arc_sin_x_over_x(na)
    return q_imagi * (2.0 * scale)

def is_less_then_epsilon_4th_root(x):
    return x < pow(np.finfo(np.float64).eps, 1.0/4.0)
    
def arc_sin_x_over_x(x):
    if is_less_then_epsilon_4th_root(abs(x)):
        return 1.0 + x * x * (1.0/6.0)
    return math.asin(x) / x
    
if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--input_path_traj", required=True)
    parser.add_argument("--input_path_timestamps", required=True)
    args = parser.parse_args()
    main(args.input_path_traj, args.input_path_timestamps)

参考

该脚本参考Maplab（C++）内插值操作，可保证旋转插值的非奇异性和机器精度下的最优性。
对C++版本有需求的可以移步https://github.com/ethz-asl/maplab/blob/master/common/maplab-common/include/maplab-common/interpolation-helpers.h

---

水平有限，如有错误之处，望不吝指出~