class TransformStampedListener:
td = TimedData(0)
posID = [1, 2, 3]
attID = [4, 5, 6, 7]
def __init__(self, td, topic, posID = [1, 2, 3], attID = [4, 5, 6, 7]):
self.td = td
self.posID = posID
self.attID = attID
rospy.Subscriber(topic, TransformStamped, self.callback)
def callback(self,msg):
self.td.append()
self.td.d[self.td.last, self.td.timeID] = msg.header.stamp.to_sec();
addTransformStamped(self.td, self.td.last, msg, self.posID, self.attID)
# Imports
import os, sys, inspect
from TimedData import TimedData
from Plotter import Plotter
import Quaternion
import Utils
import RosDataAcquisition
import numpy as np
td = TimedData()
td_aligned = TimedData()
# Setup options
testFolder = "/home/michael/datasets/rovioEval/test"
Utils.createFolderIfMissing(testFolder)
camera = "/home/michael/datasets/long_trajectory_leutiLynen/cam0.yaml"
odometryBag = "/home/michael/datasets/euroc/V1_03_difficult/okvis/okvis_paramest_V1_03_difficult.bag"
odometryTopic = "/okvis/okvis_node/okvis_odometry"
timestampBag = "/home/michael/datasets/euroc/V1_03_difficult/okvis/okvis_paramest_V1_03_difficult.bag"
timestampTopic = "/okvis/okvis_node/okvis_odometry"
R_BS = np.array(
[
0.0148655429818,
-0.999880929698,
0.00414029679422,
0.999557249008,
0.0149672133247,
0.025715529948,
-0.0257744366974,
0.00375618835797,
0.999660727178,
if plotRor:
plotterRor = Plotter(
-1, [3, 1], 'Rotational Rate', ['', '', 'time[s]'],
['$\omega_x$[rad/s]', '$\omega_y$[rad/s]', '$\omega_z$[rad/s]'],
10000)
if plotYpr:
plotterYpr = Plotter(-1, [3, 1], 'Yaw-Pitch-Roll Decomposition',
['', '', 'time[s]'],
['roll[rad]', 'pitch[rad]', 'yaw[rad]'], 10000)
if plotExt:
plotterExt = Plotter(-1, [3, 1], 'Extrinsics Translational Part',
['', '', 'time[s]'], ['x[m]', 'y[m]', 'z[m]'],
10000)
if doRovio: # Rovio Timed Data
td_rovio = TimedData()
nFeatures = 25
td_rovio.addLabelingIncremental('pos', 3)
td_rovio.addLabelingIncremental('att', 4)
td_rovio.addLabelingIncremental('vel', 3)
td_rovio.addLabelingIncremental('ror', 3)
td_rovio.addLabelingIncremental('ron', 1)
td_rovio.addLabelingIncremental('posCov', 9)
td_rovio.addLabelingIncremental('attCov', 9)
td_rovio.addLabelingIncremental('posSp', 3)
td_rovio.addLabelingIncremental('posSm', 3)
td_rovio.addLabelingIncremental('ypr', 3)
td_rovio.addLabelingIncremental('yprCov', 9)
td_rovio.addLabelingIncremental('yprSp', 3)
td_rovio.addLabelingIncremental('yprSm', 3)
td_rovio.addLabelingIncremental('extPos', 3)
TransformStamped message. Set isNode to true for that feature. """ isNode = False; if isNode is not True: """ Initialize Plotters plotter1: figureID=1, three subplots """ plotter1 = Plotter(1, [3,1]) """ Initialize TimedData td1: TimedData with 18 columns: 0:t, 1-3:pos, 4-7:att, 8-10:vel, 11-13:velInBodyFrame, 14-16:ror, 17:rorNorm td2: TimedData with 18 columns: 0:t, 1-4:att, 5-7:ror, 8:rorNorm, 9-11:pos, 12-14:vel, 15-17:velInBodyFrame """ td1 = TimedData(18) td2 = TimedData(18) posIDs1 = [1,2,3] posIDs2 = [9,10,11] attIDs1 = [4,5,6,7] attIDs2 = [1,2,3,4] velIDs1 = [8,9,10] velIDs2 = [12,13,14] velBIDs1 = [11,12,13] velBIDs2 = [15,16,17] rorIDs1 = [14,15,16] rorIDs2 = [5,6,7] rorNID1 = 17 rorNID2 = 8 """
if plotAtt:
plotterAtt = Plotter(-1, [4,1],'Attitude Quaternion',['','','','time[s]'],['w[1]','x[1]','y[1]','z[1]'],10000)
if plotPos:
plotterPos = Plotter(-1, [3,1],'Position',['','','time[s]'],['x[m]','y[m]','z[m]'],10000)
if plotVel:
plotterVel = Plotter(-1, [3,1],'Robocentric Velocity',['','','time[s]'],['$v_x$[m/s]','$v_y$[m/s]','$v_z$[m/s]'],10000)
if plotRor:
plotterRor = Plotter(-1, [3,1],'Rotational Rate',['','','time[s]'],['$\omega_x$[rad/s]','$\omega_y$[rad/s]','$\omega_z$[rad/s]'],10000)
if plotYpr:
plotterYpr = Plotter(-1, [3,1],'Yaw-Pitch-Roll Decomposition',['','','time[s]'],['roll[rad]','pitch[rad]','yaw[rad]'],10000)
if plotFea:
plotterFea = Plotter(-1, [3,1],'Feature Tracks',['','','time[s]'],['x[m]','y[m]','z[m]'],10000)
if doRovio: # Rovio Timed Data
nFeatures = 25
td_rovio = TimedData(58+nFeatures*4)
rovio_posID = [1,2,3]
rovio_attID = [4,5,6,7]
rovio_velID = [8,9,10]
rovio_rorID = [11,12,13]
rovio_ronID = 14
rovio_posCovID = np.arange(15,24)
rovio_attCovID = np.arange(24,33)
rovio_posSpID = [33,34,36]
rovio_posSmID = [37,38,39]
rovio_yprID = [40,41,42]
rovio_yprCovID = np.arange(43,52)
rovio_yprSpID = [52,53,54]
rovio_yprSmID = [55,56,57]
rovio_fea_posID = []
for i in np.arange(nFeatures):
plotterRon = Plotter(-1, [1,1],'Norm of Rotational Rate',['time [s]'],['Rotational Rate Norm [rad/s]'],10000)
if plotAtt:
plotterAtt = Plotter(-1, [4,1],'Attitude Quaternion',['','','','time[s]'],['w[1]','x[1]','y[1]','z[1]'],10000)
if plotPos:
plotterPos = Plotter(-1, [3,1],'Position',['','','time[s]'],['x[m]','y[m]','z[m]'],10000)
if plotVel:
plotterVel = Plotter(-1, [3,1],'Robocentric Velocity',['','','time[s]'],['$v_x$[m/s]','$v_y$[m/s]','$v_z$[m/s]'],10000)
if plotRor:
plotterRor = Plotter(-1, [3,1],'Rotational Rate',['','','time[s]'],['$\omega_x$[rad/s]','$\omega_y$[rad/s]','$\omega_z$[rad/s]'],10000)
if plotYpr:
plotterYpr = Plotter(-1, [3,1],'Yaw-Pitch-Roll Decomposition',['','','time[s]'],['roll[rad]','pitch[rad]','yaw[rad]'],10000)
if plotExt:
plotterExt = Plotter(-1, [3,1],'Extrinsics Translational Part',['','','time[s]'],['x[m]','y[m]','z[m]'],10000)
if doRovio: # Rovio Timed Data
td_rovio = TimedData()
nFeatures = 25
td_rovio.addLabelingIncremental('pos',3)
td_rovio.addLabelingIncremental('att',4)
td_rovio.addLabelingIncremental('vel',3)
td_rovio.addLabelingIncremental('ror',3)
td_rovio.addLabelingIncremental('ron',1)
td_rovio.addLabelingIncremental('posCov',9)
td_rovio.addLabelingIncremental('attCov',9)
td_rovio.addLabelingIncremental('posSp',3)
td_rovio.addLabelingIncremental('posSm',3)
td_rovio.addLabelingIncremental('ypr',3)
td_rovio.addLabelingIncremental('yprCov',9)
td_rovio.addLabelingIncremental('yprSp',3)
td_rovio.addLabelingIncremental('yprSm',3)
td_rovio.addLabelingIncremental('extPos',3)
# Imports
import os, sys, inspect
from TimedData import TimedData
from Plotter import Plotter
import Quaternion
import Utils
import RosDataAcquisition
import numpy as np
td = TimedData()
td_aligned = TimedData()
# Setup options
testFolder = '/home/michael/datasets/rovioEval/test'
Utils.createFolderIfMissing(testFolder)
camera = '/home/michael/datasets/long_trajectory_leutiLynen/cam0.yaml'
odometryBag = '/home/michael/datasets/euroc/V1_03_difficult/okvis/okvis_paramest_V1_03_difficult.bag'
odometryTopic = '/okvis/okvis_node/okvis_odometry'
timestampBag = '/home/michael/datasets/euroc/V1_03_difficult/okvis/okvis_paramest_V1_03_difficult.bag'
timestampTopic = '/okvis/okvis_node/okvis_odometry'
R_BS = np.array([
0.0148655429818, -0.999880929698, 0.00414029679422, 0.999557249008,
0.0149672133247, 0.025715529948, -0.0257744366974, 0.00375618835797,
0.999660727178
])
bodyTranslation = np.array(
[-0.0216401454975, -0.064676986768, 0.00981073058949])
bodyRotation = Quaternion.q_inverse(Quaternion.q_rotMatToQuat(R_BS))
inertialTranslation = None
inertialRotation = None
writeData = False
import os, sys, inspect from TimedData import TimedData from Plotter import Plotter from VIEvaluator import VIEvaluator import Quaternion import numpy as np plotRon = False plotAtt = False plotPos = True plotVel = True plotRor = False plotYpr = True plotExt = True td_rovio = TimedData() td_vicon = TimedData() rovioEvaluator = VIEvaluator() rovioEvaluator.bag = '/home/michael/datasets/bloesch_leo/planar_1/rovio/2016-02-02-17-18-09_25_4_6_2_0.bag' rovioEvaluator.odomTopic = '/rovio/odometry' rovioEvaluator.pclTopic = '/rovio/pcl' rovioEvaluator.extrinsicsTopic = '/rovio/extrinsics0' rovioEvaluator.gtFile = '/home/michael/datasets/bloesch_leo/planar_1/2015-12-22-14-15-42.bag' rovioEvaluator.gtTopic = '/bluebird/vrpn_client/estimated_transform' rovioEvaluator.startcut = 0 rovioEvaluator.endcut = 0 rovioEvaluator.doCov = True rovioEvaluator.doNFeatures = 25 rovioEvaluator.doExtrinsics = False rovioEvaluator.doBiases = False
