tracing and plotting of pose error for synthetic datasets
This commit is contained in:
Christian Forster
+1
-1
@@ -2,7 +2,7 @@
|
||||
# user build settings
|
||||
|
||||
SET(DEBUG FALSE)
|
||||
SET(TRACE FALSE)
|
||||
SET(TRACE TRUE)
|
||||
SET(HAVE_G2O FALSE)
|
||||
|
||||
################################################################################
|
||||
|
||||
@@ -10,6 +10,7 @@ import yaml
|
||||
import rospkg
|
||||
import argparse
|
||||
import time
|
||||
import svo_analysis.analyse_trajectory as analyse_trajectory
|
||||
import vikit_py.cpu_info as cpu_info
|
||||
import vikit_py.ros_node as ros_node
|
||||
|
||||
@@ -29,7 +30,7 @@ if __name__=="__main__":
|
||||
'experiments', args.experiment_file+'.yaml')
|
||||
expParams = yaml.load(open(expParamsFile, 'r'))
|
||||
expParams['time'] = time.strftime("%Y%m%d_%H%M", time.localtime())
|
||||
#expParams['platform'] = cpu_info.getCpuInfo()
|
||||
expParams['platform'] = cpu_info.get_cpu_info()
|
||||
expParams['experiment_name'] = args.name
|
||||
if(expParams['experiment_name'] == None):
|
||||
expParams['experiment_name'] = expParams['time']+'_svo_' + args.experiment_file
|
||||
@@ -61,9 +62,16 @@ if __name__=="__main__":
|
||||
with open(params_dump_file,'w') as outfile:
|
||||
outfile.write(yaml.dump(params, default_flow_style=False))
|
||||
|
||||
# start ros node
|
||||
# execute ros node
|
||||
node = ros_node.RosNode('svo_ros','benchmark')
|
||||
node.run(params)
|
||||
|
||||
# TODO: check if it is a synthetic dataset
|
||||
# plot trajectory error
|
||||
analyse_trajectory.analyse_trajectory(params['trace_dir'])
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,2 +0,0 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
|
||||
@@ -4,7 +4,6 @@ import associate
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import yaml
|
||||
import transformations
|
||||
|
||||
def loadDataset(filename):
|
||||
file = open(filename)
|
||||
|
||||
@@ -6,6 +6,10 @@ import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib import rc
|
||||
|
||||
# tell matplotlib to use latex font
|
||||
rc('font',**{'family':'serif','serif':['Cardo']})
|
||||
rc('text', usetex=True)
|
||||
|
||||
def analyse_logs(D, trace_name):
|
||||
|
||||
# identify measurements which result from normal frames and which from keyframes
|
||||
@@ -124,9 +128,5 @@ if __name__=="__main__":
|
||||
for field, value in D.items():
|
||||
D[field] = np.array(D[field])
|
||||
|
||||
# tell matplotlib to use latex font
|
||||
rc('font',**{'family':'serif','serif':['Cardo']})
|
||||
rc('text', usetex=True)
|
||||
|
||||
analyse_logs(D, args.trace_name)
|
||||
|
||||
|
||||
@@ -1,43 +1,52 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import associate
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import os
|
||||
import yaml
|
||||
import transformations
|
||||
from .tum_benchmark_tools import associate
|
||||
import matplotlib.pyplot as plt
|
||||
import vikit_py.transformations as transformations
|
||||
import vikit_py.align_trajectory as align_trajectory
|
||||
import numpy as np
|
||||
|
||||
# plot options
|
||||
from matplotlib import rc
|
||||
rc('font',**{'family':'serif','serif':['Cardo']})
|
||||
rc('text', usetex=True)
|
||||
|
||||
def alignSim3(model, data, n):
|
||||
def plot_translation_error(timestamps, translation_error, results_dir):
|
||||
fig = plt.figure(figsize=(8, 2.5))
|
||||
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [mm]', xlim=[0,timestamps[-1]-timestamps[0]+4])
|
||||
ax.plot(timestamps-timestamps[0], translation_error[:,0]*1000, 'r-', label='x')
|
||||
ax.plot(timestamps-timestamps[0], translation_error[:,1]*1000, 'g-', label='y')
|
||||
ax.plot(timestamps-timestamps[0], translation_error[:,2]*1000, 'b-', label='z')
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
fig.savefig(results_dir+'/translation_error.pdf')
|
||||
|
||||
# select the first n datapoints and remove its mean so their center is zero
|
||||
M = model[0:n,:]
|
||||
D = data[0:n,:]
|
||||
def plot_rotation_error(timestamps, rotation_error, results_dir):
|
||||
fig = plt.figure(figsize=(8, 2.5))
|
||||
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,timestamps[-1]-timestamps[0]+4])
|
||||
ax.plot(timestamps-timestamps[0], rotation_error[:,0], 'r-', label='yaw')
|
||||
ax.plot(timestamps-timestamps[0], rotation_error[:,1], 'g-', label='pitch')
|
||||
ax.plot(timestamps-timestamps[0], rotation_error[:,2], 'b-', label='roll')
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
fig.savefig(results_dir+'/orientation_error.pdf')
|
||||
|
||||
# substract mean
|
||||
mu_M = M.mean(0)
|
||||
mu_D = D.mean(0)
|
||||
M_zerocentered = M - mu_M
|
||||
D_zerocentered = D - mu_D
|
||||
def analyse_trajectory(results_dir):
|
||||
# plot translation error
|
||||
data = np.loadtxt(os.path.join(results_dir, 'translation_error.txt'))
|
||||
timestamps = data[:,0]
|
||||
translation_error = data[:,1:4]
|
||||
plot_translation_error(timestamps, translation_error, results_dir)
|
||||
|
||||
# correlation
|
||||
C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
|
||||
sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
|
||||
U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
|
||||
D_svd = np.diag(D_svd)
|
||||
V_svd = np.transpose(V_svd)
|
||||
S = np.eye(3)
|
||||
# plot orientation error
|
||||
data = np.loadtxt(os.path.join(results_dir, 'orientation_error.txt'))
|
||||
timestamps = data[:,0]
|
||||
orientation_error = data[:,1:4]
|
||||
plot_rotation_error(timestamps, orientation_error, results_dir)
|
||||
|
||||
if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
|
||||
S[2,2] = -1
|
||||
|
||||
R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
|
||||
s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
|
||||
t = mu_M-s*np.dot(R,mu_D)
|
||||
|
||||
return s, R, t
|
||||
|
||||
|
||||
def getRigidBodyTrafo(quat,trans):
|
||||
def get_rigid_body_trafo(quat,trans):
|
||||
T = transformations.quaternion_matrix(quat)
|
||||
T[0:3,3] = trans
|
||||
return T
|
||||
@@ -50,10 +59,6 @@ if __name__ == '__main__':
|
||||
save = True
|
||||
results_dir = '../results/'+dataset
|
||||
|
||||
# plot options
|
||||
rc('font',**{'family':'serif','serif':['Cardo']})
|
||||
rc('text', usetex=True)
|
||||
|
||||
# load dataset parameters
|
||||
params_stream = open(results_dir+'/params.yaml')
|
||||
params = yaml.load(params_stream)
|
||||
@@ -72,7 +77,7 @@ if __name__ == '__main__':
|
||||
P = yaml.load(dataset_param_file_stream)
|
||||
T_cm_quat = np.array([P['calib_Tcm_qx'], P['calib_Tcm_qy'], P['calib_Tcm_qz'], P['calib_Tcm_qw']])
|
||||
T_cm_tran = np.array([P['calib_Tcm_tx'], P['calib_Tcm_ty'], P['calib_Tcm_tz']])
|
||||
T_cm = getRigidBodyTrafo(T_cm_quat, T_cm_tran)
|
||||
T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
|
||||
T_mc = transformations.inverse_matrix(T_cm)
|
||||
|
||||
# load data
|
||||
@@ -94,12 +99,12 @@ if __name__ == '__main__':
|
||||
# align Sim3 to get scale
|
||||
if(n_align_frames > np.shape(matches)[0]):
|
||||
raise NameError('n_matches_for_scale is too large')
|
||||
scale,rot,trans = alignSim3(p_gt, p_es, n_align_frames)
|
||||
scale,rot,trans = align_trajectory.align_sim3(p_gt, p_es, n_align_frames)
|
||||
print 'scale = '+str(scale)
|
||||
|
||||
# get trafo between (v)ision and (o)ptitrack frame
|
||||
T_om = getRigidBodyTrafo(q_gt[0,:], p_gt[0,:])
|
||||
T_vc = getRigidBodyTrafo(q_es[0,:], scale*p_es[0,:])
|
||||
T_om = get_rigid_body_trafo(q_gt[0,:], p_gt[0,:])
|
||||
T_vc = get_rigid_body_trafo(q_es[0,:], scale*p_es[0,:])
|
||||
T_cv = transformations.inverse_matrix(T_vc)
|
||||
T_ov = np.dot(T_om, np.dot(T_mc, T_cv))
|
||||
print 'T_ov = ' + str(T_ov)
|
||||
@@ -111,7 +116,7 @@ if __name__ == '__main__':
|
||||
|
||||
# rotate trajectory
|
||||
for i in range(np.shape(matches)[0]):
|
||||
T_vc = getRigidBodyTrafo(q_es[i,:],p_es[i,:])
|
||||
T_vc = get_rigid_body_trafo(q_es[i,:],p_es[i,:])
|
||||
T_vc[0:3,3] *= scale
|
||||
T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
|
||||
p_es_aligned[i,:] = T_om[0:3,3]
|
||||
@@ -136,15 +141,9 @@ if __name__ == '__main__':
|
||||
|
||||
# plot position error (drift)
|
||||
translation_error = (p_gt-p_es_aligned)
|
||||
fig = plt.figure(figsize=(8, 2.5))
|
||||
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [m]', xlim=[0,t_es[-1]+4])
|
||||
ax.plot(t_es, translation_error[:,0], 'r-', label='x')
|
||||
ax.plot(t_es, translation_error[:,1], 'g-', label='y')
|
||||
ax.plot(t_es, translation_error[:,2], 'b-', label='z')
|
||||
fig.tight_layout()
|
||||
plot_translation_error(t_es, translation_error, results_dir)
|
||||
|
||||
if save:
|
||||
fig.savefig(results_dir+'/translation_error.pdf')
|
||||
f = open(translation_error_filename,'w')
|
||||
for i in range(translation_error.shape[0]):
|
||||
f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], translation_error[i,0], translation_error[i,1], translation_error[i,2]))
|
||||
@@ -152,16 +151,9 @@ if __name__ == '__main__':
|
||||
|
||||
# plot orientation error (drift)
|
||||
orientation_error = (rpy_gt - rpy_es_aligned)
|
||||
fig = plt.figure(figsize=(8, 2.5))
|
||||
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,t_es[-1]+4])
|
||||
ax.plot(t_es, orientation_error[:,0], 'r--', label='yaw')
|
||||
ax.plot(t_es, orientation_error[:,1], 'g--', label='pitch')
|
||||
ax.plot(t_es, orientation_error[:,2], 'b--', label='roll')
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
plot_rotation_error(t_es, orientation_error, results_dir)
|
||||
|
||||
if save:
|
||||
fig.savefig(results_dir+'/orientation_error.pdf')
|
||||
f = open(orientation_error_filename,'w')
|
||||
for i in range(orientation_error.shape[0]):
|
||||
f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], orientation_error[i,0], orientation_error[i,1], orientation_error[i,2]))
|
||||
|
||||
@@ -11,81 +11,6 @@ import associate
|
||||
import rotation_utils as ru
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
def alignSim3(model, data, n):
|
||||
|
||||
# select the first n datapoints and remove its mean so their center is zero
|
||||
M = model[0:n,:]
|
||||
D = data[0:n,:]
|
||||
|
||||
# substract mean
|
||||
mu_M = M.mean(0)
|
||||
mu_D = D.mean(0)
|
||||
M_zerocentered = M - mu_M
|
||||
D_zerocentered = D - mu_D
|
||||
|
||||
# correlation
|
||||
C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
|
||||
sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
|
||||
U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
|
||||
D_svd = np.diag(D_svd)
|
||||
V_svd = np.transpose(V_svd)
|
||||
S = np.eye(3)
|
||||
|
||||
if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
|
||||
S[2,2] = -1
|
||||
|
||||
R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
|
||||
s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
|
||||
t = mu_M-s*np.dot(R,mu_D)
|
||||
|
||||
return s, R, t
|
||||
|
||||
def matrixLog(A):
|
||||
theta = np.arccos((np.trace(A)-1.0)/2.0)
|
||||
log_theta = 0.5*theta/np.sin(theta) * (A - A.transpose())
|
||||
x = np.array([log_theta[2,1], log_theta[0,2], log_theta[1,0]])
|
||||
return x
|
||||
|
||||
def handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
|
||||
''' Implementation of the least squares solution described in the paper:
|
||||
Robot Sensor Calibration: Solving AX=XB on the Euclidean Group
|
||||
by Frank C. Park and Bryan J. Martin
|
||||
'''
|
||||
n = np.shape(I)[0]
|
||||
M = np.zeros([3,3])
|
||||
C = np.zeros([3*n, 3])
|
||||
b_A = np.zeros([3*n,1])
|
||||
b_B = np.zeros([3*n,1])
|
||||
for ix, i in enumerate(I):
|
||||
A1 = ru.quat2dcm(q_es[i,:])
|
||||
A2 = ru.quat2dcm(q_es[i+delta,:])
|
||||
A = np.dot(A1.transpose(), A2)
|
||||
B1 = ru.quat2dcm(q_gt[i,:])
|
||||
B2 = ru.quat2dcm(q_gt[i+delta,:])
|
||||
B = np.dot(B1.transpose(), B2)
|
||||
alpha = matrixLog(A)
|
||||
beta = matrixLog(B)
|
||||
M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha))
|
||||
C[3*ix:3*ix+3,:] = np.eye(3) - A
|
||||
b_A[3*ix:3*ix+3,0] = np.dot(np.transpose(A1), p_es[i+delta,:]-p_es[i,:])
|
||||
b_B[3*ix:3*ix+3,0] = np.dot(np.transpose(B1), p_gt[i+delta,:]-p_gt[i,:])
|
||||
|
||||
# compute rotation
|
||||
D,V = np.linalg.linalg.eig(np.dot(M.transpose(), M))
|
||||
Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])])
|
||||
Vinv = np.linalg.linalg.inv(V)
|
||||
X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose())))
|
||||
|
||||
# compute translation
|
||||
d = np.zeros([3*n,1])
|
||||
for i in range(n):
|
||||
d[3*i:3*i+3,:] = b_A[3*i:3*i+3,:] - np.dot(X, b_B[3*i:3*i+3,:])
|
||||
|
||||
b = np.dot(np.linalg.inv(np.dot(np.transpose(C),C)), np.dot(np.transpose(C),d))
|
||||
|
||||
return np.array(X),b
|
||||
|
||||
|
||||
# user config
|
||||
display = True
|
||||
dataset = '20130906_2149_ptam_i7_asl2'
|
||||
@@ -121,7 +46,7 @@ q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches]
|
||||
|
||||
# --------------------------------------------------------------------------------
|
||||
# align Sim3 to get scale
|
||||
scale,rot,trans = alignSim3(p_gt, p_es, np.shape(matches)[0])
|
||||
scale,rot,trans = align_sim3(p_gt, p_es, np.shape(matches)[0])
|
||||
p_es = scale*p_es
|
||||
#p_es = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans
|
||||
|
||||
@@ -142,7 +67,7 @@ if display:
|
||||
|
||||
# select random measurements
|
||||
I = np.array(rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0]
|
||||
R,b = handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta, True)
|
||||
R,b = hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta, True)
|
||||
|
||||
print 'quat = ' + str(ru.dcm2quat(R))
|
||||
print 'b = ' + str(b)
|
||||
|
||||
@@ -1,143 +0,0 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Fri Jun 28 10:42:37 2013
|
||||
|
||||
@author: cforster
|
||||
"""
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
# epsilon for testing whether a number is close to zero
|
||||
_EPS = np.finfo(float).eps * 4.0
|
||||
|
||||
# axis sequences for Euler angles
|
||||
_NEXT_AXIS = [1, 2, 0, 1]
|
||||
|
||||
# map axes strings to/from tuples of inner axis, parity, repetition, frame
|
||||
_AXES2TUPLE = {
|
||||
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
|
||||
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
|
||||
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
|
||||
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
|
||||
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
|
||||
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
|
||||
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
|
||||
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
|
||||
|
||||
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
|
||||
|
||||
|
||||
def quat2dcm(quaternion):
|
||||
"""Convert Quaternion [x y z w] to Direct Cosine Matrix representation """
|
||||
q = np.array(quaternion[:4], dtype=np.float64, copy=True)
|
||||
nq = np.dot(q, q)
|
||||
if nq < _EPS:
|
||||
return np.identity(4)
|
||||
q *= math.sqrt(2.0 / nq)
|
||||
q = np.outer(q, q)
|
||||
return np.array([ [1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3]],
|
||||
[ q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3]],
|
||||
[ q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1]]],
|
||||
dtype=np.float64)
|
||||
|
||||
def dcm2quat(matrix):
|
||||
"""Convert Direct Cosine Matrix to Quaternion
|
||||
|
||||
This algorithm comes from "Quaternion Calculus and Fast Animation",
|
||||
Ken Shoemake, 1987 SIGGRAPH course notes
|
||||
"""
|
||||
q = np.empty((4, ), dtype=np.float64)
|
||||
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
|
||||
t = np.trace(M)
|
||||
if (t > 0):
|
||||
t = math.sqrt(t + 1.0);
|
||||
q[3] = 0.5*t;
|
||||
t = 0.5/t;
|
||||
q[0] = (M[2,1] - M[1,2]) * t;
|
||||
q[1] = (M[0,2] - M[2,0]) * t;
|
||||
q[2] = (M[1,0] - M[0,1]) * t;
|
||||
else:
|
||||
i = 0;
|
||||
if (M[1,1] > M[0,0]):
|
||||
i = 1;
|
||||
if (M[2,2] > M[i,i]):
|
||||
i = 2;
|
||||
j = (i+1)%3;
|
||||
k = (j+1)%3;
|
||||
|
||||
t = math.sqrt(M[i,i]-M[j,j]-M[k,k] + 1.0);
|
||||
q[i] = 0.5 * t;
|
||||
t = 0.5/t;
|
||||
q[3] = (M[k,j]-M[j,k])*t;
|
||||
q[j] = (M[j,i]+M[i,j])*t;
|
||||
q[k] = (M[k,i]+M[i,k])*t;
|
||||
return q
|
||||
|
||||
def dcm2rpy(matrix, axes='sxyz'):
|
||||
"""Return Euler angles from rotation matrix for specified axis sequence.
|
||||
|
||||
axes : One of 24 axis sequences as string or encoded tuple
|
||||
|
||||
Note that many Euler angle triplets can describe one matrix.
|
||||
|
||||
>>> R0 = euler_matrix(1, 2, 3, 'syxz')
|
||||
>>> al, be, ga = euler_from_matrix(R0, 'syxz')
|
||||
>>> R1 = euler_matrix(al, be, ga, 'syxz')
|
||||
>>> numpy.allclose(R0, R1)
|
||||
True
|
||||
>>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
|
||||
>>> for axes in _AXES2TUPLE.keys():
|
||||
... R0 = euler_matrix(axes=axes, *angles)
|
||||
... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
|
||||
... if not numpy.allclose(R0, R1): print axes, "failed"
|
||||
|
||||
"""
|
||||
try:
|
||||
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
|
||||
except (AttributeError, KeyError):
|
||||
_ = _TUPLE2AXES[axes]
|
||||
firstaxis, parity, repetition, frame = axes
|
||||
|
||||
i = firstaxis
|
||||
j = _NEXT_AXIS[i+parity]
|
||||
k = _NEXT_AXIS[i-parity+1]
|
||||
|
||||
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
|
||||
if repetition:
|
||||
sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
|
||||
if sy > _EPS:
|
||||
ax = math.atan2( M[i, j], M[i, k])
|
||||
ay = math.atan2( sy, M[i, i])
|
||||
az = math.atan2( M[j, i], -M[k, i])
|
||||
else:
|
||||
ax = math.atan2(-M[j, k], M[j, j])
|
||||
ay = math.atan2( sy, M[i, i])
|
||||
az = 0.0
|
||||
else:
|
||||
cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
|
||||
if cy > _EPS:
|
||||
ax = math.atan2( M[k, j], M[k, k])
|
||||
ay = math.atan2(-M[k, i], cy)
|
||||
az = math.atan2( M[j, i], M[i, i])
|
||||
else:
|
||||
ax = math.atan2(-M[j, k], M[j, j])
|
||||
ay = math.atan2(-M[k, i], cy)
|
||||
az = 0.0
|
||||
|
||||
if parity:
|
||||
ax, ay, az = -ax, -ay, -az
|
||||
if frame:
|
||||
ax, az = az, ax
|
||||
return ax, ay, az
|
||||
|
||||
def quat2rpy(quaternion, axes='sxyz'):
|
||||
"""Return Euler angles from quaternion for specified axis sequence.
|
||||
|
||||
>>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
|
||||
>>> numpy.allclose(angles, [0.123, 0, 0])
|
||||
True
|
||||
|
||||
"""
|
||||
return dcm2rpy(quat2dcm(quaternion), axes)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -44,12 +44,15 @@ class BenchmarkNode
|
||||
svo::Visualizer visualizer_;
|
||||
int frame_count_;
|
||||
std::ofstream trace_est_pose_;
|
||||
std::ofstream trace_trans_error_;
|
||||
std::ofstream trace_rot_error_;
|
||||
vk::AbstractCamera* cam_;
|
||||
|
||||
public:
|
||||
BenchmarkNode(ros::NodeHandle& nh);
|
||||
~BenchmarkNode();
|
||||
void tracePose(const SE3& T_w_f, const double timestamp);
|
||||
void tracePoseError(const SE3& T_f_gt, const double timestamp);
|
||||
void runBenchmark(const std::string& dataset_dir);
|
||||
void runBlenderBenchmark(const std::string& dataset_dir);
|
||||
};
|
||||
@@ -91,6 +94,22 @@ void BenchmarkNode::tracePose(const SE3& T_w_f, const double timestamp)
|
||||
<< q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
|
||||
}
|
||||
|
||||
void BenchmarkNode::tracePoseError(const SE3& T_f_gt, const double timestamp)
|
||||
{
|
||||
Vector3d et(T_f_gt.translation()); // translation error
|
||||
trace_trans_error_.precision(15);
|
||||
trace_trans_error_.setf(std::ios::fixed, std::ios::floatfield );
|
||||
trace_trans_error_ << timestamp << " ";
|
||||
trace_trans_error_.precision(6);
|
||||
trace_trans_error_ << et.x() << " " << et.y() << " " << et.z() << " " << endl;
|
||||
Vector3d er(vk::dcm2rpy(T_f_gt.rotation_matrix())); // rotation error in roll-pitch-yaw
|
||||
trace_rot_error_.precision(15);
|
||||
trace_rot_error_.setf(std::ios::fixed, std::ios::floatfield );
|
||||
trace_rot_error_ << timestamp << " ";
|
||||
trace_rot_error_.precision(6);
|
||||
trace_rot_error_ << er.x() << " " << er.y() << " " << er.z() << " " << endl;
|
||||
}
|
||||
|
||||
void BenchmarkNode::runBenchmark(const std::string& dataset_dir)
|
||||
{
|
||||
// create image reader and load dataset
|
||||
@@ -140,6 +159,11 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
|
||||
std::vector<vk::blender_utils::file_format::ImageNameAndPose> dataset;
|
||||
dataset_reader.readAllEntries(dataset);
|
||||
|
||||
trace_trans_error_.open(Config::traceDir() + "/translation_error.txt");
|
||||
trace_rot_error_.open(Config::traceDir() + "/orientation_error.txt");
|
||||
if(trace_trans_error_.fail() || trace_rot_error_.fail())
|
||||
throw std::runtime_error("Could not create tracefile. Does folder exist?");
|
||||
|
||||
// process dataset
|
||||
for(auto it = dataset.begin(); it != dataset.end() && ros::ok(); ++it, ++frame_count_)
|
||||
{
|
||||
@@ -151,12 +175,15 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
|
||||
return;
|
||||
}
|
||||
|
||||
// Read ground-truth pose
|
||||
Sophus::SE3 T_w_gt(it->q_, it->t_);
|
||||
|
||||
// Set reference frame with depth
|
||||
if(frame_count_ == 0)
|
||||
{
|
||||
// set reference frame at ground-truth pose
|
||||
FramePtr frame_ref(new Frame(cam_, img, it->timestamp_));
|
||||
frame_ref->T_f_w_ = Sophus::SE3(it->q_, it->t_).inverse();
|
||||
frame_ref->T_f_w_ = T_w_gt.inverse();
|
||||
|
||||
// load ground-truth depth
|
||||
cv::Mat depthmap;
|
||||
@@ -189,8 +216,14 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
|
||||
// Add image to VO
|
||||
vo_->addImage(img, it->timestamp_);
|
||||
|
||||
if(vo_->stage() != svo::FrameHandlerMono::DEFAULT_FRAME)
|
||||
continue;
|
||||
|
||||
// Compute pose error and trace to file
|
||||
Sophus::SE3 T_f_gt(vo_->lastFrame()->T_f_w_*T_w_gt);
|
||||
tracePoseError(T_f_gt, it->timestamp_);
|
||||
|
||||
// write pose to tracefile
|
||||
if(vo_->stage() == svo::FrameHandlerMono::DEFAULT_FRAME)
|
||||
tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user