def num_euler_to_quat(euler): '''Converts 3 euler angles XYZ to a quaternion (YXZ order)''' assert(True not in [(math.isnan(x) or math.isinf(x)) for x in euler]) rx, ry, rz = euler q_xrot = cgtypes.quat().fromAngleAxis(rx, [1, 0, 0]) q_yrot = cgtypes.quat().fromAngleAxis(ry, [0, 1, 0]) q_zrot = cgtypes.quat().fromAngleAxis(rz, [0, 0, 1]) q = (q_yrot * q_xrot * q_zrot).normalize() #YXZ order return q
def num_euler_to_quat(euler):
'''Converts 3 euler angles XYZ to a quaternion (YXZ order)'''
assert (True not in [(math.isnan(x) or math.isinf(x)) for x in euler])
rx, ry, rz = euler
q_xrot = cgtypes.quat().fromAngleAxis(rx, [1, 0, 0])
q_yrot = cgtypes.quat().fromAngleAxis(ry, [0, 1, 0])
q_zrot = cgtypes.quat().fromAngleAxis(rz, [0, 0, 1])
q = (q_yrot * q_xrot * q_zrot).normalize() #YXZ order
return q
def rotate_quat(q, ax, ang):
"""
Rotate quaternion orientation by given axis and angle
q = input quaternion
ax rotation axis
ang = rotation angle
"""
# Convert quaternion to cgtypes quaternion
q1 = cgtypes.quat(q)
q_rot = cgtypes.quat(ang,ax)
q2 = q_rot*q1
return quat2tuple(q2)
def qrotate_vec(v,q):
"""
Rotate vector using a quaternion.
v = input vector (tuple)
q = quaternion (tuple)
"""
# Get cgtypes rotation quaternion and its inverse
rot_q = cgtypes.quat(q)
rot_q_inv = rot_q.inverse()
# Rotate vector
vq = cgtypes.quat(0.0, v[0], v[1], v[2])
vq_new = rot_q*vq*rot_q_inv
return vq_new.x, vq_new.y, vq_new.z
def _integrateAngularVelocities(self, angularVelocity, timeElapsed):
angularVelocityQuat = quat(0.0, angularVelocity.x, angularVelocity.y,
angularVelocity.z)
self.orientationBody = self.orientationBody * (angularVelocityQuat *
timeElapsed / 2).exp()
def calculate_quaternion(self):
from cgkit.cgtypes import quat, mat3, slerp
q = quat()
mat = mat3([num for row in self.transform[0:3] for num in row[0:3]])
try:
mat = mat.decompose()[0]
q.fromMat(mat)
except ZeroDivisionError:
q = quat(0,0,0,0)
self.do_not_use = True
a = 2*math.acos(q.w)
t = np.dot(np.array([q.x,q.y,q.z]),a)
self.rot_axis = t.tolist()
self.q = np.dot(np.array([q.x,q.y,q.z]),q.w).tolist()
def measure(self, magneto):
measurementJacobian = sp.zeros((3, 13))
magnetoJacobian = scipy_utils.d_inverse_rotation_d_quaternion_at(
cgkit_to_scipy.convert(self.orientationBody),
cgkit_to_scipy.convert(magneto))
scipy_utils.load_submatrix(measurementJacobian, (0, 6),
magnetoJacobian)
kalmanGain = sp.dot(
sp.dot(self._errorCovariance, measurementJacobian.transpose()),
linalg.inv(
sp.dot(
measurementJacobian,
sp.dot(self._errorCovariance,
measurementJacobian.transpose())) +
self._measurementCovariance))
stateCorrection = sp.dot(
kalmanGain,
(cgkit_to_scipy.convert(magneto - self._predictMagnetoBody())))
self.orientationBody = self.orientationBody + quat(
list(stateCorrection[6:10]))
self._errorCovariance = sp.dot(
(sp.identity(13) - sp.dot(kalmanGain, measurementJacobian)),
self._errorCovariance)
def rotate_vec(v, ax, ang):
"""
Rotate a vector by a given angle about a given axis.
v = input vector
ax = rotation axis
ang = rotation angle (radians)
"""
# Get rotation quaterion and its inverse
rot_q = cgtypes.quat(ang, ax)
rot_q_inv = rot_q.inverse()
# Rotate vector
vq = cgtypes.quat(0.0,v[0],v[1],v[2])
vq_new = rot_q*vq*rot_q_inv
return vq_new.x, vq_new.y, vq_new.z
def testquaternion_derivative(self):
quat = scipy_utils.to_quaternion(*self.YPRAngles)
print scipy_utils.quaternion_norm(quat)
d_quat = scipy_utils.quaternion_derivative(2*sp.pi, 0.0, 0.0, quat)
print scipy_utils.quaternion_norm(d_quat)
quat_1s_later = quat + 0.01 * d_quat
cgquat = cgtypes.quat(quat_1s_later)
print cgquat
print cgquat.normalize()
quat_1s_later = quat_1s_later / sp.sqrt(scipy_utils.quaternion_norm(quat_1s_later))
print quat_1s_later
print scipy_utils.quaternion_norm(quat_1s_later)
roll, pitch, yaw = scipy_utils.to_YPR(quat_1s_later)
testing.assert_almost_equal(roll, self.YPRAngles[0] + .01)
def calculate_quaternion(self):
from cgkit.cgtypes import quat, mat3, slerp
q = quat()
mat = mat3([num for row in self.transform[0:3] for num in row[0:3]])
mat = mat.decompose()[0]
q.fromMat(mat)
self.q = np.dot(np.array([q.x,q.y,q.z]),q.w).tolist()
def updateFromJoint(self):
# TODO: I think this has been replaced by the transformVertices function
if self.joint is None:
return
if self.updateOnlyFromGrandparentJoints:
if self.joint.parentJoint is None:
q = quat(1, 0, 0, 0)
else:
q = self.joint.parentJoint.orientation
else:
q = self.joint.orientation
# update vertex
for i in xrange(len(self.OriginalVertexList)):
scale = self.scale * self.joint.scale # scale is combination of model scale factor and joint scale factor
v = scale * (
self.OriginalVertexList[i] - self.initialRotationCenter
) # get scaled vector based on initial rotation center
v = numpy.array(q.rotateVec(v)) # apply current orientation
self.VertexList[
i] = v + self.joint.location # move vector to joint location
# update normals (orientation only, not scaled or moved)
for i in xrange(len(self.OriginalNormVectors)):
n = self.OriginalNormVectors[i]
n = numpy.array(q.rotateVec(n))
self.NormVectors[i] = n
def __init__(self):
self._processCovariance = sp.identity(13)
self._measurementCovariance = sp.identity(3)
self._errorCovariance = sp.identity(13)
self._translationRotorFromBody = vec3(0)
self._gravity = vec3([0.0, 0.0, 1.0])
self._magneticField = mat3.fromEulerXYZ(0.0, -1.22, 0.0) * vec3(
[1.0, 0.0, 0.0])
# the magnetic field vector points to the magnetic north, with a -70 degrees (-1.22 radians) pitch (towards the ground)
self._declinationAngle = 0.0
self.timestamp = 0.0
self.positionNED = vec3(0)
self.velocityBody = vec3(0)
self.orientationBody = quat(1)
self.gyroBiases = vec3(0)
self.gravityBody = self._gravity
def testDavailus(self):
q_w = cgtypes.quat(0.0, 2*sp.pi, 0.0, 0.0)
d_t = 1.0
q_w = q_w * d_t * 0.5
e_dav = q_w.exp()
quat_t0 = cgtypes.quat(scipy_utils.to_quaternion(*self.YPRAngles))
quat_t1 = quat_t0 * e_dav
roll, pitch, yaw = scipy_utils.to_YPR((quat_t1.w, quat_t1.x, quat_t1.y, quat_t1.z))
testing.assert_almost_equal(roll, self.YPRAngles[0])
def rotateCamera(self,camera,eyePose):
"""
This function rotates the Gluskap camera based on the Oculus Rift sensor data in eyePose parameter
"""
# Convert the OculusRift rotation quaternion to a cgkit quat object
rotationQuaternion = quat(eyePose.Orientation.toList())
# Fix the quaternion because of incompatible coordinate system
rotationQuaternion=quat(rotationQuaternion.w,rotationQuaternion.z,rotationQuaternion.y,-rotationQuaternion.x)
# Normalize the quaternion
rotationQuaternion=rotationQuaternion.normalize()
# Apply the rotation to the basevpn to achieve the new view plane normal vector of the camera
vpn=rotationQuaternion.rotateVec(self.basevpn)
# Apply the rotation to baseup to acheive the up vector of the camera
up=rotationQuaternion.rotateVec(self.baseup)
# Apply the gamepad yaw rotation to the view plane normal and up vector of camera
gamePadYawRotation=quat().fromAngleAxis(self.gamePadYaw,self.baseup)
vpn=gamePadYawRotation.rotateVec(vpn)
up=gamePadYawRotation.rotateVec(up)
# Apply the gamepad pitch to the view plane normal and up vector of camera
rightAxis=up.cross(vpn).normalize()
gamePadPitchRotation=quat().fromAngleAxis(self.gamePadPitch,rightAxis)
vpn=gamePadPitchRotation.rotateVec(vpn)
up=gamePadPitchRotation.rotateVec(up)
# Compute the angle between the new view plane normal and view plane normal of previous frame
angle=vpn.angle(self.prevpn)
# Store the prevpn for the next frame
self.prevpn=vec3(vpn)
# If angle is below certain threshold do not apply the rotation
# This is to remove image vibration caused by unstable Oculus Rift orientation
if math.fabs(angle) < 0.005:
return
# update the camera on Gluskap
camera.vpn=(vpn.x,vpn.y,vpn.z)
camera.up=(up.x,up.y,up.z)
def rotateAroundAxis(self, angle, axis):
# create rotation quat based on local axis
q = quat(angle, self.cameraQuat.rotateVec(axis))
# rotate camera around focus point
self.camLoc = numpy.array(q.rotateVec(self.camLoc - self.camFocus)) + self.camFocus
# update camera orientation
self.cameraQuat = q * self.cameraQuat
# calculate up direction from new orientation
self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
def __init__(self,
pos_0=cgtypes.vec3(0,0,0),
ori_0=cgtypes.quat(1,0,0,0),
vel_0=cgtypes.vec3(0,0,0),
):
self.pos_0 = pos_0
self.ori_0 = ori_0
self.vel_0 = vel_0
self.reset()
def rotateAroundAxis(self, angle, axis):
# create rotation quat based on local axis
q = quat(angle, self.cameraQuat.rotateVec(axis))
# rotate camera around focus point
self.camLoc = numpy.array(
q.rotateVec(self.camLoc - self.camFocus)) + self.camFocus
# update camera orientation
self.cameraQuat = q * self.cameraQuat
# calculate up direction from new orientation
self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
def walkTo(self, X, Y, Theta):
cur_position = vec3(self.nao.position)
curTheta = self.nao.orientation
movement = vec3(X, Y, 0)
rotation = quat(curTheta, AXIS_Z)
movement = rotation.rotateVec(movement)
self.nao.position = list(cur_position + movement)
self.nao.orientation = (curTheta + Theta) % (math.pi * 2)
def walkTo(self, X, Y, Theta):
cur_position = vec3(self.nao.position)
curTheta = self.nao.orientation
movement = vec3(X, Y, 0)
rotation = quat(curTheta, AXIS_Z)
movement = rotation.rotateVec(movement)
self.nao.position = list(cur_position + movement)
self.nao.orientation = (curTheta + Theta) % (math.pi * 2)
def rbm_to_dualquat(rbm):
import cgkit.cgtypes as cg
q0 = cg.quat().fromMat(cg.mat3(rbm[:3, :3].T.tolist()))
q0 = q0.normalize()
q0 = np.array([q0.w, q0.x, q0.y, q0.z])
t = rbm[:3, 3]
q1 = np.array([
-0.5 * (t[0] * q0[1] + t[1] * q0[2] + t[2] * q0[3]),
0.5 * (t[0] * q0[0] + t[1] * q0[3] - t[2] * q0[2]),
0.5 * (-t[0] * q0[3] + t[1] * q0[0] + t[2] * q0[1]),
0.5 * (t[0] * q0[2] - t[1] * q0[1] + t[2] * q0[0])
])
return np.array(q0.tolist() + q1.tolist())
def render_frame(self):
self.frame += 1
# Fetch the head pose
poses = self.hmd.get_eye_poses(self.frame, self.eyeOffsets)
self.hmd.begin_frame(self.frame)
for i in range(0, 2):
eye = self.hmdDesc.EyeRenderOrder[i]
glMatrixMode(GL_PROJECTION)
glLoadMatrixf(self.projections[eye])
self.eyeview = mat4(1.0)
# Apply the head orientation
rot = poses[eye].Orientation
# Convert the OVR orientation (a quaternion
# structure) to a cgkit quaternion class, and
# from there to a mat4 Coordinates are camera
# coordinates
rot = quat(rot.toList())
rot = rot.toMat4()
# Apply the head position
pos = poses[eye].Position
# Convert the OVR position (a vector3 structure)
# to a cgcit vector3 class. Position is in camera /
# Rift coordinates
pos = vec3(pos.toList())
pos = mat4(1.0).translate(pos)
pose = pos * rot
# apply it to the eyeview matrix
self.eyeview = pose;
# The subclass is responsible for taking eyeview
# and applying it to whatever camera or modelview
# coordinate system it uses before rendering the
# scene
# Active the offscreen framebuffer and render the scene
glBindFramebuffer(GL_FRAMEBUFFER, self.fbo[eye])
size = self.eyeTextures[eye].Texture.Header.RenderViewport.Size
glViewport(0, 0, size.w, size.h)
self.render_scene()
glBindFramebuffer(GL_FRAMEBUFFER, 0)
self.hmd.end_frame(poses, self.eyeTextures)
glGetError()
def _rotVector_(self, v, angle, axis):
"""
Rotate a vector by an angle around an axis
INPUTS:
v: 3-dim float array
angle: float, the rotation angle in radians
axis: string, 'x', 'y', or 'z'
RETURNS:
float array(3): the rotated vector
"""
# axisd = {'x':[1,0,0], 'y':[0,1,0], 'z':[0,0,1]}
# construct quaternion and rotate...
rot = cgt.quat()
rot.fromAngleAxis(angle, axis)
return list(rot.rotateVec(v))
def ovrPoseToMat4(pose): # Apply the head orientation rot = pose.Orientation # Convert the OVR orientation (a quaternion structure) to a cgkit quaternion # class, and from there to a mat4 Coordinates are camera coordinates rot = quat(rot[-1:]+rot[:-1]) # reorder xyzw -> wxyz rot = rot.toMat4() # Apply the head position pos = pose.Position # Convert the OVR position (a vector3 structure) to a cgcit vector3 class. # Position is in camera / Rift coordinates pos = vec3(pos) pos = mat4(1.0).translate(pos) pose = pos * rot return pose
def ovrPoseToMat4(pose): # Apply the head orientation rot = pose.Orientation # Convert the OVR orientation (a quaternion structure) to a cgkit quaternion # class, and from there to a mat4 Coordinates are camera coordinates rot = quat(rot[-1:]+rot[:-1]) # reorder xyzw -> wxyz rot = rot.toMat4() # Apply the head position pos = pose.Position # Convert the OVR position (a vector3 structure) to a cgcit vector3 class. # Position is in camera / Rift coordinates pos = vec3(pos) pos = mat4(1.0).translate(pos) pose = pos * rot return pose
def residual(parameters, motion, statisticsByLabelBeforeMotion, samplesByLabelInMotion, statisticsByLabelAfterMotion, acceleroMisalignmentsAndScales, acceleroBias):
# gravityVectorBefore = sp.array([statisticsByLabelBeforeMotion['x'][0], statisticsByLabelBeforeMotion['y'][0], statisticsByLabelBeforeMotion['z'][0]])
#
# gravityVectorBefore = utils.correct_measures(acceleroMisalignmentsAndScales, acceleroBias, gravityVectorBefore.reshape((3,1)))
#
# gravityVectorAfter = sp.array([statisticsByLabelAfterMotion['x'][0], statisticsByLabelAfterMotion['y'][0], statisticsByLabelAfterMotion['z'][0]])
#
# gravityVectorAfter = utils.correct_measures(acceleroMisalignmentsAndScales, acceleroBias, gravityVectorAfter.reshape((3,1)))
gyroMisalignmentsAndScales = sp.reshape(parameters, (3, 3))
correctedMeasures = utils.extract_and_correct_gyro_motion(gyroMisalignmentsAndScales, statisticsByLabelBeforeMotion, samplesByLabelInMotion)
aSamples, bSamples, cSamples = correctedMeasures[0,:], correctedMeasures[1,:], correctedMeasures[2,:]
# gravityVector = vec3(*sp.reshape(gravityVectorBefore, 3))
fromAngularPosition, toAngularPosition, feedrate, endTime, beginTime = motion
gravityVector = utils.gravityVector_IMU_coords(math.radians(fromAngularPosition[0]), math.radians(fromAngularPosition[1]))
gravityVectorAfter = utils.gravityVector_IMU_coords(math.radians(toAngularPosition[0]), math.radians(toAngularPosition[1]))
timestamps = samplesByLabelInMotion['timestamps']
previousTimestamp = timestamps[0]
for index in range(len(timestamps)) :
angularVelocityQuat = quat(0.0, aSamples[index], bSamples[index], cSamples[index])
timeElapsed = timestamps[index] - previousTimestamp
previousTimestamp = timestamps[index]
gravityVector = (angularVelocityQuat * timeElapsed / 2).exp().rotateVec(gravityVector)
residualGavity = gravityVector - gravityVectorAfter
residualGavity = sp.array([residualGavity.x, residualGavity.y, residualGavity.z])
return sp.sqrt(sp.dot(residualGavity, residualGavity))
def updateFromJoint(self):
# TODO: I think this has been replaced by the transformVertices function
if self.joint is None:
return
if self.updateOnlyFromGrandparentJoints:
if self.joint.parentJoint is None:
q = quat(1, 0, 0, 0)
else:
q = self.joint.parentJoint.orientation
else:
q = self.joint.orientation
# update vertex
for i in xrange(len(self.OriginalVertexList)):
scale = self.scale * self.joint.scale # scale is combination of model scale factor and joint scale factor
v = scale * (self.OriginalVertexList[i] - self.initialRotationCenter) # get scaled vector based on initial rotation center
v = numpy.array(q.rotateVec(v)) # apply current orientation
self.VertexList[i] = v + self.joint.location # move vector to joint location
# update normals (orientation only, not scaled or moved)
for i in xrange(len(self.OriginalNormVectors)):
n = self.OriginalNormVectors[i]
n = numpy.array(q.rotateVec(n))
self.NormVectors[i] = n
def calculate_plane_to_xy_transform(self):
"""
Calculate the transform to rotate the plane of the circle into the yz plane.
"""
def chunks(l, n):
""" Yield successive n-sized chunks from l.
"""
for i in xrange(0, len(l), n):
yield l[i:i+n]
x = self.best_fit_plane()
normal = [x[0],x[1],-1]
self.normal = normal
from cgkit.cgtypes import quat, mat3, slerp
axis = np.cross(np.array(normal),np.array([0,0,1]))
angle = math.acos(np.dot(np.array(normal),np.array([0,0,1]))/np.linalg.norm(normal))
q = quat()
q = q.fromAngleAxis(angle,axis.tolist())
transform = [i for i in chunks(q.toMat4().toList(rowmajor=True),4)]
self.plane_to_xy_transform = transform
return transform
def createCone(*args, **kwargs):
'''
Function Signatures:
createCone(...)
createCone(radius, ...)
createCone(radius, height, ...)
createCone(radius, height, offset, ...)
Arguments {default value}:
radius
radius of cone {1.0}
height
height of cone {twice value as radius}
offset
offsets the center of the bottom cylinder cap.
Given in form [x,y,z]. { [0.0,0.0,0.0] }
Keywords:
joint
Joint object that the model will rotate around. {None}
ngon
number of edges used to approximate the circle bottom {10}
axis
axis this cone is parallel to, 'x', 'y', or 'z'
'''
if len(args) > 0:
r = float(args[0])
else:
r = 1.0
if len(args) > 1:
height = float(args[1])
else:
height = r * 2.0
if 'ngon' in kwargs:
N = int(kwargs['ngon'])
elif 'Ngon' in kwargs:
N = int(kwargs['Ngon'])
else:
N = 10
if N < 3:
N = 3
if 'axis' in kwargs:
if kwargs['axis'].lower() == 'x':
initialOrintation = quat(math.pi / 2, [0, 1, 0])
elif kwargs['axis'].lower() == 'y':
initialOrintation = quat(-math.pi / 2, [1, 0, 0])
else:
initialOrintation = quat(1)
else:
initialOrintation = quat(1)
vList = []
normList = []
for n in xrange(N):
x1 = r * math.cos(2 * math.pi * n / N)
y1 = r * math.sin(2 * math.pi * n / N)
x2 = r * math.cos(2 * math.pi * (n + 1) / N)
y2 = r * math.sin(2 * math.pi * (n + 1) / N)
# draw cone
f = [[0.0, 0.0, height],
[x2, y2, 0.0],
[x1, y1, 0.0]]
vList.extend(f)
f = numpy.array(f)
norm = numpy.cross(f[2] - f[0], f[1] - f[0])
normList.append(norm)
# draw circle
f = [[0.0, 0.0, 0.0],
[x1, y1, 0.0],
[x2, y2, 0.0]]
vList.extend(f)
f = numpy.array(f)
norm = numpy.cross(f[2] - f[0], f[1] - f[0])
normList.append(norm)
vList = numpy.array(vList)
normList = numpy.array(normList)
# change vertex to new orientation
for i in xrange(len(vList)):
vList[i] = numpy.array(initialOrintation.rotateVec(vList[i]))
# add offset
if len(args) > 2:
vList += numpy.array(args[2], dtype=float)
triVert = numpy.array(range(len(vList)))
triVert = triVert.reshape([len(vList) / 3, 3])
if 'joint' in kwargs:
rect = TriModel(vList, triVert, kwargs['joint'], normalVectors=normList, **kwargs)
else:
rect = TriModel(vList, triVert, normalVectors=normList, **kwargs)
return rect
def createCone(*args, **kwargs):
'''
Function Signatures:
createCone(...)
createCone(radius, ...)
createCone(radius, height, ...)
createCone(radius, height, offset, ...)
Arguments {default value}:
radius
radius of cone {1.0}
height
height of cone {twice value as radius}
offset
offsets the center of the bottom cylinder cap.
Given in form [x,y,z]. { [0.0,0.0,0.0] }
Keywords:
joint
Joint object that the model will rotate around. {None}
ngon
number of edges used to approximate the circle bottom {10}
axis
axis this cone is parallel to, 'x', 'y', or 'z'
'''
if len(args) > 0:
r = float(args[0])
else:
r = 1.0
if len(args) > 1:
height = float(args[1])
else:
height = r * 2.0
if 'ngon' in kwargs:
N = int(kwargs['ngon'])
elif 'Ngon' in kwargs:
N = int(kwargs['Ngon'])
else:
N = 10
if N < 3:
N = 3
if 'axis' in kwargs:
if kwargs['axis'].lower() == 'x':
initialOrintation = quat(math.pi / 2, [0, 1, 0])
elif kwargs['axis'].lower() == 'y':
initialOrintation = quat(-math.pi / 2, [1, 0, 0])
else:
initialOrintation = quat(1)
else:
initialOrintation = quat(1)
vList = []
normList = []
for n in xrange(N):
x1 = r * math.cos(2 * math.pi * n / N)
y1 = r * math.sin(2 * math.pi * n / N)
x2 = r * math.cos(2 * math.pi * (n + 1) / N)
y2 = r * math.sin(2 * math.pi * (n + 1) / N)
# draw cone
f = [[0.0, 0.0, height], [x2, y2, 0.0], [x1, y1, 0.0]]
vList.extend(f)
f = numpy.array(f)
norm = numpy.cross(f[2] - f[0], f[1] - f[0])
normList.append(norm)
# draw circle
f = [[0.0, 0.0, 0.0], [x1, y1, 0.0], [x2, y2, 0.0]]
vList.extend(f)
f = numpy.array(f)
norm = numpy.cross(f[2] - f[0], f[1] - f[0])
normList.append(norm)
vList = numpy.array(vList)
normList = numpy.array(normList)
# change vertex to new orientation
for i in xrange(len(vList)):
vList[i] = numpy.array(initialOrintation.rotateVec(vList[i]))
# add offset
if len(args) > 2:
vList += numpy.array(args[2], dtype=float)
triVert = numpy.array(range(len(vList)))
triVert = triVert.reshape([len(vList) / 3, 3])
if 'joint' in kwargs:
rect = TriModel(vList,
triVert,
kwargs['joint'],
normalVectors=normList,
**kwargs)
else:
rect = TriModel(vList, triVert, normalVectors=normList, **kwargs)
return rect
def quaternionForAxisAngle(angle, axis):
return quat().fromAngleAxis(angle, axis)
def __init__(self, *args, **kwargs):
'''
Function Signatures:
Joint()
Joint(location, ...)
Arguments {default value}:
location
array like object of form [x,y,z] containing x, y, and z coordinates
of this Joint. { [0,0,0] }
Keywords:
parentJoint
Joint object of which this Joint is a child. {None}
models
TriModel object of list of TriModel objects that represent bones
that are attached to this joint. { [] }
name
Name of joint
initialOrientation
quaternion (type cgkit.cgtypes.quat) representing the initial
orientation. { quat(1,0,0,0) }
showAxis
Boolean that determines whether or not a 3D representation of the
Joint is visible. { False }
axisScale
Number that determines the size of the drawn axis. Must be greater
than 0. { 1.0 }
'''
# self.translateMat = cgtypes.mat4.identity()
self.scaleMat = numpy.matrix(numpy.identity(4))
self.rotateMat = numpy.matrix(numpy.identity(4))
if len(args) > 0:
self.location = numpy.array(args[0], dtype=float)
else:
self.location = numpy.array([0.0, 0.0, 0.0], dtype=float)
self.initialLocationMat = numpy.matrix([[1.0, 0.0, 0.0, self.location[0]],
[0.0, 1.0, 0.0, self.location[1]],
[0.0, 0.0, 1.0, self.location[2]],
[0.0, 0.0, 0.0, 1.0]])
self.translateMat = numpy.matrix([[1.0, 0.0, 0.0, self.location[0]],
[0.0, 1.0, 0.0, self.location[1]],
[0.0, 0.0, 1.0, self.location[2]],
[0.0, 0.0, 0.0, 1.0]])
self.transformICP = numpy.matrix(numpy.identity(4))
self.childJoints = []
# self.scale = 1.0
self.locationUnityScale = self.location.copy()
self.type = 'ball'
self.length = 10.0
self.proximodistalVec = numpy.array([1.0, 0.0, 0.0])
self.secondaryVec = numpy.array([0.0, 1.0, 0.0])
self.tertiaryVec = numpy.array([0.0, 0.0, 1.0])
self.proximodistalVecTransformed = numpy.array([1.0, 0.0, 0.0])
self.secondaryVecTransformed = numpy.array([0.0, 1.0, 0.0])
self.tertiaryVecTransformed = numpy.array([0.0, 0.0, 1.0])
self.proximodistalVecScaled = self.length * self.proximodistalVec
self.proximodistalVecTransformedScaled = self.length * self.proximodistalVecTransformed
self.DOFvec = numpy.array([0, 0, 10.0])
self.DOFangle = math.radians(45.0)
self.DOFtrans = 5.0
if 'parentJoint' in kwargs and isinstance(kwargs['parentJoint'], Joint):
self.parentJoint = kwargs['parentJoint']
self.parentJoint.childJoints.append(self)
else:
self.parentJoint = None
self.models = []
if 'models' in kwargs:
try:
for model in kwargs['models']:
if isinstance(model, TriModel):
self.models.append(model)
self.models[-1].setJoint(self)
except TypeError:
if isinstance(kwargs['models'], TriModel):
self.models.append(kwargs['models'])
self.models[-1].setJoint(self)
if 'initialOrientation' in kwargs and isinstance(kwargs['initialOrientation'], quat):
self.orientation = kwargs['initialOrientation']
else:
self.orientation = quat(1, 0, 0, 0)
self.xAngle = 0.0
self.yAngle = 0.0
self.zAngle = 0.0
if 'showAxis' in kwargs and isinstance(kwargs['showAxis'], bool):
self.showAxis = kwargs['showAxis']
else:
self.showAxis = False
if 'axisScale' in kwargs and kwargs['axisScale'] > 0.0:
self.axisScale = kwargs['axisScale']
else:
self.axisScale = 1.0
if 'name' in kwargs:
self.name = kwargs['name']
else:
self.name = 'Joint'
for model in self.models:
model.initialRotationCenter = self.location.copy()
if self.parentJoint is None:
self.initalLocationRelativeToParentJoint = self.location.copy()
self.initialRelativeOrientationFromParent = self.orientation * quat(1, 0, 0, 0).inverse()
else:
self.initalLocationRelativeToParentJoint = self.location - self.parentJoint.location
self.initialRelativeOrientationFromParent = self.orientation * self.parentJoint.orientation.inverse()
self.relativeOrientationFromParent = quat(self.initialRelativeOrientationFromParent)
self.createAxis(self.showAxis)
def rotate(self, *args, **kwargs):
'''
Function Signatures:
rotate(q, ...)
rotate(mat, ...)
rotate(angle, axis, ...)
rotate(xAngle, yAngle, zAngle, ...)
rotate(elevation, azimuth, spin, sphericalCoord=True, ...)
Arguments {default value}:
q
quaternion (cgkit.cgtypes.quat) that defines joint orientation
(relative or absolute)
mat
4x4 rotation matrix (cgkit.cgtypes.mat4) that defines joint orientation
angle
angle (degrees or radians, radians default) which to rotate around
axis
axis
vector [i,j,k] which defines axis to rotate around
xAngle
angle (degrees or radians, radians default) which to rotate around
x axis
yAngle
angle (degrees or radians, radians default) which to rotate around
y axis
zAngle
angle (degrees or radians, radians default) which to rotate around
z axis
elevation
elevation angle (spherical coordinate system)
azimuth
azimuth angle (spherical coordinate system)
spin
spin angle around vector defined by elevation and azimuth
Keywords:
relative
defines whether the change in orientation is relative or absolute
{False}
updateModels
flag that determines if child models should be updated {True}
angleOrder
string that determines the order that Euler angle rotations are
applied. {'xyz'}
unitsDegrees
flag that indicates what units the passed angles are in, degrees or
radians. {False}
sphericalCoord
flag that indicates passed angles are spherical coordinates. In the
spherical coordinate system, elevation rotates around the x axis
first, then azimuth rotates around the y axis, finally spin rotates
around the vector created by elevation and azimuth {False}
'''
if 'relative' in kwargs:
relative = kwargs['relative']
else:
relative = False
if 'updateModels' in kwargs:
updateModels = kwargs['updateModels']
else:
updateModels = True
if 'sphericalCoord' in kwargs:
sphericalCoord = kwargs['sphericalCoord']
else:
sphericalCoord = False
# the baseOrientation of the joint is either its current orientation, relativeAngle=True
# or the baseOrientation is the initial relative orientation from the parent joint, relativeAngle=False
if relative:
baseOrientation = quat(self.orientation)
# change by original orientation difference, to regain original orientation, relative to parent
elif self.parentJoint is None:
baseOrientation = self.initialRelativeOrientationFromParent * quat(1, 0, 0, 0)
else:
baseOrientation = self.initialRelativeOrientationFromParent * self.parentJoint.orientation
if len(args) == 1: # Quaternion rotation
if isinstance(args[0], quat):
rotateMat = args[0].toMat4()
else:
rotateMat = args[0]
elif len(args) == 2: # angle and axis rotation
angle = args[0]
axis = args[1]
# convert angle units to radians if required
if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
angle = math.radians(angle)
angle = NormalizeAngleRad(angle)
# create rotate matrix
rotateMat = numpyTransform.rotation(angle, axis, N=4)
elif len(args) == 3: # Euler angle rotation
xAngle = args[0]
yAngle = args[1]
zAngle = args[2]
if 'angleOrder' in kwargs and not sphericalCoord:
angleOrder = kwargs['angleOrder'].lower()
if len(angleOrder) != 3 or angleOrder.find('x') < 0 or angleOrder.find('y') < 0 or angleOrder.find('z') < 0:
raise Exception('invalid angle order string')
else:
angleOrder = 'xyz'
if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
xAngle = math.radians(xAngle)
yAngle = math.radians(yAngle)
zAngle = math.radians(zAngle)
if 'relative' in kwargs and kwargs['relative']:
self.xAngle += xAngle
self.yAngle += yAngle
self.zAngle += zAngle
else:
self.xAngle = xAngle
self.yAngle = yAngle
self.zAngle = zAngle
if sphericalCoord:
self.xAngle = NormalizeAngleRad(self.xAngle, -math.pi / 2, math.pi / 2, math.pi)
self.yAngle = NormalizeAngleRad(self.yAngle)
self.zAngle = NormalizeAngleRad(self.zAngle)
else:
self.xAngle = NormalizeAngleRad(self.xAngle)
self.yAngle = NormalizeAngleRad(self.yAngle)
self.zAngle = NormalizeAngleRad(self.zAngle)
rotateMat = numpy.matrix(numpy.identity(4))
# create orientation quaternion by multiplying rotations around local
# x,y, and z axis
# TODO: maybe flip the order of this rotation application?
# FIXME: Spherical rotation not working
for i in xrange(3):
if angleOrder[i] == 'x':
rotateMat *= numpyTransform.rotation(self.xAngle, [1.0, 0.0, 0.0], N=4)
if angleOrder[i] == 'y':
rotateMat *= numpyTransform.rotation(self.yAngle, [0.0, 1.0, 0.0], N=4)
if angleOrder[i] == 'z':
rotateMat *= numpyTransform.rotation(self.zAngle, [0.0, 0.0, 1.0], N=4)
else: # invalid signature
raise Exception("Invalid Function Signature")
if relative:
self.rotateMat *= rotateMat
else:
self.rotateMat = rotateMat
def get_quaternion_from_matrix(matrix):
q = quat()
mat = mat3([num for row in matrix for num in row])
mat = mat.decompose()[0]
q.fromMat(mat)
return q
import os
import cgkit.cgtypes as cgtypes
import fsee
import fsee.Observer
import fsee.plot_utils
import pylab
import sys
pos_vec3 = cgtypes.vec3( 600, 400, 200)
ori_quat = cgtypes.quat( 1, 0, 0, 0)
model_path = os.path.join(fsee.data_dir,"models/mamarama_checkerboard/mamarama_checkerboard.osg")
optics = 'buchner71'
vision = fsee.Observer.Observer(model_path=model_path,
scale=1000.0, # convert model from meters to mm
hz=200.0,
skybox_basename=None,
full_spectrum=True,
optics=optics,
do_luminance_adaptation=False,
)
vision.step(pos_vec3,ori_quat)
#vision.save_last_environment_map('simple_save_envmap.png')
R=vision.get_last_retinal_imageR()
G=vision.get_last_retinal_imageG()
B=vision.get_last_retinal_imageB()
emds = vision.get_last_emd_outputs()
for fname in ['optics.png',
def __init__(self):
self.cameraQuat = quat(1) # identity quaternion
self.camLoc = numpy.array([0.0, 0.0, 2.0])
self.camFocus = numpy.array([0.0, 0.0, 0.0])
self.camUp = numpy.array([0.0, 1.0, 0.0])
def setView(self, *args, **kwargs):
'''
function signatures:
setView(self, vMin, vMax, view='ortho1')
setView(self, orientation, position, focusPoint):
'''
if isinstance(args[0], quat) and len(args) >= 3:
self.cameraQuat = args[0]
self.camLoc = numpy.array(args[1])
self.camFocus = numpy.array(args[2])
self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
elif len(args) >= 2:
vMin = numpy.array(args[0])
vMax = numpy.array(args[1])
view = 'ortho1'
if 'view' in kwargs:
view = kwargs['view']
yAngle = None
xAngle = None
diameter = 2.0 * numpy.sqrt(numpy.sum((vMax - vMin) ** 2))
self.camFocus = (vMax + vMin) / 2.0
if view == 'top':
yAngle = 0.0
xAngle = -0.5 * math.pi
elif view == 'bottom':
yAngle = 0.0
xAngle = 0.5 * math.pi
elif view == 'right':
yAngle = 0.5 * math.pi
xAngle = 0.0
elif view == 'left':
yAngle = -0.5 * math.pi
xAngle = 0.0
elif view == 'front':
yAngle = 0.0
xAngle = 0.0
elif view == 'back':
yAngle = math.pi
xAngle = 0.0
elif view == 'ortho1':
yAngle = 0.25 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho2':
yAngle = 0.75 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho3':
yAngle = 1.25 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho4':
yAngle = 1.75 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho5':
yAngle = 0.75 * math.pi
xAngle = 0.25 * math.pi
elif view == 'ortho6':
yAngle = 1.25 * math.pi
xAngle = 0.25 * math.pi
elif view == 'ortho7':
yAngle = 1.75 * math.pi
xAngle = 0.25 * math.pi
elif view == 'ortho8':
yAngle = 0.25 * math.pi
xAngle = 0.25 * math.pi
if yAngle is not None and xAngle is not None:
# create quat as rotation around y then around (local)x
startQuat = quat(yAngle, [0, 1, 0])
# startQuat = quat(numpy.pi, startQuat.rotateVec([0,1,0]))*startQuat
self.cameraQuat = quat(xAngle, startQuat.rotateVec([1, 0, 0])) * startQuat
self.camLoc = self.cameraQuat.rotateVec([0.0, 0.0, diameter])
self.camLoc += self.camFocus
self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
def quaternionForAxisAngle( angle, axis ):
return quat().fromAngleAxis( angle, axis )
def addBP(self, bp, parent=None, joint_end=None):
"""Recursively add BodyParts to the simulation.
bp -- current BodyPart to process
parent -- parent BP to add to
joint_end -- which end of the parent ccylinder we should add to"""
log.debug('addBP')
body = ode.Body(self.world)
mass = ode.Mass()
if not parent:
# if this is the root we have an absolute length,
# root scale is relative to midpoint of min..max
bp.length = bp.scale * (
bpg.BP_MIN_LENGTH +
(bpg.BP_MAX_LENGTH - bpg.BP_MIN_LENGTH) / 2)
bp.isRoot = 1
else:
# otherwise child scale is relative to parent
bp.length = parent.length * bp.scale
bp.isRoot = 0
# limit the bp length
bp.length = min(bp.length, bpg.BP_MAX_LENGTH)
# mass along x axis, with length without caps==bp.length
# arg 3 means aligned along z-axis - must be same in renderer and Geoms
mass.setCappedCylinder(CYLINDER_DENSITY, 3, CYLINDER_RADIUS, bp.length)
# attach mass to body
body.setMass(mass)
# create Geom
# aligned along z-axis by default!!
geom = ode.GeomCCylinder(self.space, CYLINDER_RADIUS, bp.length)
self.geoms.append(geom)
self.geom_contact[geom] = 0
# remember parent for collison detection
if not parent:
geom.parent = None
else:
geom.parent = parent.geom
# attach geom to body
geom.setBody(body)
log.debug('created CappedCylinder(radius=%f, len=%f)', CYLINDER_RADIUS,
bp.length)
# assert(not in a loop)
assert not hasattr(bp, 'geom')
# ref geom from bodypart (used above to find parent geom)
bp.geom = geom
# set rotation
(radians, v) = bp.rotation
log.debug('radians,v = %f,%s', radians, str(v))
q = quat(radians, vec3(v))
rotmat = q.toMat3()
if parent:
# rotate relative to parent
p_r = mat3(parent.geom.getRotation()) # joint_end *
log.debug('parent rotation = %s', str(p_r))
rotmat = p_r * rotmat
geom.setRotation(rotmat.toList(rowmajor=1))
log.debug('r=%s', str(rotmat))
geom_axis = rotmat * vec3(0, 0, 1)
log.debug('set geom axis to %s', str(geom_axis))
(x, y, z) = geom.getBody().getRelPointPos((0, 0, bp.length / 2.0))
log.debug('real position of joint is %f,%f,%f', x, y, z)
# set position
if not parent:
# root - initially located at 0,0,0
# (once the model is constructed we translate it until all
# bodies have z>0)
geom.setPosition((0, 0, 0))
log.debug('set root geom x,y,z = 0,0,0')
else:
# child - located relative to the parent. from the
# parents position move along their axis of orientation to
# the joint position, then pick a random angle within the
# joint limits, move along that vector by half the length
# of the child cylinder, and we have the position of the
# child.
# vector from parent xyz is half of parent length along
# +/- x axis rotated by r
p_v = vec3(parent.geom.getPosition())
p_r = mat3(parent.geom.getRotation())
p_hl = parent.geom.getParams()[1] / 2.0 # half len of par
j_v = p_v + p_r * vec3(0, 0, p_hl * joint_end) # joint vector
# rotation is relative to parent
c_v = j_v + rotmat * vec3(0, 0, bp.length / 2.0)
geom.setPosition(tuple(c_v))
log.debug('set geom x,y,z = %f,%f,%f', c_v[0], c_v[1], c_v[2])
jointclass = {
'hinge': ode.HingeJoint,
'universal': ode.UniversalJoint,
'ball': ode.BallJoint
}
j = jointclass[bp.joint](self.world)
self.joints.append(j)
# attach bodies to joint
j.attach(parent.geom.getBody(), body)
# set joint position
j.setAnchor(j_v)
geom.parent_joint = j
# create motor and attach to this geom
motor = Motor(self.world, None)
motor.setJoint(j)
self.joints.append(motor)
bp.motor = motor
geom.motor = motor
motor.attach(parent.geom.getBody(), body)
motor.setMode(ode.AMotorEuler)
if bp.joint == 'hinge':
# we have 3 points - parent body, joint, child body
# find the normal to these points
# (hinge only has 1 valid axis!)
try:
axis1 = ((j_v - p_v).cross(j_v - c_v)).normalize()
except ZeroDivisionError:
v = (j_v - p_v).cross(j_v - c_v)
v.z = 1**-10
axis1 = v.normalize()
log.debug('setting hinge joint axis to %s', axis1)
log.debug('hinge axis = %s', j.getAxis())
axis_inv = rotmat.inverse() * axis1
axis2 = vec3((0, 0, 1)).cross(axis_inv)
log.debug('hinge axis2 = %s', axis2)
j.setAxis(tuple(axis1))
# some anomaly here.. if we change the order of axis2 and axis1,
# it should make no difference. instead there appears to be an
# instability when the angle switches from -pi to +pi
# so.. use parameter3 to control the hinge
# (maybe this is only a problem in the test case with perfect axis alignment?)
motor.setAxes(axis2, rotmat * axis1)
elif bp.joint == 'universal':
# bp.axis1/2 is relative to bp rotation, so rotate axes
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
j.setAxis1(tuple(axis1))
j.setAxis2(tuple(axis2))
motor.setAxes(axis1, axis2)
elif bp.joint == 'ball':
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
motor.setAxes(axis1, axis2)
log.debug('created joint with parent at %f,%f,%f', j_v[0], j_v[1],
j_v[2])
# recurse on children
geom.child_joint_ends = set([e.joint_end for e in bp.edges])
geom.parent_joint_end = joint_end
if joint_end == None:
# root
if -1 in geom.child_joint_ends:
geom.left = 'internal'
else:
geom.left = 'external'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
else:
# not root
geom.left = 'internal'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
for e in bp.edges:
self.addBP(e.child, bp, e.joint_end)
data_scale = 1
xgain *= data_scale
ygain *= data_scale
X = (X - xoffset) * xgain
Y = (Y - yoffset) * ygain
if 0:
import pylab
pylab.plot(X[:, 0], Y[:, 0], '.')
pylab.show()
#sys.exit(0)
if 0:
pos_vec3, ori_quat = cgtypes.vec3(nums[0:3]), cgtypes.quat(nums[3:7])
M = cgtypes.mat4().identity().translate(pos_vec3)
M = M * ori_quat.toMat4()
# grr, I hate cgtypes to numpy conversions!
M = np.array((M[0, 0], M[1, 0], M[2, 0], M[3, 0], M[0, 1], M[1, 1],
M[2, 1], M[3, 1], M[0, 2], M[1, 2], M[2, 2], M[3, 2],
M[0, 3], M[1, 3], M[2, 3], M[3, 3]))
M.shape = (4, 4)
fly_model_node_filename = os.path.join(fsee.data_dir,
'models/fly/body.osg')
model_path = os.path.join(fsee.data_dir,
"models/alice_cylinder/alice_cylinder.osg")
model_path = os.path.join(fsee.data_dir,
"models/alice_cylinder/white_cylinder.osg")
#model_path = None
def get_quaternion_from_matrix(matrix):
q = quat()
mat = mat3([num for row in matrix for num in row])
mat = mat.decompose()[0]
q.fromMat(mat)
return q
def run():
f_fts = open(sys.argv[1], 'r')
f_tfs = open(sys.argv[2], 'r')
fn_output = sys.argv[3]
DL = '\t'
tfs = []
tfs_file = ""
for l in f_tfs:
s = l.split(DL)
if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
if len(s) > 0 and s[0] == "tf": tfs.append(s[1:])
if len(s) > 0 and s[0] == "file": tfs_file = s[1]
fts_file = ""
T = None
keypoints = []
KPstart = 0
frames = 0
took = {}
for l in f_fts:
s = l.split(DL)
if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
if len(s) > 0 and s[0] == "file": fts_file = s[1]
if len(s) > 0 and s[0] == "header":
T = None
ts = float(s[1])
best = None
mi = 0.075 #minimum
for t in tfs:
if abs(float(t[0]) - ts) < mi:
mi = abs(float(t[0]) - ts)
best = t
if best == None:
#print "skipping frame as no valid tf was found"
continue
#print mi
T = [
vec4(float(best[1]), float(best[2]), float(best[3]), 1.),
quat(float(v) for v in best[4:])
]
KPstart = len(keypoints)
KPtype = ""
frames += 1
if len(s) > 0 and s[0] == "took":
if not s[1] in took: took[s[1]] = []
took[s[1]].append(reduce(lambda x, y: float(x) + float(y), s[2:]))
if len(s) == 4 and s[0] == "keypoint" and T != None:
pt = vec4(float(s[1]), float(s[2]), float(s[3]), 1.)
TT = T[1].toMat4().setColumn(3, T[0])
#pt = TT.inverse()*pt
pt = TT * pt
keypoints.append({"pt": pt})
#print pt
if len(s) > 4 and s[0] == "keypoint" and T != None:
if KPtype != s[1]:
KPtype = s[1]
KP = KPstart
keypoints[KP][s[1]] = [float(v) for v in s[2:]]
KP += 1
print "read input file ", len(keypoints)
f_out = file(fn_output + "_timing.csv", "w")
i = 1
for ft in took:
aa, bb, cc = mquantiles(took[ft])
f_out.write(str(i) + "\t")
f_out.write(str(min(took[ft])) + "\t")
f_out.write(str(aa) + "\t")
f_out.write(str(bb) + "\t")
f_out.write(str(cc) + "\t")
f_out.write(str(max(took[ft])) + "\t")
f_out.write(
str(reduce(lambda x, y: x + y, took[ft]) / len(took[ft])) + "\t")
f_out.write(ft)
f_out.write("\n")
i += 1
f_out.close()
print "written timing output"
borders = [x / 20. for x in range(1, 31)]
avg = {}
fts = []
for i in xrange(len(keypoints)):
if len(keypoints) > 1000:
if (i) % (len(keypoints) / 1000) == 0:
print "#",
sys.stdout.flush()
#if i==3: exit()
kp1 = keypoints[i]
for j in range(i + 1, len(keypoints)):
kp2 = keypoints[j]
L = (kp1["pt"] - kp2["pt"]).length()
#print L,"\t",
for ft in kp1:
if ft == "pt" or not ft in kp2: continue
#D=dist(kp1[ft], kp2[ft], ft)
#if D==0:
# #print i,j, kp1["pt"], kp2["pt"]
# if i+1==j: continue
#print D,"\t",
if not ft in avg:
avg[ft] = 0.
avg[ft + "num"] = 0
avg[ft + "b"] = 0.
avg[ft + "bnum"] = 0
#avg[ft+"ar"]=[]
#avg[ft+"bar"]=[]
avg[ft + "border"] = [[] for x in xrange(len(borders))]
#avg[ft+"bborder"]=[[] for x in xrange(len(borders))]
fts.append(ft)
#p=""
#if L>0.7/3: p="b"
#avg[ft+p]+=D
#avg[ft+p+"num"]+=1
#avg[ft+p+"ar"].append(D)
D = False
for b in range(int(L / 0.05),
len(borders)): #for b in xrange(len(borders)):
pp = ""
if L > borders[b]: continue #pp="b"
if D == False: D = dist(kp1[ft], kp2[ft], ft)
avg[ft + pp + "border"][b].append(D)
#print ""
print "frames ", frames
for ft in fts:
#if avg[ft+"bnum"]>0 and avg[ft+"bnum"] and avg[ft]>0:
# print ft, avg[ft+"num"], avg[ft+"bnum"], avg[ft]/avg[ft+"num"], " ", avg[ft+"b"]/avg[ft+"bnum"], " -> ", (avg[ft+"b"]/avg[ft+"bnum"])/(avg[ft]/avg[ft+"num"])
#else:
# print ft," is emtpy"
print ft
#print min(avg[ft+"ar"]), mquantiles(avg[ft+"ar"]), max(avg[ft+"ar"])
#print min(avg[ft+"bar"]), mquantiles(avg[ft+"bar"]), max(avg[ft+"bar"])
f_out = file(fn_output + "_" + ft + ".csv", "w")
aa, bb, cc = mquantiles(avg[ft + "border"][len(borders) - 1])
FACT = 1 / bb
for pp in [""]: #["", "b"]:
for b in xrange(len(borders)):
if len(avg[ft + pp + "border"][b]) < 1: continue
f_out.write(str(borders[b]) + "\t")
aa, bb, cc = mquantiles(avg[ft + pp + "border"][b])
f_out.write(str(min(avg[ft + pp + "border"][b]) * FACT) + "\t")
f_out.write(str(aa * FACT) + "\t")
f_out.write(str(bb * FACT) + "\t")
f_out.write(str(cc * FACT) + "\t")
f_out.write(str(max(avg[ft + pp + "border"][b]) * FACT) + "\t")
f_out.write(
str(
reduce(lambda x, y: x + y, avg[ft + pp + "border"][b])
/ len(avg[ft + pp + "border"][b]) * FACT))
f_out.write("\n")
#print ""
f_out.close()
assert (tfs_file == fts_file)
def run():
f_fts = open(sys.argv[1], 'r')
f_tfs = open(sys.argv[2], 'r')
fn_output = sys.argv[3]
DL = '\t'
tfs = []
tfs_file = ""
for l in f_tfs:
s = l.split(DL)
if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
if len(s) > 0 and s[0] == "tf": tfs.append(s[1:])
if len(s) > 0 and s[0] == "file": tfs_file = s[1]
fts_file = ""
T = None
frames = []
took = {}
radius = 0
for l in f_fts:
s = l.split(DL)
if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
if len(s) > 0 and s[0] == "file": fts_file = s[1]
if len(s) > 0 and s[0] == "header":
T = None
ts = float(s[1])
radius = None
best = None
mi = 0.075 #minimum
for t in tfs:
if abs(float(t[0]) - ts) < mi:
mi = abs(float(t[0]) - ts)
best = t
if best == None:
#print "skipping frame as no valid tf was found"
continue
T = [
vec4(float(best[1]), float(best[2]), float(best[3]), 1.),
quat(float(v) for v in best[4:])
]
frames.append({})
elif radius == None:
radius = float(s[1])
if len(s) > 0 and s[0] == "took":
name = '_'.join(s[1:4])
if not name in took: took[name] = []
if not name in frames[len(frames) - 1]:
frames[len(frames) - 1][name] = []
took[name].append(
float(reduce(lambda x, y: float(x) + float(y), s[4:])))
if len(s) == 7 and s[0] == "eval_keypoint" and T != None:
name = '_'.join(s[1:4])
pt = vec4(float(s[4]), float(s[5]), float(s[6]), 1.)
TT = T[1].toMat4().setColumn(3, T[0])
#pt = TT.inverse()*pt
pt = TT * pt
frames[len(frames) - 1][name].append({"pt": pt})
print "read input file "
f_out = file(fn_output + "_timing.csv", "w")
i = 1
for ft in took:
aa, bb, cc = mquantiles(took[ft])
f_out.write(str(i) + "\t")
f_out.write(str(min(took[ft])) + "\t")
f_out.write(str(aa) + "\t")
f_out.write(str(bb) + "\t")
f_out.write(str(cc) + "\t")
f_out.write(str(max(took[ft])) + "\t")
f_out.write(
str(reduce(lambda x, y: x + y, took[ft]) / len(took[ft])) + "\t")
f_out.write(ft)
f_out.write("\n")
i += 1
f_out.close()
print "written timing output"
print "frames ", len(frames)
names = [n for n in frames[0]]
f_out = file(fn_output + ".csv", "a")
for name in names:
print name
dist = []
gtp = 0
gfp = 0
ges = 0
for i in xrange(len(frames)):
ges += len(frames[i][name])
for ii in xrange(len(frames[i][name])):
kp1 = frames[i][name][ii]
for j in range(i + 1, len(frames)):
tp = 0
fp = 0
mi = radius
for jj in xrange(len(frames[j][name])):
kp2 = frames[j][name][jj]
L2 = (kp1["pt"] - kp2["pt"]).length()
if L2 <= radius / 4:
if tp > 0:
fp += 1
else:
tp += 1
mi = min(mi, L2)
if tp > 0: dist.append(mi)
gtp += tp
gfp += fp
print ""
aa, bb, cc = mquantiles(dist)
f_out.write(str(name) + "\t")
f_out.write(str(gtp) + "\t")
f_out.write(str(gfp) + "\t")
f_out.write(str(ges) + "\t")
f_out.write(str(len(frames)) + "\t")
f_out.write(str(min(dist)) + "\t")
f_out.write(str(aa) + "\t")
f_out.write(str(bb) + "\t")
f_out.write(str(cc) + "\t")
f_out.write(str(max(dist)) + "\t")
f_out.write(str(reduce(lambda x, y: x + y, dist) / len(dist)) + "\t")
f_out.write(str(max(dist)) + "\n")
f_out.flush()
assert (tfs_file == fts_file)
def run():
global num_radii, num_angles
f_fts = open(sys.argv[1], 'r')
f_tfs = open(sys.argv[2], 'r')
fn_output = sys.argv[3]
DL = '\t'
tfs = []
tfs_file = ""
for l in f_tfs:
s = l.split(DL)
if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
if len(s) > 0 and s[0] == "tf": tfs.append(s[1:])
if len(s) > 0 and s[0] == "file": tfs_file = s[1]
fts_file = ""
T = None
keypoints = []
KPstart = 0
frames = 0
took = {}
for l in f_fts:
s = l.split(DL)
if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
if len(s) > 0 and s[0] == "file": fts_file = s[1]
if len(s) > 0 and s[0] == "num_radii": num_radii = int(s[1])
if len(s) > 0 and s[0] == "num_angles": num_angles = int(s[1])
if len(s) > 0 and s[0] == "header":
T = None
ts = float(s[1])
best = None
mi = 0.075 #minimum
for t in tfs:
if abs(float(t[0]) - ts) < mi:
mi = abs(float(t[0]) - ts)
best = t
if best == None:
#print "skipping frame as no valid tf was found"
continue
#print mi
T = [
vec4(float(best[1]), float(best[2]), float(best[3]), 1.),
quat(float(v) for v in best[4:])
]
KPstart = len(keypoints)
KPtype = ""
frames += 1
if len(s) > 0 and s[0] == "took":
if not s[1] in took: took[s[1]] = []
took[s[1]].append(reduce(lambda x, y: float(x) + float(y), s[2:]))
if len(s) == 4 and s[0] == "keypoint" and T != None:
pt = vec4(float(s[1]), float(s[2]), float(s[3]), 1.)
TT = T[1].toMat4().setColumn(3, T[0])
#pt = TT.inverse()*pt
pt = TT * pt
keypoints.append({"pt": pt})
#print pt
if len(s) > 4 and s[0] == "keypoint" and T != None:
if KPtype != s[1]:
KPtype = s[1]
KP = KPstart
keypoints[KP][s[1]] = [float(v) for v in s[2:]]
KP += 1
print "read input file ", len(keypoints)
borders = [x / 20. for x in range(1, 31)]
avg = {}
avgDBG = {}
for i in xrange(len(keypoints)):
if len(keypoints) > 1000:
if (i) % (len(keypoints) / 1000) == 0:
print "#",
sys.stdout.flush()
if i > 10: break
#if i==3: exit()
fts = []
md = {}
mL = {}
mF = {}
mdDBG = {}
mLDBG = {}
mFDBG = {}
kp1 = keypoints[i]
NNNN = 0
for j in range(i + 1, len(keypoints)):
kp2 = keypoints[j]
L = (kp1["pt"] - kp2["pt"]).length()
#print L,"\t",
for ft in kp1:
if ft == "pt" or not ft in kp2: continue
d = dist(kp1[ft], kp2[ft], ft)
if not ft in md:
md[ft] = None
mL[ft] = None
mdDBG[ft] = None
mLDBG[ft] = None
fts.append(ft)
if md[ft] == None or d < md[ft]:
md[ft] = d
mL[ft] = L
mF[ft] = [kp1[ft], kp2[ft]]
if mdDBG[ft] == None or L < mLDBG[ft]:
mdDBG[ft] = d
mLDBG[ft] = L
mFDBG[ft] = [kp1[ft], kp2[ft]]
'''for ft in kp1:
if ft=="pt" or not ft in kp2: continue
d=dist(kp1[ft], kp2[ft], ft)
if d<=mdDBG[ft]:
NNNN+=1
print L'''
for ft in fts:
if md[ft] == None: continue
if not ft in avg:
avg[ft] = []
avgDBG[ft] = []
avg[ft].append([md[ft], mL[ft], mF[ft]])
avgDBG[ft].append([mdDBG[ft], mLDBG[ft], mFDBG[ft]])
print ft, NNNN
print md[ft], mL[ft]
print mdDBG[ft], mLDBG[ft]
print mFDBG[ft][0]
print mFDBG[ft][1]
print mF[ft][1]
print "frames ", frames
print avg
print avgDBG
for b in xrange(len(borders)):
for ft in avg:
f1 = file(fn_output + "_" + ft + "_pr_" + str(borders[b]) + ".csv",
"w")
R = calc_pr(avg[ft], borders[b])
for r in R:
f1.write(str(r[0]))
for i in range(1, len(r)):
f1.write("\t" + str(r[i]))
f1.write("\n")
f1.close()
assert (tfs_file == fts_file)
def __init__(self):
self.cameraQuat = quat(1) # identity quaternion
self.camLoc = numpy.array([0.0, 0.0, 2.0])
self.camFocus = numpy.array([0.0, 0.0, 0.0])
self.camUp = numpy.array([0.0, 1.0, 0.0])
def setView(self, *args, **kwargs):
'''
function signatures:
setView(self, vMin, vMax, view='ortho1')
setView(self, orientation, position, focusPoint):
'''
if isinstance(args[0], quat) and len(args) >= 3:
self.cameraQuat = args[0]
self.camLoc = numpy.array(args[1])
self.camFocus = numpy.array(args[2])
self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0,
0.0]))
elif len(args) >= 2:
vMin = numpy.array(args[0])
vMax = numpy.array(args[1])
view = 'ortho1'
if 'view' in kwargs:
view = kwargs['view']
yAngle = None
xAngle = None
diameter = 2.0 * numpy.sqrt(numpy.sum((vMax - vMin)**2))
self.camFocus = (vMax + vMin) / 2.0
if view == 'top':
yAngle = 0.0
xAngle = -0.5 * math.pi
elif view == 'bottom':
yAngle = 0.0
xAngle = 0.5 * math.pi
elif view == 'right':
yAngle = 0.5 * math.pi
xAngle = 0.0
elif view == 'left':
yAngle = -0.5 * math.pi
xAngle = 0.0
elif view == 'front':
yAngle = 0.0
xAngle = 0.0
elif view == 'back':
yAngle = math.pi
xAngle = 0.0
elif view == 'ortho1':
yAngle = 0.25 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho2':
yAngle = 0.75 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho3':
yAngle = 1.25 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho4':
yAngle = 1.75 * math.pi
xAngle = -0.25 * math.pi
elif view == 'ortho5':
yAngle = 0.75 * math.pi
xAngle = 0.25 * math.pi
elif view == 'ortho6':
yAngle = 1.25 * math.pi
xAngle = 0.25 * math.pi
elif view == 'ortho7':
yAngle = 1.75 * math.pi
xAngle = 0.25 * math.pi
elif view == 'ortho8':
yAngle = 0.25 * math.pi
xAngle = 0.25 * math.pi
if yAngle is not None and xAngle is not None:
# create quat as rotation around y then around (local)x
startQuat = quat(yAngle, [0, 1, 0])
# startQuat = quat(numpy.pi, startQuat.rotateVec([0,1,0]))*startQuat
self.cameraQuat = quat(xAngle, startQuat.rotateVec(
[1, 0, 0])) * startQuat
self.camLoc = self.cameraQuat.rotateVec([0.0, 0.0, diameter])
self.camLoc += self.camFocus
self.camUp = numpy.array(
self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
def rotate(self, *args, **kwargs):
'''
Function Signatures:
rotate(q, ...)
rotate(mat, ...)
rotate(angle, axis, ...)
rotate(xAngle, yAngle, zAngle, ...)
rotate(elevation, azimuth, spin, sphericalCoord=True, ...)
Arguments {default value}:
q
quaternion (cgkit.cgtypes.quat) that defines joint orientation
(relative or absolute)
mat
4x4 rotation matrix (cgkit.cgtypes.mat4) that defines joint orientation
angle
angle (degrees or radians, radians default) which to rotate around
axis
axis
vector [i,j,k] which defines axis to rotate around
xAngle
angle (degrees or radians, radians default) which to rotate around
x axis
yAngle
angle (degrees or radians, radians default) which to rotate around
y axis
zAngle
angle (degrees or radians, radians default) which to rotate around
z axis
elevation
elevation angle (spherical coordinate system)
azimuth
azimuth angle (spherical coordinate system)
spin
spin angle around vector defined by elevation and azimuth
Keywords:
relative
defines whether the change in orientation is relative or absolute
{False}
updateModels
flag that determines if child models should be updated {True}
angleOrder
string that determines the order that Euler angle rotations are
applied. {'xyz'}
unitsDegrees
flag that indicates what units the passed angles are in, degrees or
radians. {False}
sphericalCoord
flag that indicates passed angles are spherical coordinates. In the
spherical coordinate system, elevation rotates around the x axis
first, then azimuth rotates around the y axis, finally spin rotates
around the vector created by elevation and azimuth {False}
'''
if 'relative' in kwargs:
relative = kwargs['relative']
else:
relative = False
if 'updateModels' in kwargs:
updateModels = kwargs['updateModels']
else:
updateModels = True
if 'sphericalCoord' in kwargs:
sphericalCoord = kwargs['sphericalCoord']
else:
sphericalCoord = False
# the baseOrientation of the joint is either its current orientation, relativeAngle=True
# or the baseOrientation is the initial relative orientation from the parent joint, relativeAngle=False
if relative:
baseOrientation = quat(self.orientation)
# change by original orientation difference, to regain original orientation, relative to parent
elif self.parentJoint is None:
baseOrientation = self.initialRelativeOrientationFromParent * quat(
1, 0, 0, 0)
else:
baseOrientation = self.initialRelativeOrientationFromParent * self.parentJoint.orientation
if len(args) == 1: # Quaternion rotation
if isinstance(args[0], quat):
rotateMat = args[0].toMat4()
else:
rotateMat = args[0]
elif len(args) == 2: # angle and axis rotation
angle = args[0]
axis = args[1]
# convert angle units to radians if required
if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
angle = math.radians(angle)
angle = NormalizeAngleRad(angle)
# create rotate matrix
rotateMat = numpyTransform.rotation(angle, axis, N=4)
elif len(args) == 3: # Euler angle rotation
xAngle = args[0]
yAngle = args[1]
zAngle = args[2]
if 'angleOrder' in kwargs and not sphericalCoord:
angleOrder = kwargs['angleOrder'].lower()
if len(angleOrder) != 3 or angleOrder.find(
'x') < 0 or angleOrder.find(
'y') < 0 or angleOrder.find('z') < 0:
raise Exception('invalid angle order string')
else:
angleOrder = 'xyz'
if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
xAngle = math.radians(xAngle)
yAngle = math.radians(yAngle)
zAngle = math.radians(zAngle)
if 'relative' in kwargs and kwargs['relative']:
self.xAngle += xAngle
self.yAngle += yAngle
self.zAngle += zAngle
else:
self.xAngle = xAngle
self.yAngle = yAngle
self.zAngle = zAngle
if sphericalCoord:
self.xAngle = NormalizeAngleRad(self.xAngle, -math.pi / 2,
math.pi / 2, math.pi)
self.yAngle = NormalizeAngleRad(self.yAngle)
self.zAngle = NormalizeAngleRad(self.zAngle)
else:
self.xAngle = NormalizeAngleRad(self.xAngle)
self.yAngle = NormalizeAngleRad(self.yAngle)
self.zAngle = NormalizeAngleRad(self.zAngle)
rotateMat = numpy.matrix(numpy.identity(4))
# create orientation quaternion by multiplying rotations around local
# x,y, and z axis
# TODO: maybe flip the order of this rotation application?
# FIXME: Spherical rotation not working
for i in xrange(3):
if angleOrder[i] == 'x':
rotateMat *= numpyTransform.rotation(self.xAngle,
[1.0, 0.0, 0.0],
N=4)
if angleOrder[i] == 'y':
rotateMat *= numpyTransform.rotation(self.yAngle,
[0.0, 1.0, 0.0],
N=4)
if angleOrder[i] == 'z':
rotateMat *= numpyTransform.rotation(self.zAngle,
[0.0, 0.0, 1.0],
N=4)
else: # invalid signature
raise Exception("Invalid Function Signature")
if relative:
self.rotateMat *= rotateMat
else:
self.rotateMat = rotateMat
def __init__(self, *args, **kwargs):
'''
Function Signatures:
Joint()
Joint(location, ...)
Arguments {default value}:
location
array like object of form [x,y,z] containing x, y, and z coordinates
of this Joint. { [0,0,0] }
Keywords:
parentJoint
Joint object of which this Joint is a child. {None}
models
TriModel object of list of TriModel objects that represent bones
that are attached to this joint. { [] }
name
Name of joint
initialOrientation
quaternion (type cgkit.cgtypes.quat) representing the initial
orientation. { quat(1,0,0,0) }
showAxis
Boolean that determines whether or not a 3D representation of the
Joint is visible. { False }
axisScale
Number that determines the size of the drawn axis. Must be greater
than 0. { 1.0 }
'''
# self.translateMat = cgtypes.mat4.identity()
self.scaleMat = numpy.matrix(numpy.identity(4))
self.rotateMat = numpy.matrix(numpy.identity(4))
if len(args) > 0:
self.location = numpy.array(args[0], dtype=float)
else:
self.location = numpy.array([0.0, 0.0, 0.0], dtype=float)
self.initialLocationMat = numpy.matrix(
[[1.0, 0.0, 0.0, self.location[0]],
[0.0, 1.0, 0.0, self.location[1]],
[0.0, 0.0, 1.0, self.location[2]], [0.0, 0.0, 0.0, 1.0]])
self.translateMat = numpy.matrix([[1.0, 0.0, 0.0, self.location[0]],
[0.0, 1.0, 0.0, self.location[1]],
[0.0, 0.0, 1.0, self.location[2]],
[0.0, 0.0, 0.0, 1.0]])
self.transformICP = numpy.matrix(numpy.identity(4))
self.childJoints = []
# self.scale = 1.0
self.locationUnityScale = self.location.copy()
self.type = 'ball'
self.length = 10.0
self.proximodistalVec = numpy.array([1.0, 0.0, 0.0])
self.secondaryVec = numpy.array([0.0, 1.0, 0.0])
self.tertiaryVec = numpy.array([0.0, 0.0, 1.0])
self.proximodistalVecTransformed = numpy.array([1.0, 0.0, 0.0])
self.secondaryVecTransformed = numpy.array([0.0, 1.0, 0.0])
self.tertiaryVecTransformed = numpy.array([0.0, 0.0, 1.0])
self.proximodistalVecScaled = self.length * self.proximodistalVec
self.proximodistalVecTransformedScaled = self.length * self.proximodistalVecTransformed
self.DOFvec = numpy.array([0, 0, 10.0])
self.DOFangle = math.radians(45.0)
self.DOFtrans = 5.0
if 'parentJoint' in kwargs and isinstance(kwargs['parentJoint'],
Joint):
self.parentJoint = kwargs['parentJoint']
self.parentJoint.childJoints.append(self)
else:
self.parentJoint = None
self.models = []
if 'models' in kwargs:
try:
for model in kwargs['models']:
if isinstance(model, TriModel):
self.models.append(model)
self.models[-1].setJoint(self)
except TypeError:
if isinstance(kwargs['models'], TriModel):
self.models.append(kwargs['models'])
self.models[-1].setJoint(self)
if 'initialOrientation' in kwargs and isinstance(
kwargs['initialOrientation'], quat):
self.orientation = kwargs['initialOrientation']
else:
self.orientation = quat(1, 0, 0, 0)
self.xAngle = 0.0
self.yAngle = 0.0
self.zAngle = 0.0
if 'showAxis' in kwargs and isinstance(kwargs['showAxis'], bool):
self.showAxis = kwargs['showAxis']
else:
self.showAxis = False
if 'axisScale' in kwargs and kwargs['axisScale'] > 0.0:
self.axisScale = kwargs['axisScale']
else:
self.axisScale = 1.0
if 'name' in kwargs:
self.name = kwargs['name']
else:
self.name = 'Joint'
for model in self.models:
model.initialRotationCenter = self.location.copy()
if self.parentJoint is None:
self.initalLocationRelativeToParentJoint = self.location.copy()
self.initialRelativeOrientationFromParent = self.orientation * quat(
1, 0, 0, 0).inverse()
else:
self.initalLocationRelativeToParentJoint = self.location - self.parentJoint.location
self.initialRelativeOrientationFromParent = self.orientation * self.parentJoint.orientation.inverse(
)
self.relativeOrientationFromParent = quat(
self.initialRelativeOrientationFromParent)
self.createAxis(self.showAxis)
data_scale = 1
xgain *= data_scale
ygain *= data_scale
X=(X-xoffset)*xgain
Y=(Y-yoffset)*ygain
if 0:
import pylab
pylab.plot(X[:,0],Y[:,0],'.')
pylab.show()
#sys.exit(0)
if 0:
pos_vec3,ori_quat = cgtypes.vec3(nums[0:3]),cgtypes.quat(nums[3:7])
M = cgtypes.mat4().identity().translate(pos_vec3)
M = M*ori_quat.toMat4()
# grr, I hate cgtypes to numpy conversions!
M = np.array((M[0,0],M[1,0],M[2,0],M[3,0],
M[0,1],M[1,1],M[2,1],M[3,1],
M[0,2],M[1,2],M[2,2],M[3,2],
M[0,3],M[1,3],M[2,3],M[3,3]))
M.shape=(4,4)
fly_model_node_filename = os.path.join(fsee.data_dir,'models/fly/body.osg')
model_path = os.path.join(fsee.data_dir,"models/alice_cylinder/alice_cylinder.osg")
model_path = os.path.join(fsee.data_dir,"models/alice_cylinder/white_cylinder.osg")
#model_path = None
z = 2 # 2 mm
#for j,id in enumerate(ids):