#!/usr/bin/env python
#
__author__ = 'Mike McCann'
__version__ = '1.0'
__date__ = '2013-04-01'
__copyright__ = '2013'
__license__ = 'GPL v3'
__contact__ = 'mccann at mbari.org'
__doc__ = '''
Parse logged data from BEDs .EVT or .WAT file to compute translations in x, y, z and rotations
about these axes. Output as X3D text so that the data can be visualized in an X3D Browser.
'''
import os
import sys
import csv
import math
import datetime
import numpy as np
from BEDS import BEDS
from util import quatern2euler, quaternConj, quatern2eulervector
from optparse import OptionParser
class BEDS_X3D(BEDS):
def __init__(self, args, argv):
'''
Initialize with options and set base X3D text
'''
self.args = args
self.argv = argv
self.inputFileNames = args.input
for fileName in self.inputFileNames:
if fileName.endswith('.log'):
self.sensorType = 'Atomic'
elif fileName.endswith('.EVT') or fileName.endswith('.WAT'):
self.sensorType = 'Invensense'
self.x3dEulerBaseText = '''
-->
'''
# TODO: x3dQuaternionBaseText is out of sync with x3dEulerBaseText for Scrubber operation
self.x3dQuaternionBaseText = '''
'''
return super(BEDS_X3D, self).__init__()
# End __init__()
def readAtomicLogFile(self, infile):
'''
Open @infile, read values, apply offsets and scaling.
Return numpy arrays of the acceleration and rotation data in engineering units.
'''
# From: "Kieft, Brian"
# Date: Wed, 18 May 2011 14:42:54 -0700
# To: Mike McCann
# Cc: "Herlien, Bob"
# Subject: data scaling
#
# Hi Mike,
#
# Here's the first two lines of the log file 20110518135934_atomic_imu.log in your tempbox:
#
# epoch ms,accel x, accel y, accel z, pitch, roll, yaw
# 1305752376772,470.0,483.0,780.0,521.0,527.0,500.0
#
# For this sensor, in this configuration, we're running full scale at 10 bits.
# For accelerations measured in g we have: 0.00403 g/count. So, in this line you'll
# see (780-512)*.00403 = 1.08g for the Z axis. This is when it was sitting on the desk
# before I picked it up. So, 0g would be 512 counts and you'll notice the other two
# axis are slightly negative (since it all has to add up to 1 g since it was just sitting there).
#
# For pitch,roll, and yaw we're looking at .977 degrees/count, same A/D.
#
# Does that work for you?
#
# -bk-
zeroOffset = 512
gPerCount = 0.00403
degPerCount = 0.977
# Header is:
# ['epoch ms', 'accel x', ' accel y', ' accel z', ' pitch', ' roll', ' yaw']
secList = []
axList = []
ayList = []
azList = []
pitchList = []
rollList = []
yawList = []
reader = csv.reader(open(infile))
for r in reader:
if self.args.verbose > 1:
print r
try:
secList.append(float(r[0]) / 1000.0)
axList.append((float(r[1]) - zeroOffset) * gPerCount)
ayList.append((float(r[2]) - zeroOffset) * gPerCount)
azList.append((float(r[3]) - zeroOffset) * gPerCount - 1.0)
pitchList.append((float(r[4]) - zeroOffset) * degPerCount * math.pi / 180)
rollList.append((float(r[5]) - zeroOffset) * degPerCount * math.pi / 180)
yawList.append((float(r[6]) - zeroOffset) * degPerCount * math.pi / 180)
except ValueError:
if self.args.verbose > 1:
print "Skipping row = %s" % r
# Make the Lists numpy arrays so that we can do Matlab-like operations
# These arrays have units of seconds, g, and radians.
self.s = np.array(secList)
self.ax = np.array(axList)
self.ay = np.array(ayList)
self.az = np.array(azList)
self.pitch = np.array(pitchList)
self.roll = np.array(rollList)
self.yaw = np.array(yawList)
return
def readFiles(self):
'''
Read accelerometer and rotation data from log file(s). Using potentially inherited methos (e.g. readBEDsFile())
creates member items secList, axList, ayList, azList, quatList and member nparrays s, ps, pr, ax, ay, az.
'''
for fileName in self.inputFileNames:
# Make sure input file is openable
print 'Input fileName = ', fileName
try:
with open(fileName):
pass
except IOError:
raise Exception('Cannot open input file %s' % fileName)
if self.sensorType == 'Atomic':
self.readAtomicLogFile(fileName)
elif self.sensorType == 'Invensense':
self.readBEDsFile(fileName)
else:
raise Exception("No handler for sensorType = %s" % self.sensorType)
def createX3D(self):
'''
Apply processing operations to convert logged data to geometric things for X3D and produce .x3d file
'''
if self.sensorType == 'Invensense':
self.processAccelerations()
self.processRotations()
else:
raise Exception("No handler for sensorType = %s" % self.sensorType)
# Interpolate data to regularly spaced time values - may need to do this to improve accuracy
# (See http://www.freescale.com/files/sensors/doc/app_note/AN3397.pdf)
##si = linspace(self.s[0], self.s[-1], len(self.s))
##axi = interp(si, self.s, self.ax)
t = self.s
if self.args.translate:
# Double integrate accelerations to get position and construct X3D position values string
# (May need to high-pass filter the data to remove noise that can give unreasonably large positions.)
xA = self.cumtrapz(t, self.cumtrapz(t, self.ax))
yA = self.cumtrapz(t, self.cumtrapz(t, self.ay))
zA = self.cumtrapz(t, self.cumtrapz(t, self.az))
# Construct X3D strings for keys, positions, orientations, and duration of the data
pKeys = ' '.join(['%.4f' % k for k in (t - t[0]) / (t[-1] - t[0])])
posList = ['%.4f %.4f %.4f' % (x, y, z) for (x, y, z) in zip(xA, yA, zA)]
posValues = ' '.join(posList)
else:
# Dummy key-frame parameters to do no translation
pKeys = '0 1'
posValues = '0 0 0 0 0 0'
if self.args.verbose:
print "Applying playback speedup factor = %s" % self.args.speedup
cycInt = '%.4f' % ((t[-1] - t[0]) / float(self.args.speedup))
dateCreated = datetime.datetime.now().strftime("%d %B %Y %H:%M:%S")
yearCreated = datetime.datetime.now().strftime("%Y")
if self.sensorType == 'Atomic':
oKeys = ' '.join(['%.4f' % k for k in (t - t[0]) / (t[-1] - t[0])])
xRotValues = ' '.join(['1 0 0 %.6f' % p for p in self.pitch])
yRotValues = ' '.join(['0 1 0 %.6f' % r for r in self.roll])
zRotValues = ' '.join(['0 0 1 %.6f' % y for y in self.yaw])
x3dText = self.x3dEulerBaseText % {'input': self.args.input, 'output': self.args.output,
'pKeys': pKeys, 'posValues': posValues, 'oKeys': oKeys, 'xRotValues': xRotValues,
'yRotValues': yRotValues, 'zRotValues': zRotValues, 'cycInt': cycInt,
'dateCreated': dateCreated, 'yearCreated': yearCreated,
'startEpoch': self.s[0], 'endEpoch': self.s[-1]}
elif self.sensorType == 'Invensense':
oKeys = ' '.join(['%.4f' % k for k in (t - t[0]) / (t[-1] - t[0])])
# Map Invensense axes to X3D axes
xRotValues = ' '.join(['1 0 0 %.6f' % x for x in self.rxList])
yRotValues = ' '.join(['0 1 0 %.6f' % z for z in self.rzList])
zRotValues = ' '.join(['0 0 1 %.6f' % -y for y in self.ryList])
RotRateValues = ' '.join(['%f' % rr for rr in self.rotrate])
RotVecValues = ' '.join(['%f %f %f' % (mx,my,mz) for mx,my,mz in zip(self.mxList,self.myList,self.mzList)])
x3dText = self.x3dEulerBaseText % {'commandLine': ' '.join(sys.argv),
'pKeys': pKeys, 'posValues': posValues, 'oKeys': oKeys, 'xRotValues': xRotValues,
'yRotValues': yRotValues, 'zRotValues': zRotValues, 'cycInt': cycInt,
'dateCreated': dateCreated, 'yearCreated': yearCreated,
'startEpoch': self.s[0], 'endEpoch': self.s[-1],
'RotRateValues': RotRateValues, 'RotVecValues': RotVecValues}
return x3dText
if __name__ == '__main__':
import argparse
from argparse import RawTextHelpFormatter
examples = 'Examples:' + '\n\n'
examples += ' Original Atomic imu:\n'
examples += ' ' + sys.argv[0] + ' --input 20110518135934_atomic_imu.log --output 20110518135934_atomic_imu.x3d\n'
examples += ' BEDs files:\n'
examples += ' ' + sys.argv[0] + ' --input BED00012.EVT --output BED00012.x3d --translate\n'
examples += ' ' + sys.argv[0] + ' --input BED00001.WAT --output BED00001_wat.x3d --speedup 100\n'
examples += ' Combine 2 event files into one output:\n'
examples += ' ' + sys.argv[0] + ' --input BED00038.EVT BED00039.EVT --output BED01_1_June_2013.x3d\n'
parser = argparse.ArgumentParser(formatter_class=RawTextHelpFormatter,
description='Convert BED event file(s) to a NetCDF file',
epilog=examples)
parser.add_argument('-i', '--input', action='store', nargs='*', required=True, help="Specify input BED log file name(s)")
parser.add_argument('-o', '--output', action='store', help="Specify output x3d file name")
parser.add_argument('-s', '--speedup', type=int, action='store', default=1, help="Specify playback speedup, e.g. 10 for 10 times faster that realtime")
parser.add_argument('--translate', action='store_true', default=False, help="Specify to integrate accelerations to translations")
parser.add_argument('--rotation', action='store', default='EA', help="Specify 'EA' or 'EV' for Euler Angle or Euler Vector to specify rotations")
parser.add_argument('--trajectory', action='store', help="csv file with columns of latitude and longitude where first and lat row corresponds to first and last reords of input event files")
parser.add_argument('-v', '--verbose', type=int, action='store', default=0, help="Specify verbosity level, values greater than 1 give more details ")
args = parser.parse_args()
if not (args.input and args.output):
parser.error("Must specify both --input and --output options.\n")
beds_x3d = BEDS_X3D(args, sys.argv)
beds_x3d.readFiles()
x3dText = beds_x3d.createX3D()
if args.verbose > 1:
print "x3dText = %s" % x3dText
f = open(args.output, 'w')
f.write(x3dText)
f.close()
print "Wrote file %s - open it in InstantReality Player, BS Contact, Xj3D, X3DOM, or other X3D browser." % args.output