コード例 #1
0
        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

# Load Odometry Data
td.addLabelingIncremental("pos", 3)
td.addLabelingIncremental("att", 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, "pos", "att")

# Load Timestamps into new td
td_aligned.addLabelingIncremental("pos", 3)
td_aligned.addLabelingIncremental("att", 4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic, td_aligned)

# Interpolate Data
td.interpolateColumns(td_aligned, "pos", "pos")
td.interpolateQuaternion(td_aligned, "att", "att")

# Plotting
コード例 #2
0
        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)
    td_rovio.addLabelingIncremental('extPosCov',9)
    td_rovio.addLabelingIncremental('extPosSp',3)
コード例 #3
0
            -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)
    td_rovio.addLabelingIncremental('extPosCov', 9)
    td_rovio.addLabelingIncremental('extPosSp', 3)
コード例 #4
0
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

# Load Odometry Data
td.addLabelingIncremental('pos', 3)
td.addLabelingIncremental('att', 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, 'pos',
                                      'att')

# Load Timestamps into new td
td_aligned.addLabelingIncremental('pos', 3)
td_aligned.addLabelingIncremental('att', 4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic,
                                            td_aligned)

# Interpolate Data
td.interpolateColumns(td_aligned, 'pos', 'pos')
td.interpolateQuaternion(td_aligned, 'att', 'att')