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)
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
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 """
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