make rospy package
This commit is contained in:
Christian Forster
@@ -0,0 +1 @@
|
||||
*.pyc
|
||||
@@ -0,0 +1,8 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(svo_analysis)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS rospy)
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
catkin_python_setup()
|
||||
#catkin_package(CATKIN_DEPENDS sensor_msgs)
|
||||
@@ -0,0 +1,19 @@
|
||||
<package>
|
||||
|
||||
<name>svo_analysis</name>
|
||||
<version>0.0.1</version>
|
||||
<description>
|
||||
Python scripts to analyse performance of SVO.
|
||||
</description>
|
||||
<maintainer email="[email protected]">Christian Forster</maintainer>
|
||||
<author>Christian Forster</author>
|
||||
<license>BSD</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>rospy</build_depend>
|
||||
|
||||
<run_depend>python-rospkg</run_depend>
|
||||
<run_depend>python-yaml</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
|
||||
</package>
|
||||
@@ -1,67 +0,0 @@
|
||||
#!/usr/bin/python
|
||||
#
|
||||
# Requirements:
|
||||
# sudo apt-get install python-argparse
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import os
|
||||
import numpy
|
||||
|
||||
|
||||
def read_file_list(filename):
|
||||
file = open(filename)
|
||||
data = file.read()
|
||||
lines = data.replace(","," ").replace("\t"," ").split("\n")
|
||||
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
|
||||
list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
|
||||
return dict(list)
|
||||
|
||||
def associate(first_list, second_list,offset,max_difference):
|
||||
# first_list = groundtruth
|
||||
# second_list = estimate
|
||||
first_keys = first_list.keys()
|
||||
second_keys = second_list.keys()
|
||||
potential_matches = [(abs(a - (b + offset)), a, b)
|
||||
for a in first_keys
|
||||
for b in second_keys
|
||||
if abs(a - (b + offset)) < max_difference]
|
||||
|
||||
potential_matches.sort()
|
||||
matches = []
|
||||
for diff, a, b in potential_matches:
|
||||
if a in first_keys and b in second_keys:
|
||||
first_keys.remove(a)
|
||||
second_keys.remove(b)
|
||||
matches.append((a, b))
|
||||
|
||||
matches.sort()
|
||||
|
||||
return matches
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
# parse command line
|
||||
parser = argparse.ArgumentParser(description='''
|
||||
This script takes two data files with timestamps and associates them
|
||||
''')
|
||||
parser.add_argument('first_file', help='first text file (format: timestamp data)')
|
||||
parser.add_argument('second_file', help='second text file (format: timestamp data)')
|
||||
parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
|
||||
parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
|
||||
parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
|
||||
args = parser.parse_args()
|
||||
|
||||
first_list = read_file_list(args.first_file)
|
||||
second_list = read_file_list(args.second_file)
|
||||
|
||||
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
|
||||
|
||||
if args.first_only:
|
||||
for a,b in matches:
|
||||
print("%f %s"%(a," ".join(first_list[a])))
|
||||
else:
|
||||
for a,b in matches:
|
||||
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
|
||||
|
||||
|
||||
@@ -9,8 +9,8 @@ import argparse
|
||||
import csv
|
||||
import numpy as np
|
||||
from matplotlib import rc
|
||||
import scripts.analyse_logs as analyse_logs
|
||||
import scripts.analyse_timing as analyse_timing
|
||||
import svo_analysis.analyse_logs as analyse_logs
|
||||
import svo_analysis.analyse_timing as analyse_timing
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
d = generate_distutils_setup(
|
||||
packages=['svo_analysis'],
|
||||
package_dir={'': 'src'},
|
||||
install_requires=['rospkg', 'yaml'],
|
||||
)
|
||||
|
||||
setup(**d)
|
||||
@@ -0,0 +1,2 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
@@ -0,0 +1,128 @@
|
||||
#!/usr/bin/python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (c) 2013, Juergen Sturm, TUM
|
||||
# All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above
|
||||
# copyright notice, this list of conditions and the following
|
||||
# disclaimer in the documentation and/or other materials provided
|
||||
# with the distribution.
|
||||
# * Neither the name of TUM nor the names of its
|
||||
# contributors may be used to endorse or promote products derived
|
||||
# from this software without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
# Requirements:
|
||||
# sudo apt-get install python-argparse
|
||||
|
||||
"""
|
||||
The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.
|
||||
|
||||
For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import os
|
||||
import numpy
|
||||
|
||||
|
||||
def read_file_list(filename):
|
||||
"""
|
||||
Reads a trajectory from a text file.
|
||||
|
||||
File format:
|
||||
The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
|
||||
and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.
|
||||
|
||||
Input:
|
||||
filename -- File name
|
||||
|
||||
Output:
|
||||
dict -- dictionary of (stamp,data) tuples
|
||||
|
||||
"""
|
||||
file = open(filename)
|
||||
data = file.read()
|
||||
lines = data.replace(","," ").replace("\t"," ").split("\n")
|
||||
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
|
||||
list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
|
||||
return dict(list)
|
||||
|
||||
def associate(first_list, second_list,offset,max_difference):
|
||||
"""
|
||||
Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
|
||||
to find the closest match for every input tuple.
|
||||
|
||||
Input:
|
||||
first_list -- first dictionary of (stamp,data) tuples
|
||||
second_list -- second dictionary of (stamp,data) tuples
|
||||
offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
|
||||
max_difference -- search radius for candidate generation
|
||||
|
||||
Output:
|
||||
matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
|
||||
|
||||
"""
|
||||
first_keys = first_list.keys()
|
||||
second_keys = second_list.keys()
|
||||
potential_matches = [(abs(a - (b + offset)), a, b)
|
||||
for a in first_keys
|
||||
for b in second_keys
|
||||
if abs(a - (b + offset)) < max_difference]
|
||||
potential_matches.sort()
|
||||
matches = []
|
||||
for diff, a, b in potential_matches:
|
||||
if a in first_keys and b in second_keys:
|
||||
first_keys.remove(a)
|
||||
second_keys.remove(b)
|
||||
matches.append((a, b))
|
||||
|
||||
matches.sort()
|
||||
return matches
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
# parse command line
|
||||
parser = argparse.ArgumentParser(description='''
|
||||
This script takes two data files with timestamps and associates them
|
||||
''')
|
||||
parser.add_argument('first_file', help='first text file (format: timestamp data)')
|
||||
parser.add_argument('second_file', help='second text file (format: timestamp data)')
|
||||
parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
|
||||
parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
|
||||
parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
|
||||
args = parser.parse_args()
|
||||
|
||||
first_list = read_file_list(args.first_file)
|
||||
second_list = read_file_list(args.second_file)
|
||||
|
||||
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
|
||||
|
||||
if args.first_only:
|
||||
for a,b in matches:
|
||||
print("%f %s"%(a," ".join(first_list[a])))
|
||||
else:
|
||||
for a,b in matches:
|
||||
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
|
||||
|
||||
|
||||
@@ -0,0 +1,195 @@
|
||||
#!/usr/bin/python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (c) 2013, Juergen Sturm, TUM
|
||||
# All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above
|
||||
# copyright notice, this list of conditions and the following
|
||||
# disclaimer in the documentation and/or other materials provided
|
||||
# with the distribution.
|
||||
# * Neither the name of TUM nor the names of its
|
||||
# contributors may be used to endorse or promote products derived
|
||||
# from this software without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
# Requirements:
|
||||
# sudo apt-get install python-argparse
|
||||
|
||||
"""
|
||||
This script computes the absolute trajectory error from the ground truth
|
||||
trajectory and the estimated trajectory.
|
||||
"""
|
||||
|
||||
import sys
|
||||
import numpy
|
||||
import argparse
|
||||
import associate
|
||||
|
||||
def align(model,data):
|
||||
"""Align two trajectories using the method of Horn (closed-form).
|
||||
|
||||
Input:
|
||||
model -- first trajectory (3xn)
|
||||
data -- second trajectory (3xn)
|
||||
|
||||
Output:
|
||||
rot -- rotation matrix (3x3)
|
||||
trans -- translation vector (3x1)
|
||||
trans_error -- translational error per point (1xn)
|
||||
|
||||
"""
|
||||
numpy.set_printoptions(precision=3,suppress=True)
|
||||
model_zerocentered = model - model.mean(1)
|
||||
data_zerocentered = data - data.mean(1)
|
||||
|
||||
W = numpy.zeros( (3,3) )
|
||||
for column in range(model.shape[1]):
|
||||
W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
|
||||
U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
|
||||
S = numpy.matrix(numpy.identity( 3 ))
|
||||
if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
|
||||
S[2,2] = -1
|
||||
rot = U*S*Vh
|
||||
trans = data.mean(1) - rot * model.mean(1)
|
||||
|
||||
model_aligned = rot * model + trans
|
||||
alignment_error = model_aligned - data
|
||||
|
||||
trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
|
||||
|
||||
return rot,trans,trans_error
|
||||
|
||||
def plot_traj(ax,stamps,traj,style,color,label):
|
||||
"""
|
||||
Plot a trajectory using matplotlib.
|
||||
|
||||
Input:
|
||||
ax -- the plot
|
||||
stamps -- time stamps (1xn)
|
||||
traj -- trajectory (3xn)
|
||||
style -- line style
|
||||
color -- line color
|
||||
label -- plot legend
|
||||
|
||||
"""
|
||||
stamps.sort()
|
||||
interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
|
||||
x = []
|
||||
y = []
|
||||
last = stamps[0]
|
||||
for i in range(len(stamps)):
|
||||
if stamps[i]-last < 2*interval:
|
||||
x.append(traj[i][0])
|
||||
y.append(traj[i][1])
|
||||
elif len(x)>0:
|
||||
ax.plot(x,y,style,color=color,label=label)
|
||||
label=""
|
||||
x=[]
|
||||
y=[]
|
||||
last= stamps[i]
|
||||
if len(x)>0:
|
||||
ax.plot(x,y,style,color=color,label=label)
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
# parse command line
|
||||
parser = argparse.ArgumentParser(description='''
|
||||
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
|
||||
''')
|
||||
parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
|
||||
parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
|
||||
parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
|
||||
parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)
|
||||
parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
|
||||
parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
|
||||
parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
|
||||
parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
|
||||
parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')
|
||||
args = parser.parse_args()
|
||||
|
||||
first_list = associate.read_file_list(args.first_file)
|
||||
second_list = associate.read_file_list(args.second_file)
|
||||
|
||||
matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))
|
||||
if len(matches)<2:
|
||||
sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
|
||||
|
||||
|
||||
first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
|
||||
second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
|
||||
rot,trans,trans_error = align(second_xyz,first_xyz)
|
||||
|
||||
second_xyz_aligned = rot * second_xyz + trans
|
||||
|
||||
first_stamps = first_list.keys()
|
||||
first_stamps.sort()
|
||||
first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
|
||||
|
||||
second_stamps = second_list.keys()
|
||||
second_stamps.sort()
|
||||
second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
|
||||
second_xyz_full_aligned = rot * second_xyz_full + trans
|
||||
|
||||
if args.verbose:
|
||||
print "compared_pose_pairs %d pairs"%(len(trans_error))
|
||||
|
||||
print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)
|
||||
print "absolute_translational_error.median %f m"%numpy.median(trans_error)
|
||||
print "absolute_translational_error.std %f m"%numpy.std(trans_error)
|
||||
print "absolute_translational_error.min %f m"%numpy.min(trans_error)
|
||||
print "absolute_translational_error.max %f m"%numpy.max(trans_error)
|
||||
else:
|
||||
print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
|
||||
if args.save_associations:
|
||||
file = open(args.save_associations,"w")
|
||||
file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))
|
||||
file.close()
|
||||
|
||||
if args.save:
|
||||
file = open(args.save,"w")
|
||||
file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))
|
||||
file.close()
|
||||
|
||||
if args.plot:
|
||||
import matplotlib
|
||||
matplotlib.use('Agg')
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.pylab as pylab
|
||||
from matplotlib.patches import Ellipse
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111)
|
||||
plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")
|
||||
plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")
|
||||
|
||||
label="difference"
|
||||
for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
|
||||
ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
|
||||
label=""
|
||||
|
||||
ax.legend()
|
||||
|
||||
ax.set_xlabel('x [m]')
|
||||
ax.set_ylabel('y [m]')
|
||||
plt.savefig(args.plot,dpi=90)
|
||||
|
||||
+186
-159
@@ -1,25 +1,92 @@
|
||||
#!/usr/bin/python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (c) 2013, Juergen Sturm, TUM
|
||||
# All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above
|
||||
# copyright notice, this list of conditions and the following
|
||||
# disclaimer in the documentation and/or other materials provided
|
||||
# with the distribution.
|
||||
# * Neither the name of TUM nor the names of its
|
||||
# contributors may be used to endorse or promote products derived
|
||||
# from this software without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
"""
|
||||
This script computes the relative pose error from the ground truth trajectory
|
||||
and the estimated trajectory.
|
||||
"""
|
||||
|
||||
import yaml
|
||||
import argparse
|
||||
import random
|
||||
import numpy
|
||||
import sys
|
||||
import transformations
|
||||
import matplotlib
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.pylab as pylab
|
||||
from matplotlib import rc
|
||||
|
||||
_EPS = numpy.finfo(float).eps * 4.0
|
||||
|
||||
def transform44(l):
|
||||
"""
|
||||
Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion.
|
||||
|
||||
Input:
|
||||
l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where
|
||||
(tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion.
|
||||
|
||||
Output:
|
||||
matrix -- 4x4 homogeneous transformation matrix
|
||||
"""
|
||||
t = l[1:4]
|
||||
q = l[4:8]
|
||||
return numpy.dot(transformations.translation_matrix(t),transformations.quaternion_matrix(q))
|
||||
q = numpy.array(l[4:8], dtype=numpy.float64, copy=True)
|
||||
nq = numpy.dot(q, q)
|
||||
if nq < _EPS:
|
||||
return numpy.array((
|
||||
( 1.0, 0.0, 0.0, t[0])
|
||||
( 0.0, 1.0, 0.0, t[1])
|
||||
( 0.0, 0.0, 1.0, t[2])
|
||||
( 0.0, 0.0, 0.0, 1.0)
|
||||
), dtype=numpy.float64)
|
||||
q *= numpy.sqrt(2.0 / nq)
|
||||
q = numpy.outer(q, q)
|
||||
return numpy.array((
|
||||
(1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], t[0]),
|
||||
( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], t[1]),
|
||||
( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], t[2]),
|
||||
( 0.0, 0.0, 0.0, 1.0)
|
||||
), dtype=numpy.float64)
|
||||
|
||||
def read_trajectory(filename, matrix=True):
|
||||
"""
|
||||
Read a trajectory from a text file.
|
||||
|
||||
Input:
|
||||
filename -- file to be read
|
||||
matrix -- convert poses to 4x4 matrices
|
||||
|
||||
Output:
|
||||
dictionary of stamped 3D poses
|
||||
"""
|
||||
file = open(filename)
|
||||
data = file.read()
|
||||
lines = data.replace(","," ").replace("\t"," ").split("\n")
|
||||
lines = data.replace(","," ").replace("\t"," ").split("\n")
|
||||
list = [[float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
|
||||
list_ok = []
|
||||
for i,l in enumerate(list):
|
||||
@@ -27,7 +94,7 @@ def read_trajectory(filename, matrix=True):
|
||||
continue
|
||||
isnan = False
|
||||
for v in l:
|
||||
if numpy.isnan(v):
|
||||
if numpy.isnan(v):
|
||||
isnan = True
|
||||
break
|
||||
if isnan:
|
||||
@@ -41,6 +108,16 @@ def read_trajectory(filename, matrix=True):
|
||||
return traj
|
||||
|
||||
def find_closest_index(L,t):
|
||||
"""
|
||||
Find the index of the closest value in a list.
|
||||
|
||||
Input:
|
||||
L -- the list
|
||||
t -- value to be found
|
||||
|
||||
Output:
|
||||
index of the closest element
|
||||
"""
|
||||
beginning = 0
|
||||
difference = abs(L[0] - t)
|
||||
best = 0
|
||||
@@ -59,9 +136,22 @@ def find_closest_index(L,t):
|
||||
return best
|
||||
|
||||
def ominus(a,b):
|
||||
"""
|
||||
Compute the relative 3D transformation between a and b.
|
||||
|
||||
Input:
|
||||
a -- first pose (homogeneous 4x4 matrix)
|
||||
b -- second pose (homogeneous 4x4 matrix)
|
||||
|
||||
Output:
|
||||
Relative 3D transformation from a to b.
|
||||
"""
|
||||
return numpy.dot(numpy.linalg.inv(a),b)
|
||||
|
||||
def scale(a,scalar):
|
||||
"""
|
||||
Scale the translational components of a 4x4 homogeneous matrix by a scale factor.
|
||||
"""
|
||||
return numpy.array(
|
||||
[[a[0,0], a[0,1], a[0,2], a[0,3]*scalar],
|
||||
[a[1,0], a[1,1], a[1,2], a[1,3]*scalar],
|
||||
@@ -70,13 +160,22 @@ def scale(a,scalar):
|
||||
)
|
||||
|
||||
def compute_distance(transform):
|
||||
"""
|
||||
Compute the distance of the translational component of a 4x4 homogeneous matrix.
|
||||
"""
|
||||
return numpy.linalg.norm(transform[0:3,3])
|
||||
|
||||
def compute_angle(transform):
|
||||
"""
|
||||
Compute the rotation angle from a 4x4 homogeneous matrix.
|
||||
"""
|
||||
# an invitation to 3-d vision, p 27
|
||||
return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) ))
|
||||
|
||||
def distances_along_trajectory(traj):
|
||||
"""
|
||||
Compute the translational distances along a trajectory.
|
||||
"""
|
||||
keys = traj.keys()
|
||||
keys.sort()
|
||||
motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
|
||||
@@ -86,8 +185,11 @@ def distances_along_trajectory(traj):
|
||||
sum += compute_distance(t)
|
||||
distances.append(sum)
|
||||
return distances
|
||||
|
||||
|
||||
def rotations_along_trajectory(traj,scale):
|
||||
"""
|
||||
Compute the angular rotations along a trajectory.
|
||||
"""
|
||||
keys = traj.keys()
|
||||
keys.sort()
|
||||
motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
|
||||
@@ -97,60 +199,36 @@ def rotations_along_trajectory(traj,scale):
|
||||
sum += compute_angle(t)*scale
|
||||
distances.append(sum)
|
||||
return distances
|
||||
|
||||
|
||||
def interpolate_transformation(alpha,a,b):
|
||||
qa,ta = transformations.quaternion_from_matrix(a),transformations.translation_from_matrix(a)
|
||||
qb,tb = transformations.quaternion_from_matrix(b),transformations.translation_from_matrix(b)
|
||||
qr = transformations.quaternion_slerp(qa,qb,alpha)
|
||||
tr = (1-alpha) * ta + (alpha) * tb
|
||||
r = numpy.dot(transformations.translation_matrix(tr),transformations.quaternion_matrix(qr))
|
||||
return r
|
||||
|
||||
def interpolate_and_resample(traj_gt,stamps_est,param_offset,gt_max_time_difference):
|
||||
traj_new = {}
|
||||
|
||||
def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00):
|
||||
"""
|
||||
Compute the relative pose error between two trajectories.
|
||||
|
||||
Input:
|
||||
traj_gt -- the first trajectory (ground truth)
|
||||
traj_est -- the second trajectory (estimated trajectory)
|
||||
param_max_pairs -- number of relative poses to be evaluated
|
||||
param_fixed_delta -- false: evaluate over all possible pairs
|
||||
true: only evaluate over pairs with a given distance (delta)
|
||||
param_delta -- distance between the evaluated pairs
|
||||
param_delta_unit -- unit for comparison:
|
||||
"s": seconds
|
||||
"m": meters
|
||||
"rad": radians
|
||||
"deg": degrees
|
||||
"f": frames
|
||||
param_offset -- time offset between two trajectories (to model the delay)
|
||||
param_scale -- scale to be applied to the second trajectory
|
||||
|
||||
Output:
|
||||
list of compared poses and the resulting translation and rotation error
|
||||
"""
|
||||
stamps_gt = list(traj_gt.keys())
|
||||
stamps_gt.sort()
|
||||
|
||||
for t_est in stamps_est:
|
||||
t_new = t_est + param_offset
|
||||
i_nearest = find_closest_index(stamps_gt,t_est + param_offset)
|
||||
if stamps_gt[i_nearest] >= t_new and i_nearest>0:
|
||||
i_low = i_nearest-1
|
||||
i_high = i_nearest
|
||||
else:
|
||||
i_low = i_nearest
|
||||
i_high = i_nearest+1
|
||||
if i_low<0 or i_high>=len(stamps_gt):
|
||||
continue
|
||||
|
||||
t_low = stamps_gt[i_low]
|
||||
t_high = stamps_gt[i_high]
|
||||
|
||||
if(abs( t_new - (t_low + param_offset) ) > gt_max_time_difference or
|
||||
abs( t_new - (t_high + param_offset) ) > gt_max_time_difference):
|
||||
continue
|
||||
|
||||
alpha = (t_new - (t_low + param_offset)) / (t_high - t_low)
|
||||
transform = interpolate_transformation(alpha,traj_gt[t_low],traj_gt[t_high])
|
||||
traj_new[t_new] = transform
|
||||
return traj_new
|
||||
|
||||
|
||||
def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00,param_interpolation=True):
|
||||
stamps_est = list(traj_est.keys())
|
||||
stamps_est.sort()
|
||||
stamps_gt = list(traj_gt.keys())
|
||||
stamps_gt.sort()
|
||||
|
||||
gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])])
|
||||
gt_max_time_difference = 2*gt_interval
|
||||
|
||||
if param_interpolation:
|
||||
traj_gt = interpolate_and_resample(traj_gt,stamps_est,param_offset,gt_max_time_difference)
|
||||
stamps_gt = list(traj_gt.keys())
|
||||
stamps_gt.sort()
|
||||
|
||||
stamps_est.sort()
|
||||
|
||||
stamps_est_return = []
|
||||
for t_est in stamps_est:
|
||||
t_gt = stamps_gt[find_closest_index(stamps_gt,t_est + param_offset)]
|
||||
@@ -184,11 +262,14 @@ def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta
|
||||
pairs = []
|
||||
for i in range(len(traj_est)):
|
||||
j = find_closest_index(index_est,index_est[i] + param_delta)
|
||||
if j!=len(traj_est)-1:
|
||||
if j!=len(traj_est)-1:
|
||||
pairs.append((i,j))
|
||||
if(param_max_pairs!=0 and len(pairs)>param_max_pairs):
|
||||
pairs = random.sample(pairs,param_max_pairs)
|
||||
|
||||
|
||||
gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])])
|
||||
gt_max_time_difference = 2*gt_interval
|
||||
|
||||
result = []
|
||||
for i,j in pairs:
|
||||
stamp_est_0 = stamps_est[i]
|
||||
@@ -196,32 +277,29 @@ def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta
|
||||
|
||||
stamp_gt_0 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_0 + param_offset) ]
|
||||
stamp_gt_1 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_1 + param_offset) ]
|
||||
|
||||
|
||||
if(abs(stamp_gt_0 - (stamp_est_0 + param_offset)) > gt_max_time_difference or
|
||||
abs(stamp_gt_1 - (stamp_est_1 + param_offset)) > gt_max_time_difference):
|
||||
continue
|
||||
|
||||
|
||||
error44 = ominus( scale(
|
||||
ominus( traj_est[stamp_est_1], traj_est[stamp_est_0] ),param_scale),
|
||||
ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] ) )
|
||||
|
||||
|
||||
trans = compute_distance(error44)
|
||||
rot = compute_angle(error44)
|
||||
|
||||
motion_gt = ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] )
|
||||
trans_gt = compute_distance(motion_gt)
|
||||
rot_gt = compute_angle(motion_gt)
|
||||
|
||||
if(rot < 0.07):
|
||||
result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot,trans_gt,rot_gt])
|
||||
|
||||
result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot])
|
||||
|
||||
if len(result)<2:
|
||||
raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!")
|
||||
|
||||
|
||||
return result
|
||||
|
||||
def percentile(seq,q):
|
||||
"""
|
||||
Return the q-percentile of a list
|
||||
"""
|
||||
seq_sorted = list(seq)
|
||||
seq_sorted.sort()
|
||||
return seq_sorted[int((len(seq_sorted)-1)*q)]
|
||||
@@ -229,45 +307,28 @@ def percentile(seq,q):
|
||||
if __name__ == '__main__':
|
||||
random.seed(0)
|
||||
|
||||
# config -------------------------------------------------------------------------
|
||||
|
||||
dataset='20131003_1937_nslam_i7_asl2_fast'
|
||||
dataset_dir = '../results/'+dataset
|
||||
|
||||
#---------------------------------------------------------------------------------
|
||||
|
||||
# load dataset parameters
|
||||
params_stream = open(dataset_dir+'/params.yaml')
|
||||
params = yaml.load(params_stream)
|
||||
|
||||
# set trajectory groundtruth and estimate file
|
||||
traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
|
||||
traj_estimate_rotated = dataset_dir+'/traj_estimate_rotated.txt'
|
||||
|
||||
parser = argparse.ArgumentParser(description='''
|
||||
This script computes the relative pose error from the ground truth trajectory and the estimated trajectory.
|
||||
This script computes the relative pose error from the ground truth trajectory and the estimated trajectory.
|
||||
''')
|
||||
parser.add_argument('--groundtruth_file', help='ground-truth trajectory file (format: "timestamp tx ty tz qx qy qz qw")', default=traj_groundtruth)
|
||||
parser.add_argument('--estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")', default=traj_estimate_rotated)
|
||||
parser.add_argument('--interpolate', help='interpolate and resample the ground truth trajectory', default=True)
|
||||
parser.add_argument('groundtruth_file', help='ground-truth trajectory file (format: "timestamp tx ty tz qx qy qz qw")')
|
||||
parser.add_argument('estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")')
|
||||
parser.add_argument('--max_pairs', help='maximum number of pose comparisons (default: 10000, set to zero to disable downsampling)', default=10000)
|
||||
parser.add_argument('--fixed_delta', help='only consider pose pairs that have a distance of delta delta_unit (e.g., for evaluating the drift per second/meter/radian)', default=True)
|
||||
parser.add_argument('--fixed_delta', help='only consider pose pairs that have a distance of delta delta_unit (e.g., for evaluating the drift per second/meter/radian)', action='store_true')
|
||||
parser.add_argument('--delta', help='delta for evaluation (default: 1.0)',default=1.0)
|
||||
parser.add_argument('--delta_unit', help='unit of delta (options: \'s\' for seconds, \'m\' for meters, \'rad\' for radians, \'f\' for frames; default: \'s\')',default='s')
|
||||
parser.add_argument('--offset', help='time offset between ground-truth and estimated trajectory (default: 0.0)',default=0.0)
|
||||
parser.add_argument('--scale', help='scaling factor for the estimated trajectory (default: 1.0)',default=1.0)
|
||||
parser.add_argument('--save', help='text file to which the evaluation will be saved (format: stamp_est0 stamp_est1 stamp_gt0 stamp_gt1 trans_error rot_error)')
|
||||
parser.add_argument('--plot', help='plot the result to a file (requires --fixed_delta, output format: png)', default=True)
|
||||
parser.add_argument('--plotname', help='plot the result to a file (requires --fixed_delta, output format: png)', default='rpe.png')
|
||||
parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', default=True)
|
||||
parser.add_argument('--plot', help='plot the result to a file (requires --fixed_delta, output format: png)')
|
||||
parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', action='store_true')
|
||||
args = parser.parse_args()
|
||||
|
||||
|
||||
if args.plot and not args.fixed_delta:
|
||||
sys.exit("The '--plot' option can only be used in combination with '--fixed_delta'")
|
||||
|
||||
|
||||
traj_gt = read_trajectory(args.groundtruth_file)
|
||||
traj_est = read_trajectory(args.estimated_file)
|
||||
|
||||
|
||||
result = evaluate_trajectory(traj_gt,
|
||||
traj_est,
|
||||
int(args.max_pairs),
|
||||
@@ -275,81 +336,47 @@ if __name__ == '__main__':
|
||||
float(args.delta),
|
||||
args.delta_unit,
|
||||
float(args.offset),
|
||||
float(args.scale),
|
||||
args.interpolate)
|
||||
|
||||
float(args.scale))
|
||||
|
||||
stamps = numpy.array(result)[:,0]
|
||||
trans_error = numpy.array(result)[:,4]
|
||||
rot_error = numpy.array(result)[:,5]
|
||||
|
||||
|
||||
results = dict()
|
||||
results['trans_rmse'] = float(numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)))
|
||||
results['trans_mean'] = float(numpy.mean(trans_error))
|
||||
results['trans_median'] = float(numpy.median(trans_error))
|
||||
results['trans_std'] = float(numpy.std(trans_error))
|
||||
results['trans_min'] = float(numpy.min(trans_error))
|
||||
results['trans_max'] = float(numpy.max(trans_error))
|
||||
results['rot_rmse'] = float((numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi))
|
||||
results['rot_mean'] = float((numpy.mean(rot_error) * 180.0 / numpy.pi))
|
||||
results['rot_median'] = float(numpy.median(rot_error) * 180.0 / numpy.pi)
|
||||
results['rot_std'] = float((numpy.std(rot_error) * 180.0 / numpy.pi))
|
||||
results['rot_min'] = float((numpy.min(rot_error) * 180.0 / numpy.pi))
|
||||
results['rot_max'] = float((numpy.max(rot_error) * 180.0 / numpy.pi))
|
||||
|
||||
# dump experiment results to file:
|
||||
results_file = params['trace_dir']+'/rpe_results.yaml'
|
||||
with open(results_file,'w') as outfile:
|
||||
outfile.write( yaml.dump(results, default_flow_style=False) )
|
||||
|
||||
if args.save:
|
||||
if args.save:
|
||||
f = open(args.save,"w")
|
||||
f.write("\n".join([" ".join(["%f"%v for v in line]) for line in result]))
|
||||
f.close()
|
||||
|
||||
|
||||
if args.verbose:
|
||||
print "compared_pose_pairs %d pairs"%(len(trans_error))
|
||||
|
||||
print "translational_error.rmse %f m"%results['trans_rmse']
|
||||
print "translational_error.mean %f m"%results['trans_mean']
|
||||
print "translational_error.median %f m"%results['trans_median']
|
||||
print "translational_error.std %f m"%results['trans_std']
|
||||
print "translational_error.min %f m"%results['trans_min']
|
||||
print "translational_error.max %f m"%results['trans_max']
|
||||
print "translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
|
||||
print "translational_error.mean %f m"%numpy.mean(trans_error)
|
||||
print "translational_error.median %f m"%numpy.median(trans_error)
|
||||
print "translational_error.std %f m"%numpy.std(trans_error)
|
||||
print "translational_error.min %f m"%numpy.min(trans_error)
|
||||
print "translational_error.max %f m"%numpy.max(trans_error)
|
||||
|
||||
print "rotational_error.rmse %f deg"%results['rot_rmse']
|
||||
print "rotational_error.mean %f deg"%results['rot_mean']
|
||||
print "rotational_error.median %f deg"%results['rot_median']
|
||||
print "rotational_error.std %f deg"%results['rot_std']
|
||||
print "rotational_error.min %f deg"%results['rot_min']
|
||||
print "rotational_error.max %f deg"%results['rot_max']
|
||||
print "rotational_error.rmse %f deg"%(numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi)
|
||||
print "rotational_error.mean %f deg"%(numpy.mean(rot_error) * 180.0 / numpy.pi)
|
||||
print "rotational_error.median %f deg"%numpy.median(rot_error)
|
||||
print "rotational_error.std %f deg"%(numpy.std(rot_error) * 180.0 / numpy.pi)
|
||||
print "rotational_error.min %f deg"%(numpy.min(rot_error) * 180.0 / numpy.pi)
|
||||
print "rotational_error.max %f deg"%(numpy.max(rot_error) * 180.0 / numpy.pi)
|
||||
else:
|
||||
print numpy.mean(trans_error)
|
||||
|
||||
if args.plot:
|
||||
|
||||
# tell matplotlib to use latex font
|
||||
rc('font',**{'family':'serif','serif':['Cardo']})
|
||||
rc('text', usetex=True)
|
||||
|
||||
#matplotlib.use('Agg') # no window should appear?
|
||||
|
||||
fig = plt.figure(figsize=(8, 3))
|
||||
ax = fig.add_subplot(111)
|
||||
if args.plot:
|
||||
import matplotlib
|
||||
matplotlib.use('Agg')
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.pylab as pylab
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111)
|
||||
ax.plot(stamps - stamps[0],trans_error,'-',color="blue")
|
||||
#ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red")
|
||||
ax.set_xlabel('time [s]')
|
||||
ax.set_ylabel('translational error [m]')
|
||||
plt.tight_layout()
|
||||
plt.savefig(dataset_dir+'/rpe_trans_error.pdf')
|
||||
plt.show()
|
||||
plt.savefig(args.plot,dpi=300)
|
||||
|
||||
|
||||
fig = plt.figure(figsize=(8, 3))
|
||||
ax = fig.add_subplot(111)
|
||||
ax.plot(stamps - stamps[0],rot_error* 180.0 / numpy.pi,'-',color="blue")
|
||||
#ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red")
|
||||
ax.set_xlabel('time [s]')
|
||||
ax.set_ylabel('rot error [deg]')
|
||||
plt.tight_layout()
|
||||
plt.savefig(dataset_dir+'/rpe_rot_error.pdf')
|
||||
plt.show()
|
||||
Reference in New Issue
Block a user