73 lines
1.6 KiB
Python
73 lines
1.6 KiB
Python
#
|
|
# BSD 3-Clause License
|
|
#
|
|
# This file is part of the Basalt project.
|
|
# https://gitlab.com/VladyslavUsenko/basalt.git
|
|
#
|
|
# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
|
|
# All rights reserved.
|
|
#
|
|
import os
|
|
import json
|
|
import platform
|
|
import subprocess
|
|
import re
|
|
import numpy as np
|
|
|
|
from munch import munchify
|
|
|
|
|
|
def copy_subdict(d, keys):
|
|
res = dict()
|
|
for k in keys:
|
|
if k in d:
|
|
res[k] = d[k]
|
|
return res
|
|
|
|
|
|
def load_json_if_exists(filepath):
|
|
if os.path.isfile(filepath):
|
|
with open(filepath, 'r') as f:
|
|
return munchify(json.load(f))
|
|
return None
|
|
|
|
|
|
def load_text_if_exists(filepath):
|
|
if os.path.isfile(filepath):
|
|
with open(filepath, 'r') as f:
|
|
return f.read()
|
|
return None
|
|
|
|
|
|
def load_trajectory_tum(filepath):
|
|
# first row is header
|
|
# format for each row is: ts x y z qz qy qz qw
|
|
traj = np.loadtxt(filepath, delimiter=" ", skiprows=1)
|
|
# return just translation for now
|
|
traj = traj[:, 1:4]
|
|
return traj
|
|
|
|
|
|
def load_trajectory_tum_if_exists(filepath):
|
|
if os.path.isfile(filepath):
|
|
return load_trajectory_tum(filepath)
|
|
return None
|
|
|
|
|
|
def os_open_file(filepath):
|
|
if platform.system() == 'Darwin':
|
|
subprocess.call(('open', filepath))
|
|
elif platform.system() == 'Windows':
|
|
os.startfile(filepath)
|
|
else:
|
|
subprocess.call(('xdg-open', filepath))
|
|
|
|
|
|
# key for 'human' sorting
|
|
def alphanum(key):
|
|
|
|
def convert(text):
|
|
return float(text) if text.isdigit() else text
|
|
|
|
return [convert(c) for c in re.split('([-+]?[0-9]*\.?[0-9]*)', key)]
|