<html><head><meta name="color-scheme" content="light dark"></head><body><pre style="word-wrap: break-word; white-space: pre-wrap;">
import tdsmParserMultis
import os
import ConfigParser
import XMLparser
import numpy as np
from sys import stderr
import math
from mayavi import mlab
import stl

# To access any VTK object, we use 'tvtk', which is a Python wrapping of
# VTK replacing C++ setters and getters by Python properties and
# converting numpy arrays to VTK arrays when setting data.
from tvtk.api import tvtk
from tvtk.tools import visual
from tvtk.common import configure_input

def Arrow_From_A_to_B(x1, y1, z1, x2, y2, z2, c):
    ar1=visual.arrow(x=x1, y=y1, z=z1, color = c)
    ar1.length_cone=0.4

    arrow_length=np.sqrt((x2-x1)**2+(y2-y1)**2+(z2-z1)**2)
    ar1.actor.scale=[arrow_length, arrow_length, arrow_length]
    ar1.pos = ar1.pos/arrow_length
    ar1.axis = [x2-x1, y2-y1, z2-z1]
    return ar1

def get_transformation_matrix(q1, q2, q3, q4, q5, q6):
    ''' Transform from optotrak global coordinates to optotrak position sensor coordinates'''

    T = np.zeros((4, 4))

    T[0, 0] = math.cos(q6) * math.cos(q5)
    T[1, 0] = math.sin(q6) * math.cos(q5)
    T[2, 0] = -math.sin(q5)

    T[0, 1] = math.cos(q6) * math.sin(q5) * math.sin(q4) - math.sin(q6) * math.cos(q4)
    T[1, 1] = math.sin(q6) * math.sin(q5) * math.sin(q4) + math.cos(q6) * math.cos(q4)
    T[2, 1] = math.cos(q5) * math.sin(q4)

    T[0, 2] = math.cos(q6) * math.sin(q5) * math.cos(q4) + math.sin(q6) * math.sin(q4)
    T[1, 2] = math.sin(q6) * math.sin(q5) * math.cos(q4) - math.cos(q6) * math.sin(q4)
    T[2, 2] = math.cos(q5) * math.cos(q4)

    T[0, 3] = q1
    T[1, 3] = q2
    T[2, 3] = q3
    T[3, 3] = 1

    return T

def convertTOarray(data):

    data = data.replace("", '')
    data = data.split(" ")
    corrected_list = data[2:len(data)]
    corrected_list[-1] = corrected_list[-1][0:-1]
    data_float = map(float, corrected_list)

    return np.asarray(data_float)

def get_xyzrpw(T):
    x = T[0,3]
    y = T[1,3]
    z = T[2,3]
    w = math.atan2(T[1,0], T[0,0])
    p = math.atan2(-T[2,0], math.sqrt(T[2,1]**2+T[2,2]**2))
    r = math.atan2(T[2,1], T[2,2])
    return x, y, z, r, p, w

def fit_hypersphere(data, method="Hyper"):
    """
            FitHypersphere.py

            fit_hypersphere(collection of tuples or lists of real numbers)
            will return a hypersphere of the same dimension as the tuples:
                    (radius, (center))

            using the Hyper (hyperaccurate) algorithm of
            Ali Al-Sharadqah and Nikolai Chernov
            Error analysis for circle fitting algorithms
            Electronic Journal of Statistics
            Vol. 3 (2009) 886-911
            DOI: 10.1214/09-EJS419

            generalized to n dimensions

            Mon Apr 23 04:08:05 PDT 2012 Kevin Karplus

            Note: this version using SVD works with Hyper, Pratt, and Taubin methods.
            If you are not familiar with them, Hyper is probably your best choice.


            Creative Commons Attribution-ShareAlike 3.0 Unported License.
            http://creativecommons.org/licenses/by-sa/3.0/
    """

    """returns a hypersphere of the same dimension as the
        collection of input tuples
                (radius, (center))

       Methods available for fitting are "algebraic" fitting methods
        Hyper   Al-Sharadqah and Chernov's Hyperfit algorithm
        Pratt   Vaughn Pratt's algorithm
        Taubin  G. Taubin's algorithm

       The following methods, though very similar, are not implemented yet,
          because the contraint matrix N would be singular,
          and so the N_inv computation is not doable.

        Kasa    Kasa's algorithm
    """
    num_points = len(data)
    #    print &gt;&gt;stderr, "DEBUG: num_points=", num_points

    if num_points == 0:
        return (0, None)
    if num_points == 1:
        return (0, data[0])
    dimen = len(data[0])  # dimensionality of hypersphere
    #    print &gt;&gt;stderr, "DEBUG: dimen=", dimen

    if num_points &lt; dimen + 1:
        raise ValueError( \
            "Error: fit_hypersphere needs at least {} points to fit {}-dimensional sphere, but only given {}".format(
                dimen + 1, dimen, num_points))

    # central dimen columns of matrix  (data - centroid)
    central = np.matrix(data, dtype=float)  # copy the data
    centroid = np.mean(central, axis=0)
    for row in central:
        row -= centroid
    # print &gt;&gt;stderr, "DEBUG: central=", repr(central)

    # squared magnitude for each centered point, as a column vector
    square_mag = [sum(a * a for a in row.flat) for row in central]
    square_mag = np.matrix(square_mag).transpose()
    #    print &gt;&gt;stderr, "DEBUG: square_mag=", square_mag

    if method == "Taubin":
        # matrix of normalized squared magnitudes, data
        mean_square = square_mag.mean()
        data_Z = np.bmat([[(square_mag - mean_square) / (2 * math.sqrt(mean_square)), central]])
        #    print &gt;&gt; stderr, "DEBUG: data_Z=",data_Z
        u, s, v = np.linalg.svd(data_Z, full_matrices=False)
        param_vect = v[-1, :]
        params = [x for x in np.asarray(param_vect)[0]]  # convert from (dimen+1) x 1 matrix to list
        params[0] /= 2 * math.sqrt(mean_square)
        params.append(-mean_square * params[0])
        params = np.array(params)

    else:
        # matrix of squared magnitudes, data, 1s
        data_Z = np.bmat([[square_mag, central, np.ones((num_points, 1))]])
        #    print &gt;&gt; stderr, "DEBUG: data_Z=",data_Z

        # SVD of data_Z
        # Note: numpy's linalg.svd returns data_Z = u * s * v
        #         not u*s*v.H as the Release 1.4.1 documentation claims.
        #         Newer documentation is correct.
        u, s, v = np.linalg.svd(data_Z, full_matrices=False)
        #    print &gt;&gt;stderr, "DEBUG: u=",repr(u)
        #    print &gt;&gt;stderr, "DEBUG: s=",repr(s)
        #    print &gt;&gt;stderr, "DEBUG: v=",repr(v)
        #    print &gt;&gt;stderr, "DEBUG: v.I=",repr(v.I)

        if s[-1] / s[0] &lt; 1e-12:
            # singular case
            # param_vect as (dimen+2) x 1 matrix
            param_vect = v[-1, :]
            # Note: I get last ROW of v, while Chernov claims last COLUMN,
            # because of difference in definition of SVD for MATLAB and numpy

            #        print &gt;&gt; stderr, "DEBUG: singular, param_vect=", repr(param_vect)
            #        print &gt;&gt; stderr, "DEBUG: data_Z*V=", repr(data_Z*v)
            #        print &gt;&gt; stderr, "DEBUG: data_Z*VI=", repr(data_Z*v.I)
            #        print &gt;&gt; stderr, "DEBUG: data_Z*A=", repr(data_Z*v[:,-1])
        else:
            Y = v.H * np.diag(s) * v
            Y_inv = v.H * np.diag([1. / x for x in s]) * v
            #        print &gt;&gt;stderr, "DEBUG: Y=",repr(Y)
            #        print &gt;&gt;stderr, "DEBUG: Y.I=",repr(Y.I), "\nY_inv=",repr(Y_inv)
            # Ninv is the inverse of the constraint matrix, after centroid has been removed
            Ninv = np.asmatrix(np.identity(dimen + 2, dtype=float))
            if method == "Hyper":
                Ninv[0, 0] = 0
                Ninv[0, -1] = 0.5
                Ninv[-1, 0] = 0.5
                Ninv[-1, -1] = -2 * square_mag.mean()
            elif method == "Pratt":
                Ninv[0, 0] = 0
                Ninv[0, -1] = -0.5
                Ninv[-1, 0] = -0.5
                Ninv[-1, -1] = 0
            else:
                raise ValueError("Error: unknown method: {} should be 'Hyper', 'Pratt', or 'Taubin'")
                #        print &gt;&gt; stderr, "DEBUG: Ninv=", repr(Ninv)

            # get the eigenvector for the smallest positive eigenvalue
            matrix_for_eigen = Y * Ninv * Y
            #   print &gt;&gt; stderr, "DEBUG: {} matrix_for_eigen=\n{}".format(method, repr(matrix_for_eigen))
            eigen_vals, eigen_vects = np.linalg.eigh(matrix_for_eigen)
            #   print &gt;&gt; stderr, "DEBUG: eigen_vals=", repr(eigen_vals)
            #   print &gt;&gt; stderr, "DEBUG: eigen_vects=", repr(eigen_vects)

            positives = [x for x in eigen_vals if x &gt; 0]
            if len(positives) + 1 != len(eigen_vals):
                # raise ValueError("Error: for method {} exactly one eigenvalue should be negative: {}".format(method,eigen_vals))
                print&gt;&gt; stderr, "Warning: for method {} exactly one eigenvalue should be negative: {}".format(method,
                                                                                                              eigen_vals)
            smallest_positive = min(positives)
            #    print &gt;&gt; stderr, "DEBUG: smallest_positive=", smallest_positive
            # chosen eigenvector as 1 x (dimen+2) matrix
            A_colvect = eigen_vects[:, list(eigen_vals).index(smallest_positive)]
            #        print &gt;&gt; stderr, "DEBUG: A_colvect=", repr(A_colvect)
            # now have to multiply by Y inverse
            param_vect = (Y_inv * A_colvect).transpose()
            #        print &gt;&gt; stderr, "DEBUG: nonsingular, param_vect=", repr(param_vect)
            params = np.asarray(param_vect)[0]  # convert from (dimen+2) x 1 matrix to array of (dimen+2)


            #    print &gt;&gt; stderr, "DEBUG: params=", repr(params)
    radius = 0.5 * math.sqrt(sum(a * a for a in params[1:-1]) - 4 * params[0] * params[-1]) / abs(params[0])
    center = -0.5 * params[1:-1] / params[0]
    # y    print &gt;&gt; stderr, "DEBUG: center=", repr(center), "centroid=", repr(centroid)
    center += np.asarray(centroid)[0]
    return (radius, center)

def transform_sphere_points(B1m, B1mr):
    ''' Function to extract the centers of each registration marker in the respective bone Optotrak sensor coordinate
    system'''

    # Within this data set, there are 30 points, each with an x,y,and z coordinate
    # next we want to convert this data set into an array. The units for the axis
    # are all in m.

    # But first need to get rid of the quotations at the beginning and end of string.
    B1m = B1m.replace('"', '')
    B1msplit = B1m.split(" ")

    # Get rid of first two cells, and convert string into float
    corrected_list = B1msplit[2:92]
    corrected_floatlist = map(float, corrected_list)

    ## matrix of the 30 points
    B1mCoord = np.asarray(corrected_floatlist)
    B1mCoord = B1mCoord.reshape(30, -1)

    # Within this data set, there are 30 points, each with an x,y,z,roll, pitch and
    # yaw coordinate next we want to convert this data set into an array. The
    # units for the x,y, and z axis are in m and the roll, pitch, and yaw axis
    # are in rad.

    # But first need to get rid of the quotations at the beginning and end of string.
    B1mr = B1mr.replace('"', '')
    B1mrsplit = B1mr.split(" ")

    # Get rid of first two cells, and convert string into float
    corrected_list = B1mrsplit[2:182]
    corrected_floatlist = map(float, corrected_list)

    ## matrix of the 30 points
    B1mrCoord = np.asarray(corrected_floatlist)
    B1mrCoord = B1mrCoord.reshape(30, -1)

    ## Define coordinate transformations of data
    P1 = np.ones((4, 1))
    Coord1 = np.zeros((30, 3))

    for i in range(0, 30):
        q1 = B1mrCoord[i, 0]
        q2 = B1mrCoord[i, 1]
        q3 = B1mrCoord[i, 2]
        q4 = B1mrCoord[i, 3]
        q5 = B1mrCoord[i, 4]
        q6 = B1mrCoord[i, 5]

        T1 = get_transformation_matrix(q1, q2, q3, q4, q5, q6)

        P1[0, 0] = B1mCoord[i, 0]
        P1[1, 0] = B1mCoord[i, 1]
        P1[2, 0] = B1mCoord[i, 2]

        invT1 = np.linalg.inv(T1)

        A = np.dot(invT1, P1) # Transform to bone Optotrak sensor coordinate system
        Coord1[i, 0] = A[0, 0]
        Coord1[i, 1] = A[1, 0]
        Coord1[i, 2] = A[2, 0]

    ACoord1 = Coord1[0:10, 0:3]
    BCoord1 = Coord1[10:20, 0:3]
    CCoord1 = Coord1[20:30, 0:3]


    # Sphere fit for Rigid Body Collected Points (m), all three spheres. Set to NAN if points were not digitized.
    NAN = float('nan')
    try:
        B1mSphereA = fit_hypersphere(ACoord1, method="Pratt")
    except:
        B1mSphereA = [(0), (NAN, NAN, NAN)]

    try:
        B1mSphereB = fit_hypersphere(BCoord1, method="Pratt")
    except:
        B1mSphereB = [(0), (NAN, NAN, NAN)]

    try:
        B1mSphereC = fit_hypersphere(CCoord1, method="Pratt")
    except:
        B1mSphereC = [(0), (NAN, NAN, NAN)]

    return B1mSphereA, B1mSphereB, B1mSphereC

def planeFit(points):
    """
    p, n = planeFit(points)

    Given an array, points, of shape (d,...)
    representing points in d-dimensional space,
    fit an d-dimensional plane to the points.
    Return a point, p, on the plane (the point-cloud centroid),
    and the normal, n.
    """
    import numpy as np
    from numpy.linalg import svd
    points = np.reshape(points, (np.shape(points)[0], -1)) # Collapse trialing dimensions
    assert points.shape[0] &lt;= points.shape[1], "There are only {} points in {} dimensions.".format(points.shape[1], points.shape[0])
    ctr = points.mean(axis=1)
    x = points - ctr[:,np.newaxis]
    M = np.dot(x, x.T) # Could also use np.cov(x) here.
    return ctr, svd(M)[0][:,-1]

def DrawPlane(   renderer, center, normal, color, size=500, opacity=0.3):
    """Convenience function to draw a plane
        takes center of plane, normal vector, and size (length of square side)
        apply color and opacity
        return actor"""
    planeDraw = tvtk.PlaneSource(normal=normal)
    transPlane = tvtk.Transform()
    transPlane.translate(center)
    transPlane.scale(size, size, size)
    transFilter = tvtk.TransformPolyDataFilter(input_connection=planeDraw.output_port, transform=transPlane)
    planeMapper = tvtk.PolyDataMapper(input_connection=transFilter.output_port)
    planeProp = tvtk.Property(color=color, opacity=opacity)
    planeActor = tvtk.Actor(mapper=planeMapper, property=planeProp)
    renderer.add_actor(planeActor)

def format_collected_points_data(dig_points, sensor_positions):
    # Within this data set, there are 30 points, each with an x,y,and z coordinate
    # next we want to convert this data set into an array. The units for the axis
    # are all in m.

    # But first need to get rid of the quotations at the beginning and end of string.
    dig_points = dig_points.replace('"', '')
    dig_points_split = dig_points.split(" ")

    # Get rid of first two cells, and convert string into float
    size = int(dig_points_split[0][9:])
    dimension = int(dig_points_split[1][:-1])
    corrected_list = dig_points_split[2:2+size*dimension]
    corrected_floatlist = map(float, corrected_list)

    ## matrix of the 30 points
    dig_points_Coord = np.asarray(corrected_floatlist)
    dig_points_Coord = dig_points_Coord.reshape(size, -1)

    # Within this data set, there are 30 points, each with an x,y,z,roll, pitch and
    # yaw coordinate next we want to convert this data set into an array. The
    # units for the x,y, and z axis are in m and the roll, pitch, and yaw axis
    # are in rad.

    # But first need to get rid of the quotations at the beginning and end of string.
    sensor_positions = sensor_positions.replace('"', '')
    sensor_positions_split = sensor_positions.split(" ")

    # Get rid of first two cells, and convert string into float
    size_sens = int(sensor_positions_split[0][9:])
    dimension_sens = int(sensor_positions_split[1][:-1])
    corrected_list = sensor_positions_split[2:size_sens*dimension_sens+2]
    corrected_floatlist = map(float, corrected_list)

    ## matrix of the 30 points
    sensor_positions_Coord = np.asarray(corrected_floatlist)
    sensor_positions_Coord = sensor_positions_Coord.reshape(size, -1)

    ## Define coordinate transformations of data
    P1 = np.ones((4, 1))
    Coord1 = np.zeros((size, dimension))

    for i in range(0, size):
        q1 = sensor_positions_Coord[i, 0]
        q2 = sensor_positions_Coord[i, 1]
        q3 = sensor_positions_Coord[i, 2]
        q4 = sensor_positions_Coord[i, 3]
        q5 = sensor_positions_Coord[i, 4]
        q6 = sensor_positions_Coord[i, 5]

        T1 = get_transformation_matrix(q1, q2, q3, q4, q5, q6)

        P1[0, 0] = dig_points_Coord[i, 0]
        P1[1, 0] = dig_points_Coord[i, 1]
        P1[2, 0] = dig_points_Coord[i, 2]

        invT1 = np.linalg.inv(T1)
        # invT1 = np.identity(4)

        A = np.dot(invT1, P1) # Transform to Optotrak sensor coordinate system
        Coord1[i, 0] = A[0, 0]
        Coord1[i, 1] = A[1, 0]
        Coord1[i, 2] = A[2, 0]

    return Coord1

def main(dir, segment):

    dir1 = os.path.join(dir, "Registration")
    if not os.path.isdir(dir1):
        os.makedirs(dir1)

    # Read in both probe STLs that were transformed to US tip coordinate system
    filename_9L4 = 'Probe STLS/9L4.transformed.stl'
    reader_9L4 = tvtk.STLReader()
    reader_9L4.file_name = file_name = filename_9L4
    reader_9L4.update()

    filename_14L5 = 'Probe STLS/14L5.transformed.stl'
    reader_14L5 = tvtk.STLReader()
    reader_14L5.file_name = file_name = filename_14L5
    reader_14L5.update()

    # Assign seg and bone parameters for later calculations
    if segment == 'UpperLeg':
        seg = 'UL_'
        bone = 'Femur'
        markers = ["F1", "F2", "F3", "F4", "F5", "F6"]
    elif segment == 'LowerLeg':
        seg = 'LL_'
        bone = 'Tibia'
        markers = ["T1", "T2", "T3", "T4", "T5", "T6"]
    elif segment == 'UpperArm':
        seg = 'UA_'
        bone = 'Humerus'
        markers = ["H1", "H2", "H3", "H4", "N/A", "N/A"]
    elif segment == 'LowerArm':
        seg = 'LA_'
        bone = 'Radius'
        markers = ["R1", "N/A", "N/A", "R2", "R3", "N/A"]


    v = mlab.figure() # Create Mayavi figure

    ## Find and plot all ultrasound positions of relevant trials ##
    # Get accepted trials from Ultrasound experiment
    masterList, num_accept = XMLparser.getAcceptance(dir)
    accepted_list = []
    for x in masterList:
        if x[1] == 1:
            accepted_list.append(x[0])

    dir_data = os.path.join(dir, 'Data')
    files = os.listdir(dir_data)
    files.sort()

    # Plot positions of ultrasound probe for each accepted ultrasound trial for the specified segment
    for file in files:
        if any(file[0:3] in s for s in accepted_list) and seg in file:

            data = tdsmParserMultis.parseTDMSfile(os.path.join(dir_data, file))

            config = ConfigParser.RawConfigParser()
            if not config.read(os.path.join(os.path.join(dir, 'Configuration'), file[0:-5]+'_State.cfg')):
                raise IOError, "Cannot load configuration file... Check path."

            T_BOS_B = config.get('Probe-'+bone+' Position', 't_sensor1_rb1')
            T_BOS_B = convertTOarray(T_BOS_B).reshape(4,-1)

            if 'T_I_BOS' not in locals():

                B1m = config.get(bone + '-Sphere Positions',
                                 'Collected Points Rigid Body 1 (m)')  # Proximal Digitized points (global)
                B1mr = config.get(bone + '-Sphere Positions',
                                  'Collected Points Rigid Body 1 Position Sensor (m,rad)')  # Position of bone Optotrak Sensor

                B2m = config.get(bone + '-Sphere Positions',
                                 'Collected Points Rigid Body 2 (m)')  # Distal Digitized points (global)
                B2mr = config.get(bone + '-Sphere Positions',
                                  'Collected Points Rigid Body 2 Position Sensor (m,rad)')  # Position of bone Optotrak Sensor

                # Extract points representing table in the optotrak world coordinate system
                if bone == 'Femur' or bone == 'Humerus':
                    table_points = config.get('Table', 'Collected Points Rigid Body 1 (m)')
                    sensor_position = config.get('Table', 'Collected Points Rigid Body 1 Position Sensor (m,rad)')
                elif bone == 'Tibia' or bone == 'Radius':
                    table_points = config.get('Table', 'Collected Points Rigid Body 2 (m)')
                    sensor_position = config.get('Table', 'Collected Points Rigid Body 2 Position Sensor (m,rad)')

                table_points_Coord = format_collected_points_data(table_points, sensor_position)
                table_points_Coord = table_points_Coord[~np.all(table_points_Coord == 0, axis=1)]# remove lines that are zero

                # Get digitized sphere centers (bone sensor coordinate system)
                spheres_prox = transform_sphere_points(B1m, B1mr)
                spheres_dist = transform_sphere_points(B2m, B2mr)

                sphere_fit_rm = np.array(map(list, zip(*(spheres_prox+spheres_dist)))[1])
                sphere_fit_rm = sphere_fit_rm[~np.isnan(sphere_fit_rm)].reshape((-1,3))

                for i, pts in enumerate(table_points_Coord):
                    pts_transformed = np.dot(np.linalg.inv(T_BOS_B), np.append(pts, [1]))
                    table_points_Coord[i] = pts_transformed[0:3]
                    mlab.points3d(pts_transformed[0] * 1000, pts_transformed[1] * 1000, pts_transformed[2] * 1000,
                                  scale_factor=5, figure=v)
                center, normal = planeFit(table_points_Coord.T)

                DrawPlane(v.scene, center * 1000, normal, color=(0, 1, 1))

                for i, s in enumerate(spheres_prox):
                    if ~np.isnan(s[1]).any():
                        sphere = np.ones(4)
                        sphere[0:3] = s[1]
                        sphere = np.dot(np.linalg.inv(T_BOS_B), sphere)
                        mlab.points3d(sphere[0]*1000, sphere[1]*1000, sphere[2]*1000, 1, scale_factor=20, figure=v, opacity=0.1, color=(1,0,0), mode = 'sphere')


                for i, s in enumerate(spheres_dist):
                    if ~np.isnan(s[1]).any():
                        sphere = np.ones(4)
                        sphere[0:3] = s[1]
                        sphere = np.dot(np.linalg.inv(T_BOS_B), sphere)
                        mlab.points3d(sphere[0]*1000, sphere[1]*1000, sphere[2]*1000, 1, scale_factor=20, figure=v, opacity=0.1, color=(1,0,0), mode = 'sphere')

            print file

            graph = True

            probe = config.get('6-DOF Load', 'Description')  # Extract the probe that was used during experimentation
            frame = 0
            # Get ultrasound position from State (TDMS)
            x = np.array(data[u'State.Probe-'+bone+' Position'][u'Probe-'+bone+' Position x'])[frame]/1000
            y = np.array(data[u'State.Probe-'+bone+' Position'][u'Probe-'+bone+' Position y'])[frame]/1000
            z = np.array(data[u'State.Probe-'+bone+' Position'][u'Probe-'+bone+' Position z'])[frame]/1000
            r = np.radians(np.array(data[u'State.Probe-'+bone+' Position'][u'Probe-'+bone+' Position roll'])[frame])
            p = np.radians(np.array(data[u'State.Probe-'+bone+' Position'][u'Probe-'+bone+' Position pitch'])[frame])
            w = np.radians(np.array(data[u'State.Probe-'+bone+' Position'][u'Probe-'+bone+' Position yaw'])[frame])

            # Ultrasound probe tip coordinates in the image coordinate system
            A = get_transformation_matrix(x,y,z,r,p,w) # Define what coordinate system to plot everything in

            if graph == True:

                print probe[1:-1]
                if probe[1:-1] == "9L4 Ultrasound":
                    filename = filename_9L4
                elif probe[1:-1] == "14L5 Ultrasound":
                    filename = filename_14L5
                else:
                    print("No STL for ultrasound probe of this type")

                # # Remove posterior trials from visualization
                # if seg+'P' in file:
                #     pass
                # else:
                try:
                    stl_file = stl.Mesh.from_file(filename, calculate_normals=True)

                    A[0][3] = A[0][3] * 1000
                    A[1][3] = A[1][3] * 1000
                    A[2][3] = A[2][3] * 1000

                    stl_file.transform(A)
                    stl_file.save(filename[0:-4] + '_1.stl')

                    filename3 = filename[0:-4] + '_1.stl'
                    reader2 = tvtk.STLReader()
                    reader2.file_name = file_name = filename3
                    reader2.update()

                    mapper2 = tvtk.PolyDataMapper()
                    configure_input(mapper2, reader2.output)

                    # Plot US tip point for each trial
                    # Posterior is red, Anterior is yellow, Medial is green, Lateral is blue
                    if seg + 'P' in file:
                        c = (1, 0, 0)
                    elif seg + 'A' in file:
                        c = (1, 1, 0)
                    elif seg + 'M' in file:
                        c = (0, 1, 0)
                    elif seg + 'L' in file:
                        c = (0, 0, 1)

                    # Distal is more transparent than proximal
                    if 'P_' in file:
                        op = 0.6
                    elif 'C_' in file:
                        op = 0.4
                    elif 'D_' in file:
                        op = 0.2

                    # Plot ultrasound probe tip point in Mayavi scene
                    prop = tvtk.Property(opacity=op, color=c)
                    mlab.points3d(x * 1000, y * 1000, z * 1000, 1, scale_factor=5, figure=v, opacity=op, color=c)

                    # Add ultrasound probe to Mayavi scene
                    actor2 = tvtk.Actor(mapper=mapper2, property=prop)
                    v.scene.add_actor(actor2)
                except:
                    print "Error in rotation of probe STL"

    # Add coordinate system arrows to Mayavi window
    visual.set_viewer(v)
    Arrow_From_A_to_B(0, 0, 0, 0, 0, 50, (0, 0, 1))
    Arrow_From_A_to_B(0, 0, 0, 0, 50, 0, (0, 1, 0))
    Arrow_From_A_to_B(0, 0, 0, 50, 0, 0, (1, 0, 0))

    mlab.view(distance='auto')
    mlab.show()

if __name__ == "__main__":
    dir = '/home/morrile2/Documents/MULTIS Data/MULTIS_invitro/CMULTIS005-1'  # Cadaver specimen folder
    segment = 'UpperArm'  # Change this to the desired segment
    main(dir, segment)

</pre></body></html>