介绍

最近在想，怎么把通过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 mathdef 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.0time_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 = Truewith 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 = Falsetime2 = 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:returntime1 = time2q1 = q2trans1 = trans2def 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]]) * q2q_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 = 0if is_less_then_epsilon_4th_root(theta):one_over_48 = 1.0 / 48.0na = 0.5 + (theta * theta) * one_over_48else:na = math.sin(theta * 0.5) / thetact = 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) / naelse:scale = -math.acos(-eta) / naelse:#### 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) / xif __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

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

SLAM轨迹按时间插值相关推荐

SLAM轨迹全局误差计算
SLAM轨迹全局误差计算一.umeyama算法 SLAM结果输出之后,我们需要对其进行测量,判断定位的精确度如何.我们采用如下的高精度optitrack设备对SLAM设备进行GroundTruth ...
Pr进阶：时间重映射与时间插值
视频素材的录制速率已经是固定下来了,所以改变剪辑的速度指的是改变回放速率. 其它有关改变持续时间和速度的方法请参阅<Pr入门之七:改变剪辑的播放速度>. 所谓时间重映射,就是将录制速率映射 ...
evo-评估SLAM轨迹
1. 快捷安装直接安装最新的稳定发行版 pip install evo --upgrade --no-binary evo 2. 源码安装 github 地址:https://github.com/ ...
简易SLAM轨迹真值获取
坐标纸单目监控 SLAM轨迹真值获取装置 - 知乎SLAM性能的一个很重要的指标是定位精度,公开的室内数据集一般会采用动捕设备获取设备轨迹真值.但是动捕设备价格昂贵,OptiTrack百万以上,国产 ...
opencv - viz 画出SLAM轨迹
原文在这里数据集格式为:时间+平移+旋转矩阵四元数. #include<opencv2/opencv.hpp>#include<fstream> #include<io ...
SLAM轨迹测评工具evo使用
evo主要有以下几种命令: evo_ape : 计算绝对位姿误差 evo_rpe : 计算相对位姿误差 evo_traj : 轨迹管理的主要工具(常用来绘制轨迹) evo_config : 全局设置和 ...
SLAM 轨迹评估工具EVO
EVO使用教程 1 EVO环境安装 1.1 二进制安装 1.2 源码安装 2 使用方法 2.1 TUM数据集上使用 2.1.1 计算轨迹的绝对误差(evo_ape) 2.1.2 绘制多条曲线(evo_ ...
SLAM轨迹评估ATE和RPE
转自:https://zhuanlan.zhihu.com/p/105428199 1. RPE: relative pose error 相对位姿误差相对位姿误差主要描述的是相隔固定时间差两帧 ...
ICRA2022论文汇总 | 跟踪/检测/分割/传感器标定/SLAM/轨迹预测/导航规划等
点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心技术交流群后台回复[ECCV2022]获取ECCV2022所有自动驾驶方向论文! Ro ...

SLAM轨迹按时间插值

目录

介绍

插值脚本

参数分析

脚本内容

参考

SLAM轨迹按时间插值相关推荐

最新文章

热门文章