Skip to content

Instantly share code, notes, and snippets.

@nro-bot

nro-bot/2019-01-30 20:34:12_arucotagData.csv

Created March 5, 2019 00:41
Download ZIP

Revisions

  1. nro-bot created this gist Mar 5, 2019.
    491 changes: 491 additions & 0 deletions 2019-01-30 20:34:12_arucotagData.csv
    View file
    491 additions, 0 deletions not shown because the diff is too large. Please use a local Git client to view these changes.
    2,090 changes: 2,090 additions & 0 deletions 2019-01-30 20:34:12_optoforceData.csv
    View file
    2,090 additions, 0 deletions not shown because the diff is too large. Please use a local Git client to view these changes.
    399 changes: 399 additions & 0 deletions generateMultiaxialFitPlots.py
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,399 @@
    """
    Make graphs for uniaxial (F=kx, where k.shape is (1,1))
    date: 11 Feb 2019
    author: nouyang
    """

    from scipy.stats.stats import pearsonr
    from sklearn import metrics
    from sklearn import linear_model
    from scipy.stats import linregress
    from scipy.interpolate import interp1d
    import copy
    import math
    import sys
    import glob
    import os
    import seaborn as sns
    import matplotlib.pyplot as plt
    import pandas as pd
    import numpy as np


    sns.set(rc={"figure.figsize": (16, 10)},
    font_scale=1.8)
    plt.style.use('bmh')
    # plt.style.use('fivethirtyeight')
    # sns.set() #use seaborn style for matplotlib
    # ===============================================
    #### DECLARE FILES ####
    # ===============================================
    path = "data"

    DATA_FOLDER = os.getcwd()

    Fx_file = '2019-01-31 02:25:05'
    # Mx_file = '2019-01-31 02:25:05'
    Fy_file = '2019-01-31 02:48:31'
    Fz_file = '2019-01-31 02:26:49'

    # Mx_file = '2019-01-31 02:51:12'
    # Mx_file = '2019-01-31 02:50:37'
    Mx_file = '2019-01-30 20:05:53' # +- thetaA #known good

    # My_file = '2019-01-31 02:52:32'
    # My_file = '2019-01-31 02:51:58'
    My_file = '2019-01-30 20:09:20' # -phi #known good

    # Mz_file = '2019-01-31 02:49:34'
    # Mz_file = '2019-01-31 02:50:00'
    Mz_file = '2019-01-30 20:07:57' # +- gamma #known good


    datafilename = '2019-02-01 06:23:32'
    datafilename = '2019-02-01 06:28:10' # soft motion # looks ok minux z
    # datafilename = '2019-02-01 06:32:40'
    # Manually everywhere, before zero fix. but
    datafilename = '2019-01-30 20:34:12'
    # but what does zerofix mean? #TODO #best #delay 13
    # datafilename = '2019-02-01 06:26:41' # rest
    # datafilename = '2019-02-01 06:28:32' # impulses
    # datafilename = Mx_file

    # datafilename = '2019-02-01 06:29:22' # delay 12 samples / 0.04

    # datafilename = '2019-01-30 20:34:12' # tag 2 looks... ok...


    def extractDF(filename):
    opto_files = [
    file
    for file in glob.glob(
    os.path.join(DATA_FOLDER, path, filename + "_optoforceData.csv")
    )
    ]
    tag_files = [
    file
    for file in glob.glob(
    os.path.join(DATA_FOLDER, path, filename + "_arucotagData.csv")
    )
    ]
    print("\n--------------------------\n")
    print(filename)
    # print(opto_files)
    # print(tag_files)
    opto_fname = opto_files[0]
    tag_fname = tag_files[0]

    # ===============================================
    #### Import Data ####
    # ===============================================
    opto_cols = ["Description", "Timestamp",
    "x", "y", "z", "theta", "phi", "gamma"]

    # following are zero and averaged (in the original collecting)
    tag_cols = ["Description", "Timestamp", "x1", "y1", "z1", "x2", "y2", "z2",
    "theta1", "phi1", "gamma1", "theta2", "phi2", "gamma2",
    "_x", "_y", "_z", "_theta", "_phi", "_gamma", ]

    # tag_cols = ["Description", "Timestamp", "x1", "y1", "z1", "x2", "y2", "z2",
    # "theta1", "phi1", "gamma1", "theta2", "phi2", "gamma2",
    # "_x", "_y", "_z", "_theta", "_phi", "_gamma", ]

    opto_df = pd.read_csv(
    opto_fname,
    header=None,
    sep=";",
    names=opto_cols,
    skip_blank_lines=True,
    usecols=range(8),
    )

    opto_df = opto_df.drop(["Description"], 1)

    tag_df = pd.read_csv(
    tag_fname,
    header=None,
    sep=";",
    names=tag_cols,
    skip_blank_lines=True,
    usecols=range(20),
    )

    tag_df = tag_df.drop(["Description"], 1)

    # ===============================================
    #### Conversion to real units ####
    # ===============================================

    # --- Tag
    # -- MULTIPLY - convert tag from meters to mm
    for var in ["x1", "y1", "z1"] + ["_x", "_y", "_z"] + ["x2", "y2", "z2"]:
    tag_df[var] *= 1000

    # convert optoforce counts to newtons (provided by optoforce datasheet)
    opto_df["x"] /= 96.11
    opto_df["y"] /= 95.64
    opto_df["z"] /= 20.92

    # --- Optoforce
    # -- convert counts to newton-meters (provided by optoforce datasheet)
    opto_df["theta"] /= 5425.36
    opto_df["phi"] /= 5524.60
    opto_df["gamma"] /= 8103.25

    # -- MULTIPLY - convert optoforce, | N to mN | N*m to mN*m |
    # print('before zero', opto_df.head(2))
    # avg_opto_zeroing = np.average(opto_df.head(20), axis=0)
    # print('zero', avg_opto_zeroing)
    # opto_df -= avg_opto_zeroing
    # print('after zero', opto_df.head(2))

    for a in ["x", "y", "z", "theta", "phi", "gamma"]:
    opto_df[a] *= 1000

    # -- print range of forces measured
    print("\n--------- Force ranges measured -----------------\n")
    for a in ["x", "y", "z", "theta", "phi", "gamma"]:
    print("%s axis force. Min: %0.3f, Max: %0.3f mN or mN*m" %
    (a, opto_df[a].min(), opto_df[a].max()))

    # print(opto_df.head())
    # print(opto_df.describe())

    # ===============================================
    #### FIX THE DATA (swap axis etc.) ####
    # ===============================================
    # NOTE: truncate beginning, which is behaving strangely for some reason
    # TODO: diagnose this
    tag = tag_df.copy()[20:]
    opto = opto_df.copy()
    # [20:] tag = tag_df.copy()

    # Swap axis as needed to align tag axes with opto axis
    new_cols = ["Timestamp", "y1", "x1", "z1", "y2", "x2", "z2",
    "theta1", "phi1", "gamma1", "theta2", "phi2", "gamma2",
    # following are zero and averaged
    "_y", "_x", "_z", "_theta", "_phi", "_gamma", ]
    tag.columns = new_cols

    # Negate axes as needed
    negateList = ["x1", "y1", "z1", "x2", "y2", "z2",
    "theta1", "theta2", "gamma1", "gamma2",
    "_x", "_y", "_z", "_theta", "_gamma"]

    # new_cols = [ "Timestamp", "y1", "x1", "z1", "y2", "x2", "z2",
    # "phi1", "theta1", "gamma1", "phi2", "theta2", "gamma2",
    # # following are zero and averaged
    # "_y", "_x", "_z", "_phi", "_theta", "_gamma", ]

    for n in negateList:
    tag[n] *= -1

    return tag, opto


    # ===============================================
    #### Linear regress to match apriltag to optoforce & plot ####
    # ===============================================


    # tagset = str(1)
    tagset = "_"


    def gen_interp(tag_df, opto_df):
    list_axis = ['x', 'y', 'z', 'theta', 'phi', 'gamma']
    interp_tag_data = []
    interp_opto_data = []
    for AXIZ in list_axis:
    print("\n>--- delay AXIZ: ", AXIZ)
    # a_x = tag_df[AXIZ + tagset] # single tag
    a_x = tag_df[tagset + AXIZ] # single tag
    a_t = tag_df.Timestamp

    o_x = opto_df[AXIZ]
    o_t = opto_df.Timestamp

    # interp_tagfxn = interp1d(a_t - lag, a_x, kind='linear', fill_value='extrapolate')
    num_samples = len(o_t) + len(a_t)
    begin = min(o_t.min(), a_t.min())
    end = max(o_t.max(), a_t.max())
    t_regular = np.linspace(begin, end, num_samples * 2)

    # a_x = a_x[20:]
    f_o = interp1d(o_t, o_x, kind="linear", fill_value="extrapolate")
    f_a = interp1d(
    a_t, a_x, kind="linear", fill_value="extrapolate"
    ) # definitely not cubic!
    interp_o = f_o(t_regular)
    interp_a = f_a(t_regular)

    # normalize for cross-correlation
    norm_interp_a = interp_a - interp_a.mean()
    norm_interp_a /= norm_interp_a.std()
    norm_interp_o = interp_o - interp_o.mean()
    norm_interp_o /= norm_interp_o.std()

    corr = np.correlate(norm_interp_o, norm_interp_a, mode="full")
    # source https://dsp.stackexchange.com/questions/9058/measuring-time-delay-of-audio-signals
    delay = int(len(corr) / 2) - np.argmax(corr)
    delay = 11 # NOTE hardcod3

    print("\n>--- delay # datapoints: ", delay, "; in real time: ",
    delay * (t_regular[1] - t_regular[0]))
    timefix_a_x = np.roll(interp_a, shift=-delay)

    gunk = 250
    gunky = -250
    # remove some gunkiness, I think it's introduced by the "extrapolate" option
    # on the interpolation
    t_regular = t_regular[gunk:gunky]
    y = interp_o.reshape(-1, 1)[gunk:gunky]
    X = np.array(timefix_a_x).reshape(-1, 1)[gunk:gunky]
    # interp_opto_data[AXIZ] = y
    # interp_tag_data[AXIZ] = X
    # print('xamples of y', y[100:110])
    # print('samples of X', X[100:110])
    interp_opto_data.append(y)
    interp_tag_data.append(X)

    # print('interp o', interp_opto_data.shape)
    # interp_opto_data = np.vstack((interp_opto_data, y))
    # print('interp o', interp_opto_data.shape)
    # print('y', y.shape)
    # interp_tag_data = np.vstack((interp_tag_data, X))
    interp_opto_data = np.array(interp_opto_data)[:, :, 0]
    interp_tag_data = np.array(interp_tag_data)[:, :, 0]

    return t_regular, interp_tag_data, interp_opto_data


    delay = '12=0.045' # NOTE HARDCODE
    # delay = 'normal'

    afile = datafilename
    tag_df, opto_df = extractDF(afile) # all 6 axis
    t_regular, tag_data, opto_data = gen_interp(tag_df, opto_df)

    print(tag_data.shape)
    print(opto_data.shape)
    # for i, key in enumerate(tag_data):
    # print('i', i, 'key', key)
    # print('shape', tag_data[key].shape)
    tag_data = tag_data.T
    opto_data = opto_data.T
    regr = linear_model.LinearRegression().fit(tag_data, opto_data)
    regr = linear_model.Ridge().fit(tag_data, opto_data)
    y_est = regr.predict(tag_data)

    print(y_est.shape)
    print(y_est.shape[0])

    print(regr.coef_)
    print('!-- shape!--', regr.coef_.shape)
    # print(regr.intercept_)
    # print(regr.score(y_est, opto_data))
    # print(y_est - opto_data)
    print(np.sum(y_est - opto_data))

    # ------------ PLOT overlaid
    f, axess = plt.subplots(nrows=2, ncols=3, sharex=False)
    axess = np.array(axess).flatten()
    ax1, ax2, ax3, ax4, ax5, ax6, = axess

    axes_reordered = [ax1, ax4, ax2, ax5, ax3, ax6, ]

    # -- Plot row of x,y,z tag, then row of x,y,z, opto, 12 plots
    for i in range(y_est.shape[1]):
    print(y_est.shape)
    print(opto_data.shape)
    figA = axess[i]
    a_x = y_est[:, i]
    o_x = opto_data[:, i]
    figA.plot(t_regular, o_x, 'k.', ms=1.5,)
    # label=('Measured opto data: ' + str(i)))

    figA.plot(t_regular, a_x, 'r.', ms=1,)
    # label=('Measured opto data: ' + str(i)))

    # figA.plot(t_regular, tag_data[:, i], 'r.', ms=1.5,)
    # label=('Measured tag data: ' + str(i)))
    # figA.legend.

    # ax1 = fig_axs[0]
    # ax4 = fig_axs[3]
    # ax2 = fig_axs[1]
    # ax5 = fig_axs[4]
    ax1.set_title('X Force')
    ax2.set_title('Y Force')
    ax3.set_title('Z Force')

    ax4.set_title('X Moment')
    ax5.set_title('Y Moment')
    ax6.set_title('Z Moment')


    ax1.set_ylabel("Force (mN)")
    ax4.set_ylabel("Torque (mN*m)")
    ax5.set_xlabel("Time (secs)")

    plt.tight_layout()

    # plt.suptitle("Timesteps data, y_est vs y")
    plt.savefig('./plots/ridge+lessgunk-' + datafilename + '-' + 'delay-' + str(delay) +
    '-axis-' + tagset + '.png')
    # plt.show()

    # ------------ YES: PLOT LINEAR FIT ---------------------------
    f, fig_axs = plt.subplots(nrows=2, ncols=3, sharex=False)
    fig_axs = np.array(fig_axs).T.flatten(order='F')

    # Plot of Uniaxial, 6 single axis
    for i in range(y_est.shape[1]):
    fig_ax = fig_axs[i]
    a_x = y_est[:, i]
    o_x = opto_data[:, i]
    # print('shapes', a_x.shape, o_x.shape)
    fig_ax.plot(o_x, o_x, 'k-') # label='Optoforce data '+key)
    print(a_x.shape)
    print(o_x.shape)
    # regr.score(a_x.reshape(-1, 1), o_x.reshape(-1, 1))
    r_score = pearsonr(o_x, a_x)
    print(r_score)
    fig_ax.plot(o_x, a_x, 'r.', markersize=1.5,
    # label=('Linear fit estimate\nR: %.2f, axis %d' % (r_score[0]**2, i)))
    label=('$R^2$ = %.2f' % (r_score[0]**2)))
    # print('i: ', i)
    # print("coef: %f intercept: %f r-score: %f"
    # % (regr.coef_, regr.intercept_, r_score))
    fig_ax.legend(markerscale=0, markerfirst=False)

    ax1, ax2, ax3, ax4, ax5, ax6, = fig_axs
    # ax1 = fig_axs[0]
    # ax4 = fig_axs[3]
    # ax2 = fig_axs[1]
    # ax5 = fig_axs[4]
    ax1.set_title('X Force')
    ax2.set_title('Y Force')
    ax3.set_title('Z Force')

    ax4.set_title('X Moment')
    ax5.set_title('Y Moment')
    ax6.set_title('Z Moment')

    ax1.set_ylabel("Prototype Sensor (mN)")
    ax4.set_ylabel("Prototype Sensor (mN*m)")
    ax2.set_xlabel("Commercial Sensor (mN)")
    ax5.set_xlabel("Commercial Sensor (mN*m)")
    # plt.suptitle("!--- Linear fits (uniaxial) ---!\n Perfect fit is black line\nRed dots are \
    # tag measurements\n(1st order linear fit with affine term)")
    # plt.gca().set_aspect('equal', adjustable='datalim')
    plt.tight_layout()
    # plt.axis('scaled')

    plt.savefig('./plots/ridge+lessgunky+uniaxial-' + datafilename + '-' + 'delay-' + str(delay) +
    '-axis' + tagset + '.png')
    plt.show()
    420 changes: 420 additions & 0 deletions read_both.py
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,420 @@
    '''
    Read arucotag using openCV library.
    Read optoforce, using library from ShadowHand.
    Date: 18 Jan 2019
    Author: nouyang
    # https://github.com/shadow-robot/optoforce/blob/indigo-devel/optoforce/src/optoforce/optoforce_node.py
    ctrl-backslash for extra serious force quit (or sudo killall python)
    NOTE: may require running `sudo chmod 666 /dev/ttyACM0` due to permissions
    issues
    '''

    import cv2
    import cv2.aruco as aruco

    import serial
    import sys
    import signal
    import logging
    import pprint
    import optoforcelibrary as optoforce
    import copy


    from datetime import datetime
    import time

    import threading
    from argparse import ArgumentParser

    import math
    import numpy as np

    #------------------------------------
    # Run Flags

    # np.set_printoptions(suppress=True, precision=4)
    arucoFlag = True
    numTags = 2
    optoFlag = True
    writeFlag = True # write to CSV file?

    #------------------------------------
    # Data recording constants

    # writeFolder = "./data/"
    writeFolder = "../analysis/data/"
    strtime = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
    o_fname = writeFolder + strtime + '_optoforceData.csv'
    a_fname = writeFolder + strtime + '_arucotagData.csv'
    fmode = 'a'

    inittime = time.time()

    # ------------------------------------
    # Sensor Constants
    optoforce_port = "/dev/ttyACM0"

    # ------------------------------------
    # Camera Constants
    # tag_size = 0.015
    #tag_size = 0.0038
    tag_size = 0.0055 # guesstimate for old protoype

    width = 640
    height = 480
    fx = 640.0 # must be floats for apriltags._draw_pose to work
    fy = 640.0
    px = 300.0
    py = 250.0

    #camera_matrix = np.array([fx, fy, px, py])

    # cameraMatrix = np.array([[619.6207333698724, 0.0, 283.87165814439834],
    # [0.0, 613.2842389650563, 231.9993874728696],
    # [0.0, 0.0, 1.0]])


    # NOTE: I do not have actual camera matrix for the Amazon hawk sensor! (original
    # flat)
    # Approximate using another webcam... HBV-1716-2019-01-29_calib.yaml

    cameraMatrix = np.array([[521.92676671, 0., 315.15287785],
    [ 0., 519.01808261, 193.05763006],
    [ 0., 0., 1. ]])

    distCoeffs, rvecs, tvecs = np.array([]), [], []

    distCoeffs = np.array([ 0.02207713, 0.18641578, -0.01917194, -0.01310851,
    -0.11910311])

    # ------------------------------------
    # Methods to read aruco tag data

    def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6


    # Calculates rotation matrix to euler angles
    # The result is the same as MATLAB except the order
    # of the euler angles ( x and z are swapped ).
    def rotationMatrixToEulerAngles(R) :

    assert(isRotationMatrix(R))

    sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])

    singular = sy < 1e-6

    if not singular :
    x = math.atan2(R[2,1] , R[2,2])
    y = math.atan2(-R[2,0], sy)
    z = math.atan2(R[1,0], R[0,0])
    else :
    x = math.atan2(-R[1,2], R[1,1])
    y = math.atan2(-R[2,0], sy)
    z = 0

    rots = np.array([x, y, z])
    rots = np.array([math.degrees(r) for r in rots])

    rots[0] = 180 - rots[0] % 360
    return rots

    ## ------------------------------


    # ------------------------------------
    # Methods to read from optoforce

    class OptoThread(threading.Thread):
    def __init__(self, inittime, fname):
    threading.Thread.__init__(self)

    self.inittime = inittime
    self.fname = fname

    optoforce_sensor_type = "s-ch/6-axis"
    starting_index = 0
    scaling_factors = [[1,1,1,1,1,1]] #one per axis
    # if len(sys.argv) > 1:
    # port = "/dev/" + sys.argv[1]
    try:
    self.driver = optoforce.OptoforceDriver(optoforce_port,
    optoforce_sensor_type,
    scaling_factors)
    print('opened opto force!')
    except serial.SerialException as e:
    print('failed to open optoforce serial port!')
    raise


    def run(self):
    time.sleep(2) # wait for opencv tag to initialize
    while True:
    optotime = time.time()
    self.optodata = self.driver.read()

    if isinstance(self.optodata, optoforce.OptoforceData): #UNTESTED
    opto_data_str = 'Optoforce time, xyz, yawpitchroll; ' + str(optotime-self.inittime)+ '; '+\
    '; '.join([str(o) for o in self.optodata.force[0]]) + '\n'
    a = ['{0: <8}'.format(x) for x in self.optodata.force[0]]
    #pprint.pprint(' '.join(a)) # NOTE

    #print(opto_data_str)
    if writeFlag:
    with open(self.fname,'a') as outf :
    outf.write(opto_data_str)
    outf.flush()

    elif isinstance(data, optoforce.OptoforceSerialNumber):
    print("The sensor's serial number is " + str(data))


    # ------------------------------------
    # Methods to read from arucotags

    class ArucoThread(threading.Thread):
    def __init__(self, inittime, fname, camera_matrix, dist_coeffs, tag_size):
    threading.Thread.__init__(self)

    self.inittime = inittime
    self.fname = fname
    #self.camera_matrix = camera_matrix
    self.cameraMatrix = camera_matrix
    self.tagSize = tag_size
    self.distCoeffs = dist_coeffs

    self.aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
    self.aruco_params = aruco.DetectorParameters_create()

    #width=1280
    #height=720
    width=640
    height=480
    fps=30

    # ------------------------------------
    self.w = 0.2 # filter_weight

    # ------------------------------------
    # Parse commandline arguments
    parser = ArgumentParser()

    parser.add_argument('device_or_movie', metavar='INPUT', nargs='?', default=0,
    help='Movie to load or integer ID of camera device')
    options = parser.parse_args()

    # ------------------------------------
    # Open video stream
    try:
    self.stream = cv2.VideoCapture(int(options.device_or_movie))
    except ValueError:
    self.stream = cv2.VideoCapture(options.device_or_movie)

    self.stream.set(cv2.CAP_PROP_FRAME_WIDTH,width)
    self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT,height)
    #self.stream.set(cv2.CAP_PROP_FPS,fps)

    def run(self):
    # -----------------------------------
    # Set up filtering for angles
    cumTvec, cumRvec = np.ones((numTags, 3)), np.ones((numTags, 3))

    # -----------------------------------
    # Initialized zeros
    counter = 0
    avgN = 20

    # -- NOTE: this code is highly repetitive, refactor at some point
    while counter < avgN:
    print('looking for 20 tags... found: ', counter)
    success, frame = self.stream.read()
    if not success:
    print('failed to grab frame')
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    corners, ids, rejectedImgPoints = \
    aruco.detectMarkers(gray,
    self.aruco_dict,
    parameters=self.aruco_params)

    if (ids is not None) and (len(ids) == numTags):
    tagData = zip(ids, corners)
    tagData = sorted(tagData, key=lambda x: x[0]) # put corners in order of id
    ids = [tag[0] for tag in tagData]
    corners = [tag[1] for tag in tagData]

    rvec, tvec, objPoints = \
    cv2.aruco.estimatePoseSingleMarkers(corners, self.tagSize,
    self.cameraMatrix,
    self.distCoeffs);

    if rvec is not None and rvec.shape[0] == numTags:
    counter += 1
    tvec = tvec.reshape(numTags,3)

    cumTvec += tvec
    for i in range(numTags):
    rotMat, jacob = cv2.Rodrigues(rvec[i])
    rots = rotationMatrixToEulerAngles(rotMat)
    cumRvec[i] = rots

    if writeFlag == True:
    cv2.imwrite(self.fname + '_.jpg', frame); # for debugging, save a frame

    zeroThetas = copy.deepcopy(cumRvec/avgN)
    zeroDists = copy.deepcopy(cumTvec/avgN)

    # ------------------------------------
    # Look for arucotags
    #out, prevOut = np.ones((numTags, 3)), np.ones((numTags, 3))
    out, prevOut = copy.deepcopy(zeroThetas), copy.deepcopy(zeroDists)

    while True:
    atime = time.time()
    success, frame = self.stream.read()

    if not success:
    print('failed to grab frame')

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    corners, ids, rejectedImgPoints = \
    aruco.detectMarkers(gray,
    self.aruco_dict,
    parameters=self.aruco_params)


    if (ids is not None) and (len(ids) == numTags):
    tagData = zip(ids, corners)
    tagData = sorted(tagData, key=lambda x: x[0]) # put corners in order of id
    ids = [tag[0] for tag in tagData]
    corners = [tag[1] for tag in tagData]
    rvec, tvec, objPoints = \
    cv2.aruco.estimatePoseSingleMarkers(corners, self.tagSize,
    self.cameraMatrix,
    self.distCoeffs);
    gray = aruco.drawDetectedMarkers(gray, corners)
    cv2.imshow('frame', gray)

    if rvec is not None and rvec.shape[0] == numTags:
    tvec = tvec.reshape(numTags,3)
    atime = time.time()
    for i in range(numTags):
    rotMat, jacob = cv2.Rodrigues(rvec[i])
    rots = rotationMatrixToEulerAngles(rotMat)

    out[i] = self.w * rots + (1- self.w) * prevOut[i]
    prevOut[i] = out[i]
    #print('Aruco Tag ID:', i, ': ', out[i])
    # print('Aruco Tag ID:', 1, ': ', out[1])

    # collect zeros at the beginning
    # x,y,z should be zero'd and then changes averaged between the
    # two tags (maybe?)

    # NOTE: we are exponential filtering thetas but not distances
    calcDists = tvec - zeroDists
    calcThetas = out - zeroThetas
    avgCalcDists = np.average(calcDists, axis=0)
    avgCalcThetas = np.average(calcThetas, axis=0)

    # print(tvec[1]* 1000)
    # TODO print readings

    #print('shapes', [x.shape for x in [zeroThetas, out, calcThetas,
    #avgCalcThetas]])
    #print('zeroThetas', zeroThetas)
    #print('zeroDists', zeroDists)
    #print('filtered reading\n', out)
    print(out)
    #print('zerod reading', calcThetas)
    #print('averaged between two', avgCalcThetas)
    #print('\n')
    # print(outD)
    # print(zeroDists)

    if writeFlag:
    data_str = 'Arucotag time, tag1 xyz, tag2 xyz, ' + \
    'tag1 rollpitchyaw (xyz), tag2 -- xyz zerod averaged; ' + \
    str(atime -self.inittime)+ '; '+ \
    '; '.join([str(t) for t in calcDists.flatten()]) + '; ' + \
    '; '.join([str(r) for r in calcThetas.flatten()]) + '; '+ \
    '; '.join([str(t) for t in avgCalcDists.flatten()]) + '; ' + \
    '; '.join([str(r) for r in avgCalcThetas.flatten()]) + '\n'
    # '; '.join([str(t) for t in tvec.flatten()]) + '; ' + \
    # '; '.join([str(r) for r in out.flatten()]) + '; '+ \

    with open(self.fname,'a') as outf:
    outf.write(data_str)
    outf.flush()

    # a = ['{0: <8}'.format(x) for x in self.optodata.force[0]]
    # pprint.pprint(' '.join(a))




    # ------------------------------------
    def main():

    inittime = time.time()

    if optoFlag:
    optot = OptoThread(inittime, o_fname)
    optot.daemon = True
    optot.start()
    if arucoFlag:
    # arucot = ArucoThread(inittime, camera_matrix, a_fname)
    arucot = ArucoThread(inittime, a_fname, cameraMatrix, distCoeffs, tag_size)
    arucot.daemon = True
    arucot.start()

    cv2.startWindowThread()
    cv2.namedWindow("Aruco Camera")

    while threading.active_count() > 0:
    # cv2.imshow("Camera", _overlay)
    # cv2.waitKey(0) #IMPORTANT, without this line, window hangs

    k = cv2.waitKey(0)
    if k == 27 or k == ord('q'): # Escape key
    # self.stopped = True
    print('!! ---- Caught escape key')
    break
    time.sleep(0.001)

    # outf.close()
    print('!! ---- Destroying windows')
    print('=============== \nData STARTED at\n' + strtime)
    print('---------------\nData RECORDED to: ', a_fname)
    endtime = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
    print('---------------\nData collection ENDED at', endtime, '\n==================')
    cv2.destroyAllWindows()
    sys.exit(0)


    # ------------------------------------
    if __name__ == '__main__':
    #print('=============== \nData STARTED at', strtime, '\n================')
    print('Time: ', datetime.now().strftime('%Y-%m-%d %H:%M:%S'))

    def signal_handler(sig, frame):
    print('You pressed Ctrl+C!')
    sys.exit(0)

    signal.signal(signal.SIGINT, signal_handler)

    try:
    main()
    except KeyboardInterrupt:
    print('!! --- Keyboard interrupt in progress')
    finally:
    print('!! --- Finished cleaning up.')