tracing and plotting of pose error for synthetic datasets

svo/CMakeLists.txt

@@ -2,7 +2,7 @@
 # user build settings
 
 SET(DEBUG FALSE)
-SET(TRACE FALSE)
+SET(TRACE TRUE)
 SET(HAVE_G2O FALSE) 
 
 ################################################################################

svo_analysis/scripts/benchmark.py

@@ -10,6 +10,7 @@
 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 @@
                                '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
@@ -43,7 +44,7 @@
 
   # load algorithm parameters
   algoParamsFile = os.path.join(rospkg.RosPack().get_path('svo_ros'), 
-                                   'param', expParams['param_settings']+'.yaml')
+                                'param', expParams['param_settings']+'.yaml')
   algoParams = yaml.load(open(algoParamsFile,'r'))
 
   # combine all parameters
@@ -61,9 +62,16 @@
   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'])
+
+
+
+
 
 

svo_analysis/src/svo_analysis/__init__.py

@@ -1,2 +0,0 @@
-# -*- coding: utf-8 -*-
-

svo_analysis/src/svo_analysis/analyse_dataset.py

@@ -4,7 +4,6 @@
 import numpy as np
 import matplotlib.pyplot as plt
 import yaml
-import transformations
 
 def loadDataset(filename):
   file = open(filename)

svo_analysis/src/svo_analysis/analyse_logs.py

@@ -6,6 +6,10 @@
 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 @@ def analyse_logs(D, trace_name):
   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)
 

svo_analysis/src/svo_analysis/analyse_trajectory.py

@@ -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 matplotlib import rc
-
-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
+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
 
-  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)
+# plot options
+from matplotlib import rc
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
 
-  return s, R, t
+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')
 
+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')
 
-def getRigidBodyTrafo(quat,trans):
+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)
+
+  # 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)
+
+def get_rigid_body_trafo(quat,trans):
   T = transformations.quaternion_matrix(quat)
   T[0:3,3] = trans
   return T
@@ -50,10 +59,6 @@ def getRigidBodyTrafo(quat,trans):
   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 @@ def getRigidBodyTrafo(quat,trans):
   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 @@ def getRigidBodyTrafo(quat,trans):
   # 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 @@ def getRigidBodyTrafo(quat,trans):
 
   # 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,32 +141,19 @@ def getRigidBodyTrafo(quat,trans):
 
   # 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]))
     f.close()
 
   # 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()
-
-  if save:
-    fig.savefig(results_dir+'/orientation_error.pdf')
+  plot_rotation_error(t_es, orientation_error, results_dir)
+
+  if save:  
     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]))

svo_analysis/src/svo_analysis/hand_eye_calib.py

@@ -11,81 +11,6 @@
 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 @@ def handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
 
 # --------------------------------------------------------------------------------
 # 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 @@ def handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
 
 # 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)