def calibrateInertialTransform(self, pos1, att1, other, pos2, att2, B_r_BC, q_CB, calIDs=[0]):
posID1 = self.getColIDs(pos1)
attID1 = self.getColIDs(att1)
posID2 = other.getColIDs(pos2)
attID2 = other.getColIDs(att2)
# Make timing calculation
dt1 = self.getLastTime()-self.getFirstTime()
dt2 = other.getLastTime()-other.getFirstTime()
first = max(self.getFirstTime(),other.getFirstTime())
last = min(self.getLastTime(),other.getLastTime())
timeIncrement = min(dt1/(self.length()-1), dt2/(other.length()-1))
td1 = TimedData(8);
td2 = TimedData(8);
td1.initEmptyFromTimes(np.arange(first,last,timeIncrement))
td2.initEmptyFromTimes(np.arange(first,last,timeIncrement))
self.interpolateColumns(td1, posID1, [1,2,3])
other.interpolateColumns(td2, posID2, [1,2,3])
self.interpolateQuaternion(td1, attID1, [4,5,6,7])
other.interpolateQuaternion(td2, attID2, [4,5,6,7])
if(not calIDs):
calIDs = range(td1.length())
newIDs = np.arange(0,Utils.getLen(calIDs))
q_CB_vec = np.kron(np.ones([Utils.getLen(calIDs),1]),q_CB)
q_JC_vec = Quaternion.q_inverse(td2.D()[newIDs,4:8])
q_BI_vec = td1.D()[newIDs,4:8]
B_r_BC_vec = np.kron(np.ones([Utils.getLen(calIDs),1]),B_r_BC)
J_r_JC = td2.D()[newIDs,1:4];
J_r_BC = Quaternion.q_rotate(Quaternion.q_mult(q_JC_vec,q_CB_vec), B_r_BC_vec)
J_r_IB = Quaternion.q_rotate(Quaternion.q_mult(q_JC_vec,Quaternion.q_mult(q_CB_vec,q_BI_vec)),td1.D()[newIDs,1:4])
rotation = Quaternion.q_mean(Quaternion.q_inverse(Quaternion.q_mult(Quaternion.q_mult(q_JC_vec,q_CB_vec),q_BI_vec)))
translation = np.mean(J_r_JC-J_r_BC-J_r_IB, axis=0)
return translation.flatten(), rotation.flatten()
def vis_marker_received(self, markerInfo):
markerId, position, orientation = markerInfo.id, markerInfo.pose.position, markerInfo.pose.orientation
# if int(markerId) not in self.allowed: return
# print markerInfo.id
px, py, pz = tuple([position.x, position.y, position.z])
# print distFromCamera
position.z = np.cos(self.angle) * pz - np.sin(self.angle) * py
position.y = np.sin(self.angle) * pz + np.cos(self.angle) * py
q = (orientation.x, orientation.y, orientation.z, orientation.w)
if q in self.cache:
ori = self.cache[q]
markerInfo.pose.orientation.x = ori[0]
markerInfo.pose.orientation.y = ori[1]
markerInfo.pose.orientation.z = ori[2]
markerInfo.pose.orientation.w = ori[3]
else:
originalQuat = Q.Quat(Q.normalize(list(q)))
quatOffsetFix = Q.Quat([0, 0, -1 * self.angle])
fixed = originalQuat * quatOffsetFix
x, y, z, w = tuple(fixed._get_q())
markerInfo.pose.orientation.x = x
markerInfo.pose.orientation.y = y
markerInfo.pose.orientation.z = z
markerInfo.pose.orientation.w = w
self.cache[q] = [x, y, z, w]
self.markerSeen = markerInfo
def calibrateInertialTransform(self, pos1, att1, other, pos2, att2, B_r_BC, q_CB, calIDs=[0]):
posID1 = self.getColIDs(pos1)
attID1 = self.getColIDs(att1)
posID2 = other.getColIDs(pos2)
attID2 = other.getColIDs(att2)
# Make timing calculation
dt1 = self.getLastTime()-self.getFirstTime()
dt2 = other.getLastTime()-other.getFirstTime()
first = max(self.getFirstTime(),other.getFirstTime())
last = min(self.getLastTime(),other.getLastTime())
timeIncrement = min(dt1/(self.length()-1), dt2/(other.length()-1))
td1 = TimedData(8);
td2 = TimedData(8);
td1.initEmptyFromTimes(np.arange(first,last,timeIncrement))
td2.initEmptyFromTimes(np.arange(first,last,timeIncrement))
self.interpolateColumns(td1, posID1, [1,2,3])
other.interpolateColumns(td2, posID2, [1,2,3])
self.interpolateQuaternion(td1, attID1, [4,5,6,7])
other.interpolateQuaternion(td2, attID2, [4,5,6,7])
newIDs = np.arange(0,Utils.getLen(calIDs))
q_CB_vec = np.kron(np.ones([Utils.getLen(calIDs),1]),q_CB)
q_JC_vec = Quaternion.q_inverse(td2.D()[newIDs,4:8])
q_BI_vec = td1.D()[newIDs,4:8]
B_r_BC_vec = np.kron(np.ones([Utils.getLen(calIDs),1]),B_r_BC)
J_r_JC = td2.D()[newIDs,1:4];
J_r_BC = Quaternion.q_rotate(Quaternion.q_mult(q_JC_vec,q_CB_vec), B_r_BC_vec)
J_r_IB = Quaternion.q_rotate(Quaternion.q_mult(q_JC_vec,Quaternion.q_mult(q_CB_vec,q_BI_vec)),td1.D()[newIDs,1:4])
rotation = Quaternion.q_mean(Quaternion.q_inverse(Quaternion.q_mult(Quaternion.q_mult(q_JC_vec,q_CB_vec),q_BI_vec)))
translation = np.mean(J_r_JC-J_r_BC-J_r_IB, axis=0)
return translation.flatten(), rotation.flatten()
def trinterp(T0, T1, r):
"""
Interpolate homogeneous transformations.
Compute a homogeneous transform interpolation between C{T0} and C{T1} as
C{r} varies from 0 to 1 such that::
trinterp(T0, T1, 0) = T0
trinterp(T0, T1, 1) = T1
Rotation is interpolated using quaternion spherical linear interpolation.
@type T0: 4x4 homogeneous transform
@param T0: Initial value
@type T1: 4x4 homogeneous transform
@param T1: Final value
@type r: number
@param r: Interpolation index, in the range 0 to 1 inclusive
@rtype: 4x4 homogeneous transform
@return: Interpolated value
@see: L{quaternion}, L{ctraj}
"""
q0 = Q.quaternion(T0)
q1 = Q.quaternion(T1)
p0 = transl(T0)
p1 = transl(T1)
qr = q0.interp(q1, r)
pr = p0 * (1 - r) + r * p1
return vstack((concatenate((qr.r(), pr), 1), mat([0, 0, 0, 1])))
def OPQ(self):
O = vctr.array2vec(self.array[0:3])
OR0 = vctr.array2vec([0, 0, np.sqrt(0.75) * self.l])
OR1 = qtrn.RotOper(self.Phi, vctr.Vector(0, 1, 0), OR0)
OR2 = qtrn.RotOper(self.Theta, vctr.Vector(0, 0, 1), OR1.purify())
R = vctr.Add(O, OR2)
OR0.contents()
OR1.purify().contents()
OR2.purify().contents()
R.contents()
if self.Phi == 0:
Pr = qtrn.RotOper(self.Theta, vctr.Vector(0, 0, 1),
vctr.Vector(0, 1, 0))
PR0 = vctr.ScalarMul(0.5 * self.l, Pr.purify().unit())
else:
Pr = vctr.Cross(vctr.Vector(0, 0, 1), OR2.purify())
PR0 = vctr.ScalarMul(0.5 * self.l, Pr.unit())
PR = qtrn.RotOper(self.Psy, OR2.purify(), PR0)
#PR0 = vctr.ScalarMul(0.5*self.l,Pr.unit())
P = vctr.Add(R, PR.purify())
Q = vctr.Subtract(R, PR.purify())
p = np.round(P.array().reshape(1, 3), 8)
o = np.round(O.array().reshape(1, 3), 8)
q = np.round(Q.array().reshape(1, 3), 8)
OP_Array = np.append(o, p, axis=0)
OPQ_Array = np.append(OP_Array, q, axis=0)
return OPQ_Array
def trinterp(T0, T1, r):
"""
Interpolate homogeneous transformations.
Compute a homogeneous transform interpolation between C{T0} and C{T1} as
C{r} varies from 0 to 1 such that::
trinterp(T0, T1, 0) = T0
trinterp(T0, T1, 1) = T1
Rotation is interpolated using quaternion spherical linear interpolation.
@type T0: 4x4 homogeneous transform
@param T0: Initial value
@type T1: 4x4 homogeneous transform
@param T1: Final value
@type r: number
@param r: Interpolation index, in the range 0 to 1 inclusive
@rtype: 4x4 homogeneous transform
@return: Interpolated value
@see: L{quaternion}, L{ctraj}
"""
q0 = Quaternion(T0)
q1 = Quaternion(T1)
p0 = transl(T0)
p1 = transl(T1)
qr = q0.interp(q1, r)
pr = p0*(1-r) + r*p1
return vstack( (concatenate((qr.R(),pr),1), mat([0,0,0,1])) )
def joint_quaternion(u, n):
import numpy as np
import Quaternion
# basis vector
e = np.identity(3)[u]
return Quaternion.from_vectors(e, n)
## mtournier: ????
## returns a quaternion that orients the u-th axis to the given axis n
# build an orthogonal basis as a column matrix and convert it to a quaternion
v=(u+1)%3
w=(v+1)%3
import numpy as np
from numpy.linalg import norm
m=np.zeros((3,3))
m[:,u]=n
m[:,u] = m[:,u]/norm(m[:,u]) # normalize to be sure
threshold = 1e-8 # TODO what is the right way to get numeric limits in python?
if abs(n[1]) > threshold: m[:,v] = [0, -n[2], n[1]]
elif abs(n[2]) > threshold: m[:,v] = [n[2], 0, -n[0]]
elif abs(n[0]) > threshold: m[:,v] = [n[1], -n[0], 0]
else: raise Exception("Null normal")
m[:,v] = m[:,v]/norm(m[:,v]) # normalize
m[:,w] = np.cross(m[:,u],m[:,v])
import Quaternion
return Quaternion.from_matrix(m)
def getFInv():
q=Quaternion.Quaternion()
q1=sympy.symbols('v3_q1')
q2=sympy.symbols('v3_q2')
q3=sympy.symbols('v3_q3')
q4=sympy.symbols('v3_q4')
q.setValues(q1,q2,q3,q4)
pt3=Quaternion.Quaternion()
X=sympy.symbols('v2_X')
Y=sympy.symbols('v2_Y')
Z=sympy.symbols('v2_Z')
d=sympy.symbols('v2_d')
pt3.setValues(0,X/d,Y/d,Z/d)
CX,CY,CZ=sympy.symbols('v3_CX,v3_CY,v3_CZ')
pt3c=q.conj()*pt3*q
pt3c[1]=pt3c[1]+CX
pt3c[2]=pt3c[2]+CY
pt3c[3]=pt3c[3]+CZ
CX2,CY2,CZ2=sympy.symbols('v4_CX,v4_CY,v4_CZ')
qq2=Quaternion.Quaternion()
q12=sympy.symbols('v4_q1')
q22=sympy.symbols('v4_q2')
q32=sympy.symbols('v4_q3')
q42=sympy.symbols('v4_q4')
qq2.setValues(q12,q22,q32,q42)
pt3c[1]=pt3c[1]-CX2
pt3c[2]=pt3c[2]-CY2
pt3c[3]=pt3c[3]-CZ2
pt3c2=qq2*pt3c*qq2.conj()
fx,fy,u0,v0,s,k1=sympy.symbols('v5_fx,v5_fy,v5_u0,v5_v0,v5_s,v5_k1')
ptx=pt3c2[1]/pt3c2[3]
pty=pt3c2[2]/pt3c2[3]
ptx2=ptx*fx+u0
pty2=pty*fy+v0
pt=sympy.Matrix([ptx2,pty2])
ptxd,ptyd=sympy.symbols('v1_x, v1_y')
r2=k1*(ptxd*ptx+ptyd*ptyd)
ptx=(1.0+r2)*ptxd
pty=(1.0+r2)*ptyd
pt[0]=((ptx-pt[0]))
pt[1]=((pty-pt[1]))
return pt
def applyBodyTransformToTwist(self, velID, rorID, translation, rotation):
newVel = Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),rotation),
self.cols(velID) \
+ np.cross(self.cols(rorID),np.kron(np.ones([self.length(),1]),translation)))
newRor = Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),rotation),
self.cols(rorID))
for i in np.arange(0,3):
self.setCol(newVel[:,i],velID[i])
self.setCol(newRor[:,i],rorID[i])
def __mul__(self, other): res = Frame() res.translation = vec.sum(self.translation, quat.rotate(self.rotation, other.translation)) res.rotation = quat.prod( self.rotation, other.rotation) return res
def cube_axes(N=1, **kwargs):
"""Create an N x N x N rubiks cube
kwargs are passed to the PolyView3D instance.
"""
stickerwidth = 0.9
small = 0.5 * (1. - stickerwidth)
d1 = 1 - small
d2 = 1 - 2 * small
d3 = 1.01
base_sticker = np.array([[d1, d2, d3], [d2, d1, d3],
[-d2, d1, d3], [-d1, d2, d3],
[-d1, -d2, d3], [-d2, -d1, d3],
[d2, -d1, d3], [d1, -d2, d3],
[d1, d2, d3]], dtype=float)
base_face = np.array([[1, 1, 1],
[1, -1, 1],
[-1, -1, 1],
[-1, 1, 1],
[1, 1, 1]], dtype=float)
x, y, z = np.eye(3)
rots = [Quaternion.from_v_theta(x, theta)
for theta in (np.pi / 2, -np.pi / 2)]
rots += [Quaternion.from_v_theta(y, theta)
for theta in (np.pi / 2, -np.pi / 2, np.pi, 2 * np.pi)]
cubie_width = 2. / N
translations = np.array([[-1 + (i + 0.5) * cubie_width,
-1 + (j + 0.5) * cubie_width, 0]
for i in range(N) for j in range(N)])
colors = ['blue', 'green', 'white', 'yellow', 'orange', 'red']
factor = np.array([1. / N, 1. / N, 1])
ax = PolyView3D(**kwargs)
facecolor = []
polys = []
for t in translations:
base_face_trans = factor * base_face + t
base_sticker_trans = factor * base_sticker + t
for r, c in zip(rots, colors):
polys += [r.rotate(base_face_trans),
r.rotate(base_sticker_trans)]
facecolor += ['k', c]
ax.poly3D_batch(polys, facecolor=facecolor)
ax.figure.text(0.05, 0.05,
("Drag Mouse or use arrow keys to change perspective.\n"
"Hold shift to adjust z-axis rotation"),
ha='left', va='bottom')
return ax
def applyInertialTransform(self, posID, attID, translation, rotation):
newTranslation = np.kron(np.ones([self.length(),1]),translation) \
+ Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),Quaternion.q_inverse(rotation)),
self.cols(posID))
newRotation = Quaternion.q_mult(self.cols(attID),
np.kron(np.ones([self.length(),1]),rotation))
for i in np.arange(0,3):
self.setCol(newTranslation[:,i],posID[i])
for i in np.arange(0,4):
self.setCol(newRotation[:,i],attID[i])
def dis4DoF(gripperOrientation, simpleL1=True):
gripperQ = Quaternion()
gripperQ.rotationMatrixInit(gripperOrientation)
if simpleL1:
return abs(gripperQ.x) + abs(gripperQ.y)
else:
# return sqrt(dof4Q.x * dof4Q.x + dof4Q.y * dof4Q.y)
# return acos(sqrt(dof4Q.w * dof4Q.w + dof4Q.z * dof4Q.z))
return 1 - sqrt(gripperQ.w * gripperQ.w + gripperQ.z * gripperQ.z)
def transformRatesFromWorldToBody(self, colAtt, colVel, colRor):
colAttIDs = self.getColIDs(colAtt)
colVelIDs = self.getColIDs(colVel)
colRorIDs = self.getColIDs(colRor)
self.setCol(
Quaternion.q_rotate(self.col(colAttIDs), self.col(colRorIDs)),
colRorIDs)
self.setCol(
Quaternion.q_rotate(self.col(colAttIDs), self.col(colVelIDs)),
colVelIDs)
def YtoZ(Y):
Z = np.zeros((7, 3))
quatg = np.array([0, 0, 0, 1])
for i in range(7):
qk = Y[i, :]
qkinv = Quaternion.inverse_quaternion(qk)
prod = Quaternion.multiply_quaternion(
Quaternion.multiply_quaternion(qkinv, quatg), qk)
Z[i, :] = Quaternion.quattovec(prod)
#print(Z.T)
return Z
def XtoY(X, deltat, omegak):
Y = np.zeros((7, 4))
norm_omegak = np.linalg.norm(omegak)
if norm_omegak == 0:
qdelta = np.array([1, 0, 0, 0])
else:
edelta = omegak * deltat
qdelta = Quaternion.vectoquat(edelta)
for i in range(7):
Y[i, :] = Quaternion.multiply_quaternion(X[i, :], qdelta)
return Y
def getContactPoint(gripperLength, gripperCenter, gripperOrientation):
rotation = Quaternion()
rotation.listInit(gripperOrientation)
# print(rotation.getMatrix())
contact1Relative = [- gripperLength / 2, 0, 0]
contact2Relative = [gripperLength / 2, 0, 0]
contact1Relative = rotation.applyRotation(contact1Relative)
contact2Relative = rotation.applyRotation(contact2Relative)
contact1 = [c + r for c, r in zip(gripperCenter, contact1Relative)]
contact2 = [c + r for c, r in zip(gripperCenter, contact2Relative)]
return contact1, contact2
def applyBodyTransformToTwist(self, vel, ror, translation, rotation):
velID = self.getColIDs(vel)
rorID = self.getColIDs(ror)
newVel = Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),rotation),
self.col(velID) \
+ np.cross(self.col(rorID),np.kron(np.ones([self.length(),1]),translation)))
newRor = Quaternion.q_rotate(
np.kron(np.ones([self.length(), 1]), rotation), self.col(rorID))
for i in np.arange(0, 3):
self.setCol(newVel[:, i], velID[i])
self.setCol(newRor[:, i], rorID[i])
def applyInertialTransform(self, pos, att, translation, rotation):
posID = self.getColIDs(pos)
attID = self.getColIDs(att)
newTranslation = np.kron(np.ones([self.length(),1]),translation) \
+ Quaternion.q_rotate(np.kron(np.ones([self.length(),1]),Quaternion.q_inverse(rotation)),
self.col(posID))
newRotation = Quaternion.q_mult(
self.col(attID), np.kron(np.ones([self.length(), 1]), rotation))
for i in np.arange(0, 3):
self.setCol(newTranslation[:, i], posID[i])
for i in np.arange(0, 4):
self.setCol(newRotation[:, i], attID[i])
def getPRY(self):
self.q = Quaternion(100, 0.002, 1 / self.frq)
self.pitch, self.yaw, self.roll = [], [], []
self.q4 = []
for index in range(self.num):
r, p, y, qur = self.q.getAngle(self.ax[index], self.ay[index],
self.az[index], self.gx[index],
self.gy[index], self.gz[index])
self.roll.append(r)
self.pitch.append(p)
self.yaw.append(y)
self.q4.append(qur)
def WtoX(W, previous_state_x):
qkminus1 = previous_state_x[:4]
# W is 6*12
qwvector = W
#omegakminus1 = previous_state_x[4:]
#omegaW = W[3:,:]
X = np.zeros((7, 4))
for i in range(6):
qw = Quaternion.vectoquat(qwvector[:, i])
qkminus1qw = Quaternion.multiply_quaternion(qkminus1, qw)
#omegakmius1plusomegaW = omegakminus1+omegaW[:,i]
#X[:,i] = np.hstack((qkminus1qw, omegakmius1plusomegaW))
X[i, :] = qkminus1qw
X[6, :] = previous_state_x
return X
def alignBodyFrame(self):
if(self.extraTransformPos != None or self.extraTransformAtt != None):
if(self.alignMode == 0 or self.alignMode == 2):
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
if(self.alignMode == 1 or self.alignMode == 3):
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
if(self.doCov):
self.td.applyBodyTransformToAttCov('attCov', self.extraTransformAtt)
if((self.extraTransformPos != np.array([0.0, 0.0, 0.0])).any()):
print('Warning: Covariance are not properly mapped if there is a translational component on the Body transform')
if(self.alignMode == 0):
B_r_BC_est, qCB_est = self.td.calibrateBodyTransform('vel', 'ror', self.tdgt, 'vel','ror')
print('Align Body Transform (estimate to GT):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.doCov):
print('Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction')
if(self.alignMode == 1):
B_r_BC_est, qCB_est = self.tdgt.calibrateBodyTransform('vel', 'ror', self.td, 'vel','ror')
print('Align Body Transform (GT to estimate):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.alignMode == 2):
B_r_BC_est = self.MrMV
qCB_est = self.qVM
print('Fixed Body Alignment (estimate to GT):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.doCov):
print('Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction')
if(self.extraTransformPos != None or self.extraTransformAtt != None):
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
if(self.doCov):
self.td.applyBodyTransformToAttCov('attCov', self.extraTransformAtt)
if(self.extraTransformPos != np.array([0.0, 0.0, 0.0])):
print('Warning: Covariance are not properly mapped if there is a translational component on the Body transform')
if(self.alignMode == 3):
B_r_BC_est = -Quaternion.q_rotate(self.qVM, self.MrMV)
qCB_est = Quaternion.q_inverse(self.qVM)
print('Fixed Body Alignment (GT to estimate):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.extraTransformPos != None or self.extraTransformAtt != None):
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
def alignBodyFrame(self):
if(self.extraTransformPos != None or self.extraTransformAtt != None):
if(self.alignMode == 0 or self.alignMode == 2):
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
if(self.alignMode == 1 or self.alignMode == 3):
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
if(self.doCov):
self.td.applyBodyTransformToAttCov('attCov', self.extraTransformAtt)
if((self.extraTransformPos != np.array([0.0, 0.0, 0.0])).any()):
print('Warning: Covariance are not properly mapped if there is a translational component on the Body transform')
if(self.alignMode == 0):
B_r_BC_est, qCB_est = self.td.calibrateBodyTransform('vel', 'ror', self.tdgt, 'vel','ror')
print('Align Body Transform (estimate to GT):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.doCov):
print('Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction')
if(self.alignMode == 1):
B_r_BC_est, qCB_est = self.tdgt.calibrateBodyTransform('vel', 'ror', self.td, 'vel','ror')
print('Align Body Transform (GT to estimate):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.alignMode == 2):
B_r_BC_est = self.MrMV
qCB_est = self.qVM
print('Fixed Body Alignment (estimate to GT):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.doCov):
print('Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction')
if(self.extraTransformPos != None or self.extraTransformAtt != None):
self.td.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
if(self.doCov):
self.td.applyBodyTransformToAttCov('attCov', self.extraTransformAtt)
if(self.extraTransformPos != np.array([0.0, 0.0, 0.0])):
print('Warning: Covariance are not properly mapped if there is a translational component on the Body transform')
if(self.alignMode == 3):
B_r_BC_est = -Quaternion.q_rotate(self.qVM, self.MrMV)
qCB_est = Quaternion.q_inverse(self.qVM)
print('Fixed Body Alignment (GT to estimate):')
print('Quaternion Rotation qCB_est:\tw:' + str(qCB_est[0]) + '\tx:' + str(qCB_est[1]) + '\ty:' + str(qCB_est[2]) + '\tz:' + str(qCB_est[3]))
print('Translation Vector B_r_BC_est:\tx:' + str(B_r_BC_est[0]) + '\ty:' + str(B_r_BC_est[1]) + '\tz:' + str(B_r_BC_est[2]))
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', B_r_BC_est, qCB_est)
if(self.extraTransformPos != None or self.extraTransformAtt != None):
self.tdgt.applyBodyTransformFull('pos', 'att','vel', 'ror', self.extraTransformPos, self.extraTransformAtt)
def insertVisual(parentNode, solid, color):
node = parentNode.createChild("node_"+solid.name)
translation=solid.position[:3]
rotation = Quaternion.to_euler(solid.position[3:]) * 180.0 / math.pi
for m in solid.mesh:
Tools.meshLoader(node, m.source, name="loader_"+m.id)
node.createObject("OglModel",src="@loader_"+m.id, translation=concat(translation),rotation=rotation.tolist(), color=color)
def computeRotationalRateFromAttitude(self, attitudeID, rotationalrateID, a=1, b=0):
dv = Quaternion.q_boxMinus(self.d[a+b:self.end(),attitudeID],self.d[0:self.end()-(a+b),attitudeID])
dt = self.getTime()[a+b:self.end()]-self.getTime()[0:self.end()-(a+b)]
for i in np.arange(0,3):
self.d[0:a,rotationalrateID[i]].fill(0)
self.d[self.end()-b:self.end(),rotationalrateID[i]].fill(0)
self.d[a:self.end()-b,rotationalrateID[i]] = np.divide(dv[:,i],dt);
def insertVisual(parentNode, solid, color):
node = parentNode.createChild("node_"+solid.name)
translation=solid.position[:3]
rotation = Quaternion.to_euler(solid.position[3:]) * 180.0 / math.pi
for m in solid.mesh:
Tools.meshLoader(node, m.source, name="loader_"+m.id)
node.createObject("OglModel",src="@loader_"+m.id, translation=concat(translation),rotation=concat(rotation), color=color)
def invertTransform(self, pos, att):
posID = self.getColIDs(pos)
attID = self.getColIDs(att)
newTranslation = -Quaternion.q_rotate(self.col(attID),self.col(posID))
for i in np.arange(0,3):
self.setCol(newTranslation[:,i],posID[i])
self.invertRotation(attID)
def invertTransform(self, pos, att):
posID = self.getColIDs(pos)
attID = self.getColIDs(att)
newTranslation = -Quaternion.q_rotate(self.col(attID), self.col(posID))
for i in np.arange(0, 3):
self.setCol(newTranslation[:, i], posID[i])
self.invertRotation(attID)
def applyBodyTransform(self, pos, att, translation, rotation):
posID = self.getColIDs(pos)
attID = self.getColIDs(att)
if translation == None:
translation = np.array([0.0, 0.0, 0.0])
if rotation == None:
rotation = np.array([1.0, 0, 0, 0])
newTranslation = self.col(posID) \
+ Quaternion.q_rotate(Quaternion.q_inverse(self.col(attID)),
np.kron(np.ones([self.length(),1]),translation))
newRotation = Quaternion.q_mult(np.kron(np.ones([self.length(),1]),rotation),
self.col(attID))
for i in np.arange(0,3):
self.setCol(newTranslation[:,i],posID[i])
for i in np.arange(0,4):
self.setCol(newRotation[:,i],attID[i])
def interpolateQuaternion(self, other, colIn, colOut=None):
# All quaternions before self start are set to the first entry
if colOut is None:
colOut = colIn
colInIDs = self.getColIDs(colIn)
colOutIDs = other.getColIDs(colOut)
counterOut = 0
counter = 0
while other.getTime()[counterOut] <= self.getTime()[counter]:
other.d[counterOut, colOutIDs] = self.d[counter, colInIDs]
counterOut += 1
# Interpolation
while (counterOut < other.length()):
while (self.getTime()[counter] < other.getTime()[counterOut]
and counter < self.last):
counter += 1
counter
if (counter != self.last):
d = (other.getTime()[counterOut] -
self.getTime()[counter - 1]) / (
self.getTime()[counter] - self.getTime()[counter - 1])
other.d[counterOut, colOutIDs] = Quaternion.q_slerp(
self.d[counter - 1, colInIDs], self.d[counter, colInIDs],
d)
else:
# Set quaternion equal to last quaternion constant extrapolation
other.d[counterOut, colOutIDs] = self.d[counter, colInIDs]
counterOut += 1
def applyBodyTransform(self, pos, att, translation, rotation):
posID = self.getColIDs(pos)
attID = self.getColIDs(att)
if translation is None:
translation = np.array([0.0, 0.0, 0.0])
if rotation is None:
rotation = np.array([1.0, 0, 0, 0])
newTranslation = self.col(posID) \
+ Quaternion.q_rotate(Quaternion.q_inverse(self.col(attID)),
np.kron(np.ones([self.length(),1]),translation))
newRotation = Quaternion.q_mult(
np.kron(np.ones([self.length(), 1]), rotation), self.col(attID))
for i in np.arange(0, 3):
self.setCol(newTranslation[:, i], posID[i])
for i in np.arange(0, 4):
self.setCol(newRotation[:, i], attID[i])
def F_DX(t, y, a):
#print("F_DX:\n\ty=%s\n\tt=%s\n\ta=%s" % (y, t, a))
dy = [0.0 for i in range(len(y))]
force, torque = a[0](y, t)
I_cm = a[1](y, t)
mass = a[2](y, t)
# The (w x I_cm w) term originates in the Newton-Euler equations, and
# should correspond to torque-free precession.
q = y[6:10]
q_w = np.asarray([0, y[10], y[11], y[12]])
q_dot = Q.mult_s_q(0.5, Q.mult_q_q(q, q_w))
pt_q_vec = np.asmatrix(Q.mult_q_q(Q.inv_q(q), q_dot)[1:4]).T
pseudo_torque = 4 * np.cross(pt_q_vec.T, (I_cm * pt_q_vec).T).T
alpha = np.linalg.solve(I_cm,np.asmatrix(torque).T - pseudo_torque)
# This assertion only holds for systems with diagonal I_cm, which is not
# the general case
#assert(np.linalg.norm(pseudo_torque) < 1e-6)
#q_len = np.sqrt(y[6]**2 + y[7]**2 + y[8]**2 + y[9]**2)
#q = [yq / q_len for yq in y[6:10]]
dy[0] = y[3]
dy[1] = y[4]
dy[2] = y[5]
dy[3] = force[0] / mass
dy[4] = force[1] / mass
dy[5] = force[2] / mass
dy[6:10] = q_dot
#dy[6] = 0.5 * (-y[7]*y[10] - y[8]*y[11] - y[9]*y[12])
#dy[7] = 0.5 * ( y[6]*y[10] - y[9]*y[11] - y[8]*y[12])
#dy[8] = 0.5 * ( y[9]*y[10] + y[6]*y[11] - y[7]*y[12])
#dy[9] = 0.5 * (-y[8]*y[10] + y[7]*y[11] + y[6]*y[12])
dy[10] = alpha[0]
dy[11] = alpha[1]
dy[12] = alpha[2]
dy[13:] = [l(y,t) for l in a[3]]
#print("dy=%s" % dy)
return np.asarray(dy)
def F_DX(t, y, a):
#print("F_DX:\n\ty=%s\n\tt=%s\n\ta=%s" % (y, t, a))
dy = [0.0 for i in range(len(y))]
force, torque = a[0](y, t)
I_cm = a[1](y, t)
mass = a[2](y, t)
# The (w x I_cm w) term originates in the Newton-Euler equations, and
# should correspond to torque-free precession.
q = y[6:10]
q_w = np.asarray([0, y[10], y[11], y[12]])
q_dot = Q.mult_s_q(0.5, Q.mult_q_q(q, q_w))
pt_q_vec = np.asmatrix(Q.mult_q_q(Q.inv_q(q), q_dot)[1:4]).T
pseudo_torque = 4 * np.cross(pt_q_vec.T, (I_cm * pt_q_vec).T).T
alpha = np.linalg.solve(I_cm, np.asmatrix(torque).T - pseudo_torque)
# This assertion only holds for systems with diagonal I_cm, which is not
# the general case
#assert(np.linalg.norm(pseudo_torque) < 1e-6)
#q_len = np.sqrt(y[6]**2 + y[7]**2 + y[8]**2 + y[9]**2)
#q = [yq / q_len for yq in y[6:10]]
dy[0] = y[3]
dy[1] = y[4]
dy[2] = y[5]
dy[3] = force[0] / mass
dy[4] = force[1] / mass
dy[5] = force[2] / mass
dy[6:10] = q_dot
#dy[6] = 0.5 * (-y[7]*y[10] - y[8]*y[11] - y[9]*y[12])
#dy[7] = 0.5 * ( y[6]*y[10] - y[9]*y[11] - y[8]*y[12])
#dy[8] = 0.5 * ( y[9]*y[10] + y[6]*y[11] - y[7]*y[12])
#dy[9] = 0.5 * (-y[8]*y[10] + y[7]*y[11] + y[6]*y[12])
dy[10] = alpha[0]
dy[11] = alpha[1]
dy[12] = alpha[2]
dy[13:] = [l(y, t) for l in a[3]]
#print("dy=%s" % dy)
return np.asarray(dy)
def applyBodyTransformToAttCov(self, attCov, rotation):
attCovIDs = self.getColIDs(attCov)
R = np.resize(Quaternion.q_toRotMat(rotation), (3, 3))
# Do the multiplication by hand, improve if possible
for i in xrange(0, self.length()):
self.d[i, attCovIDs] = np.resize(
np.dot(np.dot(R, np.resize(self.d[i, attCovIDs], (3, 3))),
R.T), (9))
def quaternionToYprCov(self, att, attCov, yprCov):
attIDs = self.getColIDs(att)
attCovIDs = self.getColIDs(attCov)
yprCovIDs = self.getColIDs(yprCov)
J = Quaternion.q_toYprJac(self.col(attIDs))
# Do the multiplication by hand, improve if possible
for i in xrange(0,self.length()):
self.d[i,yprCovIDs] = np.resize(np.dot(np.dot(np.resize(J[i,:],(3,3)),np.resize(self.col(attCovIDs)[i,:],(3,3))),np.resize(J[i,:],(3,3)).T),(9))
def getHeading(self):
""" Returns the heading angle, in radians, counterclockwise from the x-axis
Note that the sign changes at pi radians, i.e. the heading goes from 0
to pi, then from -pi back to 0 for a complete circuit
"""
pose = self.getPose()['Pose']['Orientation']
heading = Quaternion.heading(pose)
return atan2(heading["Y"], heading["X"])
def __init__(s):
s.pos = Vector3D()
s.facing = Quaternion()
s.heading = Vector3D()
s.hits = 1
s.mass = 1
s.radius = 1.0
def quaternionToYprCov(self, att, attCov, yprCov):
attIDs = self.getColIDs(att)
attCovIDs = self.getColIDs(attCov)
yprCovIDs = self.getColIDs(yprCov)
J = Quaternion.q_toYprJac(self.col(attIDs))
# Do the multiplication by hand, improve if possible
for i in xrange(0,self.length()):
self.d[i,yprCovIDs] = np.resize(np.dot(np.dot(np.resize(J[i,:],(3,3)),np.resize(self.col(attCovIDs)[i,:],(3,3))),np.resize(J[i,:],(3,3)).T),(9))
class Transform: '''This is the transform class with components position, rotation and scale''' position = Vector3(0.0, 0.0, 0.0) rotation = Quaternion(0.0, 0.0, 0.0, 1.0) scale = Vector3(0.0, 0.0, 0.0) def Rotate(self, pitch, yaw, roll): self.rotation.EulerRotation(pitch, yaw, roll)
def meanY(Y, previous_state_x):
for j in range(100):
evec = np.zeros((7, 3))
for i in range(7):
prev_inv = Quaternion.inverse_quaternion(previous_state_x)
ei = Quaternion.multiply_quaternion(Y[i, :], prev_inv)
#print(ei)
ei = Quaternion.normalize_quaternion(ei)
eve = Quaternion.quattovec(ei)
if np.linalg.norm(eve) == 0:
evec[i, :] = np.zeros((3, ))
else:
evec[i, :] = (-np.pi + np.remainder(
np.linalg.norm(eve) + np.pi,
2 * np.pi)) / np.linalg.norm(eve) * eve
#evec[i,:] = eve#/np.linalg.norm(eve)
ei_avg = np.mean(evec, axis=0)
previous_state_x = Quaternion.normalize_quaternion(
Quaternion.multiply_quaternion(Quaternion.vectoquat(ei_avg),
previous_state_x))
if np.linalg.norm(ei_avg) < 0.01:
# print(j)
break
return previous_state_x, evec
def computeVelocitiesInBodyFrameFromPostionInWorldFrame(
self, colPos, colVel, colAtt, a=1, b=0):
colPosIDs = self.getColIDs(colPos)
colVelIDs = self.getColIDs(colVel)
colAttIDs = self.getColIDs(colAtt)
self.computeVectorNDerivative(colPosIDs, colVelIDs, a, b)
self.setCol(
Quaternion.q_rotate(self.col(colAttIDs), self.col(colVelIDs)),
colVelIDs)
def computeRotationalRateFromAttitude(self, colAtt, colRor, a=1, b=0):
colAttIDs = self.getColIDs(colAtt)
colRorIDs = self.getColIDs(colRor)
dv = Quaternion.q_boxMinus(self.d[a+b:self.end(),colAttIDs],self.d[0:self.end()-(a+b),colAttIDs])
dt = self.getTime()[a+b:self.end()]-self.getTime()[0:self.end()-(a+b)]
for i in np.arange(0,3):
self.d[0:a,colRorIDs[i]].fill(0)
self.d[self.end()-b:self.end(),colRorIDs[i]].fill(0)
self.d[a:self.end()-b,colRorIDs[i]] = np.divide(dv[:,i],dt);
def computeRotationalRateFromAttitude(self, colAtt, colRor, a=1, b=0):
colAttIDs = self.getColIDs(colAtt)
colRorIDs = self.getColIDs(colRor)
dv = Quaternion.q_boxMinus(self.d[a+b:self.end(),colAttIDs],self.d[0:self.end()-(a+b),colAttIDs])
dt = self.getTime()[a+b:self.end()]-self.getTime()[0:self.end()-(a+b)]
for i in np.arange(0,3):
self.d[0:a,colRorIDs[i]].fill(0)
self.d[self.end()-b:self.end(),colRorIDs[i]].fill(0)
self.d[a:self.end()-b,colRorIDs[i]] = np.divide(dv[:,i],dt);
def applyRotationToCov(self, cov, att, doInverse=False):
covIDs = self.getColIDs(cov)
attIDs = self.getColIDs(att)
# Do the multiplication by hand, improve if possible
for i in xrange(0,self.length()):
R = np.resize(Quaternion.q_toRotMat(self.d[i,attIDs]),(3,3))
if(doInverse):
self.d[i,covIDs] = np.resize(np.dot(np.dot(R.T,np.resize(self.d[i,covIDs],(3,3))),R),(9))
else:
self.d[i,covIDs] = np.resize(np.dot(np.dot(R,np.resize(self.d[i,covIDs],(3,3))),R.T),(9))
def calibrateBodyTransform(self, vel1, ror1, other, vel2, ror2):
velID1 = self.getColIDs(vel1)
rorID1 = self.getColIDs(ror1)
velID2 = other.getColIDs(vel2)
rorID2 = other.getColIDs(ror2)
# Make timing calculation
dt1 = self.getLastTime()-self.getFirstTime()
dt2 = other.getLastTime()-other.getFirstTime()
first = max(self.getFirstTime(),other.getFirstTime())
last = min(self.getLastTime(),other.getLastTime())
timeIncrement = min(dt1/(self.length()-1), dt2/(other.length()-1))
td1 = TimedData(7);
td2 = TimedData(7);
td1.initEmptyFromTimes(np.arange(first,last,timeIncrement))
td2.initEmptyFromTimes(np.arange(first,last,timeIncrement))
self.interpolateColumns(td1, velID1, [1,2,3])
self.interpolateColumns(td1, rorID1, [4,5,6])
other.interpolateColumns(td2, velID2, [1,2,3])
other.interpolateColumns(td2, rorID2, [4,5,6])
AA = np.zeros([4,4])
# TODO: Speed up by removing for loop
for i in np.arange(0,td1.length()):
q1 = np.zeros(4)
q2 = np.zeros(4)
q1[1:4] = td1.D()[i,4:7];
q2[1:4] = td2.D()[i,4:7];
A = Quaternion.q_Rmat(q1)-Quaternion.q_Lmat(q2)
AA += A.T.dot(A)
w, v = np.linalg.eigh(AA)
rotation = v[:,0]
# Find transformation
A = np.zeros([3*td1.length(),3])
b = np.zeros([3*td1.length()])
for i in np.arange(0,td1.length()):
A[3*i:3*i+3,] = np.resize(Utils.skew(td1.D()[i,4:7]),(3,3))
b[3*i:3*i+3] = Quaternion.q_rotate(Quaternion.q_inverse(rotation), td2.D()[i,1:4])-td1.D()[i,1:4]
translation = np.linalg.lstsq(A, b)[0]
return translation, rotation
def calc_vis_values(iproc, ephem_Times, chandra_ecis, q1s, q2s, q3s, q4s):
outvals = []
for t, chandra_eci, q1, q2, q3, q4 in zip(ephem_Times, chandra_ecis, q1s, q2s, q3s, q4s):
alt = np.sqrt(np.sum(chandra_eci**2))/1e3
date = re.sub(r'\.000$', '', DateTime(t).date)
q_att = Quaternion.normalize([q1,q2,q3,q4])
vis, illum, rays = taco.calc_earth_vis(chandra_eci, q_att, ngrid=opt.ngrid)
title = '%s alt=%6.0f illum=%6.4f' % (date, alt, illum)
outvals.append((t, illum, alt, q1, q2, q3, q4))
if opt.verbose:
print title, taco.norm(chandra_eci), q1, q2, q3, q4
elif iproc == 0:
print 'Progress: %d%%\r' % int((100. * len(outvals)) / len(ephem_Times) + 0.5),
sys.stdout.flush()
def euler2q(rx0, ry0, rz0):
rx = rx0 * (np.pi / 180) / 2
ry = ry0 * (np.pi / 180) / 2
rz = rz0 * (np.pi / 180) / 2
cz = np.cos(rz)
sz = np.sin(rz)
cy = np.cos(ry)
sy = np.sin(ry)
cx = np.cos(rx)
sx = np.sin(rx)
x = sx * cy * cz - cx * sy * sz
y = cx * sy * cz + sx * cy * sz
z = cx * cy * sz - sx * sy * cz
w = cx * cy * cz + sx * sy * sz
return Q.Quat(Q.normalize([x, y, z, w])).q
def computeLeutiScore(self, posID1, attID1, velID1, other, posID2, attID2, distances, spacings, start):
output = np.empty((len(distances),6))
outputFull = []
startIndex = np.nonzero(self.getTime() >= start)[0][0]
for j in np.arange(len(distances)):
tracks = [[startIndex, 0.0]]
lastAddedStart = 0.0
posErrors = []
attErrors = []
other_interp = TimedData(8)
other_interp.initEmptyFromTimes(self.getTime())
other.interpolateColumns(other_interp, posID2, [1,2,3])
other.interpolateQuaternion(other_interp, attID2, [4,5,6,7])
it = startIndex
while it+1<self.last:
# Check for new seeds
if(lastAddedStart>spacings[j]):
tracks.append([it, 0.0])
lastAddedStart = 0.0
# Add travelled distance
d = np.asscalar(Utils.norm(self.d[it,velID1]*(self.d[it+1,0]-self.d[it,0])))
lastAddedStart += d
tracks = [[x[0], x[1]+d] for x in tracks]
# Check if travelled distance large enough
while len(tracks) > 0 and tracks[0][1] > distances[j]:
pos1 = Quaternion.q_rotate(self.d[tracks[0][0],attID1], self.d[it+1,posID1]-self.d[tracks[0][0],posID1])
pos2 = Quaternion.q_rotate(other_interp.d[tracks[0][0],[4,5,6,7]], other_interp.d[it+1,[1,2,3]]-other_interp.d[tracks[0][0],[1,2,3]])
att1 = Quaternion.q_mult(self.d[it+1,attID1], Quaternion.q_inverse(self.d[tracks[0][0],attID1]))
att2 = Quaternion.q_mult(other_interp.d[it+1,[4,5,6,7]], Quaternion.q_inverse(other_interp.d[tracks[0][0],[4,5,6,7]]))
posErrors.append(np.asscalar(np.sum((pos2-pos1)**2,axis=-1)))
attErrors.append(np.asscalar(np.sum((Quaternion.q_boxMinus(att1,att2))**2,axis=-1)))
tracks.pop(0)
it += 1
N = len(posErrors)
posErrorRMS = (np.sum(posErrors,axis=-1)/N)**(0.5)
posErrorMedian = np.median(posErrors)**(0.5)
posErrorStd = np.std(np.array(posErrors)**(0.5))
attErrorRMS = (np.sum(attErrors,axis=-1)/N)**(0.5)
attErrorMedian = np.median(attErrors)**(0.5)
attErrorStd = np.std(np.array(attErrors)**(0.5))
print('Evaluated ' + str(N) + ' segments with a travelled distance of ' + str(distances[j]) \
+ ':\n posRMS of ' + str(posErrorRMS) + ' (Median: ' + str(posErrorMedian) + ', Std: ' + str(posErrorStd) + ')' \
+ '\n attRMS of ' + str(attErrorRMS) + ' (Median: ' + str(attErrorMedian) + ', Std: ' + str(attErrorStd) + ')')
output[j,:] = [posErrorRMS, posErrorMedian, posErrorStd, attErrorRMS, attErrorMedian, attErrorStd]
outputFull.append(np.array(posErrors)**(0.5))
return outputFull
def calc_vis_values(queue, iproc, times, chandra_ecis, q1s, q2s, q3s, q4s):
outvals = []
for t, chandra_eci, q1, q2, q3, q4 in zip(times, chandra_ecis, q1s, q2s, q3s, q4s):
alt = np.sqrt(np.sum(chandra_eci**2))/1e3
date = re.sub(r'\.000$', '', DateTime(t).date)
q_att = Quaternion.normalize([q1,q2,q3,q4])
vis, illum, rays = taco.calc_earth_vis(chandra_eci, q_att)
title = '%s alt=%6.0f illum=%s' % (date, alt, illum)
outvals.append((t, illum[0], sum(illum[1:]), alt, q1, q2, q3, q4))
if opt.verbose:
print title, taco.norm(chandra_eci), q1, q2, q3, q4
elif iproc == 0:
print 'Progress: %d%%\r' % int((100. * len(outvals)) / len(times) + 0.5),
sys.stdout.flush()
if opt.movie:
plot_vis_image(title, date, rays, vis, iproc)
queue.put(outvals)
def interpolateQuaternion(self, other, colID, otherColID=None):
# All quaternions before self start are set to the first entry
if otherColID is None:
otherColID = colID
counterOut = 0;
counter = 0;
while other.getTime()[counterOut] <= self.getTime()[counter]:
other.d[counterOut,otherColID] = self.d[counter,colID]
counterOut += 1
# Interpolation
while(counterOut < other.length()):
while(self.getTime()[counter] < other.getTime()[counterOut] and counter < self.last):
counter += 1
counter
if (counter != self.last):
d = (other.getTime()[counterOut]-self.getTime()[counter-1])/(self.getTime()[counter]-self.getTime()[counter-1]);
other.d[counterOut,otherColID] = Quaternion.q_slerp(self.d[counter-1,colID], self.d[counter,colID], d)
else:
# Set quaternion equal to last quaternion constant extrapolation
other.d[counterOut,otherColID] = self.d[counter,colID];
counterOut +=1
def alignBodyFrame(self):
if self.extraTransformPos != None or self.extraTransformAtt != None:
if self.alignMode == 0 or self.alignMode == 2:
self.tdgt.applyBodyTransformFull(
"pos", "att", "vel", "ror", self.extraTransformPos, self.extraTransformAtt
)
if self.alignMode == 1 or self.alignMode == 3:
self.td.applyBodyTransformFull(
"pos", "att", "vel", "ror", self.extraTransformPos, self.extraTransformAtt
)
if self.doCov:
self.td.applyBodyTransformToAttCov("attCov", self.extraTransformAtt)
if (self.extraTransformPos != np.array([0.0, 0.0, 0.0])).any():
print(
"Warning: Covariance are not properly mapped if there is a translational component on the Body transform"
)
if self.alignMode == 0:
B_r_BC_est, qCB_est = self.td.calibrateBodyTransform("vel", "ror", self.tdgt, "vel", "ror")
print("Align Body Transform (estimate to GT):")
print(
"Quaternion Rotation qCB_est:\tw:"
+ str(qCB_est[0])
+ "\tx:"
+ str(qCB_est[1])
+ "\ty:"
+ str(qCB_est[2])
+ "\tz:"
+ str(qCB_est[3])
)
print(
"Translation Vector B_r_BC_est:\tx:"
+ str(B_r_BC_est[0])
+ "\ty:"
+ str(B_r_BC_est[1])
+ "\tz:"
+ str(B_r_BC_est[2])
)
self.td.applyBodyTransformFull("pos", "att", "vel", "ror", B_r_BC_est, qCB_est)
if self.doCov:
print(
"Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction"
)
if self.alignMode == 1:
B_r_BC_est, qCB_est = self.tdgt.calibrateBodyTransform("vel", "ror", self.td, "vel", "ror")
print("Align Body Transform (GT to estimate):")
print(
"Quaternion Rotation qCB_est:\tw:"
+ str(qCB_est[0])
+ "\tx:"
+ str(qCB_est[1])
+ "\ty:"
+ str(qCB_est[2])
+ "\tz:"
+ str(qCB_est[3])
)
print(
"Translation Vector B_r_BC_est:\tx:"
+ str(B_r_BC_est[0])
+ "\ty:"
+ str(B_r_BC_est[1])
+ "\tz:"
+ str(B_r_BC_est[2])
)
self.tdgt.applyBodyTransformFull("pos", "att", "vel", "ror", B_r_BC_est, qCB_est)
if self.alignMode == 2:
B_r_BC_est = self.MrMV
qCB_est = self.qVM
print("Fixed Body Alignment (estimate to GT):")
print(
"Quaternion Rotation qCB_est:\tw:"
+ str(qCB_est[0])
+ "\tx:"
+ str(qCB_est[1])
+ "\ty:"
+ str(qCB_est[2])
+ "\tz:"
+ str(qCB_est[3])
)
print(
"Translation Vector B_r_BC_est:\tx:"
+ str(B_r_BC_est[0])
+ "\ty:"
+ str(B_r_BC_est[1])
+ "\tz:"
+ str(B_r_BC_est[2])
)
self.td.applyBodyTransformFull("pos", "att", "vel", "ror", B_r_BC_est, qCB_est)
if self.doCov:
print(
"Warning: Covariance are not properly for Body alignment of estimate to GT, please use the other direction"
)
if self.extraTransformPos != None or self.extraTransformAtt != None:
self.td.applyBodyTransformFull(
"pos", "att", "vel", "ror", self.extraTransformPos, self.extraTransformAtt
)
if self.doCov:
self.td.applyBodyTransformToAttCov("attCov", self.extraTransformAtt)
if self.extraTransformPos != np.array([0.0, 0.0, 0.0]):
print(
"Warning: Covariance are not properly mapped if there is a translational component on the Body transform"
)
if self.alignMode == 3:
B_r_BC_est = -Quaternion.q_rotate(self.qVM, self.MrMV)
qCB_est = Quaternion.q_inverse(self.qVM)
print("Fixed Body Alignment (GT to estimate):")
print(
"Quaternion Rotation qCB_est:\tw:"
+ str(qCB_est[0])
+ "\tx:"
+ str(qCB_est[1])
+ "\ty:"
+ str(qCB_est[2])
+ "\tz:"
+ str(qCB_est[3])
)
print(
"Translation Vector B_r_BC_est:\tx:"
+ str(B_r_BC_est[0])
+ "\ty:"
+ str(B_r_BC_est[1])
+ "\tz:"
+ str(B_r_BC_est[2])
)
self.tdgt.applyBodyTransformFull("pos", "att", "vel", "ror", B_r_BC_est, qCB_est)
if self.extraTransformPos != None or self.extraTransformAtt != None:
self.tdgt.applyBodyTransformFull(
"pos", "att", "vel", "ror", self.extraTransformPos, self.extraTransformAtt
)
def transformRatesFromWorldToBody(self, colAtt, colVel, colRor):
colAttIDs = self.getColIDs(colAtt)
colVelIDs = self.getColIDs(colVel)
colRorIDs = self.getColIDs(colRor)
self.setCol(Quaternion.q_rotate(self.col(colAttIDs), self.col(colRorIDs)),colRorIDs)
self.setCol(Quaternion.q_rotate(self.col(colAttIDs), self.col(colVelIDs)),colVelIDs)
timestampTopic = "/okvis/okvis_node/okvis_odometry"
R_BS = np.array(
[
0.0148655429818,
-0.999880929698,
0.00414029679422,
0.999557249008,
0.0149672133247,
0.025715529948,
-0.0257744366974,
0.00375618835797,
0.999660727178,
]
)
bodyTranslation = np.array([-0.0216401454975, -0.064676986768, 0.00981073058949])
bodyRotation = Quaternion.q_inverse(Quaternion.q_rotMatToQuat(R_BS))
inertialTranslation = None
inertialRotation = None
writeData = False
# Load Odometry Data
td.addLabelingIncremental("pos", 3)
td.addLabelingIncremental("att", 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, "pos", "att")
# Load Timestamps into new td
td_aligned.addLabelingIncremental("pos", 3)
td_aligned.addLabelingIncremental("att", 4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic, td_aligned)
def invertRotation(self, att):
attID = self.getColIDs(att)
newRotation = Quaternion.q_inverse(self.col(attID))
for i in np.arange(0,4):
self.setCol(newRotation[:,i],attID[i])
def test_QuaternionClass(self):
v1 = np.array([0.2, 0.2, 0.4])
v2 = np.array([1, 0, 0])
q1 = Quaternion.q_exp(v1)
q2 = Quaternion.q_exp(v2)
v=np.array([1, 2, 3])
# Testing Mult and rotate
np.testing.assert_almost_equal(Quaternion.q_rotate(Quaternion.q_mult(q1,q2),v), Quaternion.q_rotate(q1,Quaternion.q_rotate(q2,v)), decimal=7)
np.testing.assert_almost_equal(Quaternion.q_rotate(q1,v2), np.resize(Quaternion.q_toRotMat(q1),(3,3)).dot(v2), decimal=7)
# Testing Boxplus, Boxminus, Log and Exp
np.testing.assert_almost_equal(Quaternion.q_boxPlus(q1,Quaternion.q_boxMinus(q2,q1)), q2, decimal=7)
np.testing.assert_almost_equal(Quaternion.q_log(q1), v1, decimal=7)
# Testing Lmat and Rmat
np.testing.assert_almost_equal(Quaternion.q_mult(q1,q2), Quaternion.q_Lmat(q1).dot(q2), decimal=7)
np.testing.assert_almost_equal(Quaternion.q_mult(q1,q2), Quaternion.q_Rmat(q2).dot(q1), decimal=7)
# Testing ypr and quat
roll = 0.2
pitch = -0.5
yaw = 2.5
q_test = Quaternion.q_mult(np.array([np.cos(0.5*pitch), 0, np.sin(0.5*pitch), 0]),np.array([np.cos(0.5*yaw), 0, 0, np.sin(0.5*yaw)]))
q_test = Quaternion.q_mult(np.array([np.cos(0.5*roll), np.sin(0.5*roll), 0, 0]),q_test)
np.testing.assert_almost_equal(Quaternion.q_toYpr(q_test), np.array([roll, pitch, yaw]), decimal=7)
# Testing Jacobian of Ypr
for i in np.arange(0,3):
dv1 = np.array([0.0, 0.0, 0.0])
dv1[i] = 1.0
epsilon = 1e-6
ypr1 = Quaternion.q_toYpr(q1)
ypr1_dist = Quaternion.q_toYpr(Quaternion.q_boxPlus(q1,dv1*epsilon))
dypr1_1 = (ypr1_dist-ypr1)/epsilon
J = np.resize(Quaternion.q_toYprJac(q1),(3,3))
dypr1_2 = J.dot(dv1)
np.testing.assert_almost_equal(dypr1_1,dypr1_2, decimal=5)
def quaternionToYpr(self, att, ypr):
attIDs = self.getColIDs(att)
yprIDs = self.getColIDs(ypr)
self.d[0:self.end(),yprIDs] = Quaternion.q_toYpr(self.col(attIDs))
def applyBodyTransformToAttCov(self, attCov, rotation):
attCovIDs = self.getColIDs(attCov)
R = np.resize(Quaternion.q_toRotMat(rotation),(3,3))
# Do the multiplication by hand, improve if possible
for i in xrange(0,self.length()):
self.d[i,attCovIDs] = np.resize(np.dot(np.dot(R,np.resize(self.d[i,attCovIDs],(3,3))),R.T),(9))
def doFeatureDepthEvaluation(
self, figureId=-1, startTime=25.0, plotFeaTimeEnd=3.0, startAverage=1.0, frequency=20.0
):
if self.doNFeatures > 0 and figureId >= -1:
plt.ion()
plt.show(block=False)
if figureId == -1:
figure()
elif figureId >= 0:
figure(figureId)
x = [[]]
y = [[]]
cov = [[]]
feaIdxID = self.td.getColIDs("feaIdx")
feaPosID = self.td.getColIDs("feaPos")
feaCovID = self.td.getColIDs("feaCov")
for j in np.arange(self.doNFeatures):
newFea = self.td.col("pos") + Quaternion.q_rotate(
Quaternion.q_inverse(self.td.col("att")), self.td.col(feaPosID[j])
)
self.td.applyRotationToCov(feaCovID[j], "att", True)
for i in np.arange(0, 3):
self.td.setCol(newFea[:, i], feaPosID[j][i])
lastStart = 0.0
lastID = -1
startID = 0
for i in np.arange(self.td.length()):
if self.td.d[i, self.td.timeID] > startTime:
if self.td.d[i, feaIdxID[j]] < 0.0 or self.td.d[i, feaIdxID[j]] != lastID:
if len(x[-1]) > 0:
x.append([])
y.append([])
cov.append([])
if self.td.d[i, feaIdxID[j]] >= 0.0:
if len(x[-1]) == 0:
lastStart = self.td.d[i, self.td.timeID]
lastID = self.td.d[i, feaIdxID[j]]
startID = i
x[-1].append(self.td.d[i, self.td.timeID] - lastStart)
y[-1].append(self.td.d[i, feaPosID[j][2]])
cov[-1].append(sqrt(self.td.d[i, feaCovID[j][8]]))
axis = subplot(2, 1, 1)
for i in np.arange(len(x)):
plot(x[i], y[i], color=Utils.colors["gray"])
average = 0
averageCount = 0
for i in np.arange(len(y)):
for j in np.arange(int(startAverage * frequency), len(y[i])):
average += y[i][j]
averageCount += 1
average = average / averageCount
means = []
meanCovs = []
stds = []
for j in np.arange(int(plotFeaTimeEnd * frequency)):
values = []
covValues = []
for i in np.arange(len(y)):
if len(y[i]) > j:
values.append(y[i][j] - average)
covValues.append(cov[i][j])
means.append(mean(values))
meanCovs.append(mean(covValues))
stds.append(std(values))
print("Final standard deviation: " + str(stds[-1]))
plot(
np.arange(plotFeaTimeEnd * frequency) / frequency,
np.ones(plotFeaTimeEnd * frequency) * average,
color=Utils.colors["blue"],
lw=3,
label="Estimated groundtruth",
)
plot(
np.arange(plotFeaTimeEnd * frequency) / frequency,
average + np.array(means),
color=Utils.colors["red"],
lw=3,
label="Average",
)
plot(
np.arange(plotFeaTimeEnd * frequency) / frequency,
average + np.array(means) + np.array(stds),
"--",
color=Utils.colors["red"],
lw=3,
label="Measured standard deviation",
)
plot(
np.arange(plotFeaTimeEnd * frequency) / frequency,
average + np.array(means) - np.array(stds),
"--",
color=Utils.colors["red"],
lw=3,
)
plot(
np.arange(plotFeaTimeEnd * frequency) / frequency,
average + np.array(means) + np.array(meanCovs),
"--",
color=Utils.colors["green"],
lw=3,
label="Estimated standard deviation",
)
plot(
np.arange(plotFeaTimeEnd * frequency) / frequency,
average + np.array(means) - np.array(meanCovs),
"--",
color=Utils.colors["green"],
lw=3,
)
plt.axis([0, plotFeaTimeEnd, -4.0, -1.0])
plt.legend(loc=4)
axis.set_ylabel("Landmark height [m]")
axis = subplot(2, 1, 2)
line_th = plot(
[0, plotFeaTimeEnd],
[sqrt(6.64), sqrt(6.64)],
"--",
color=Utils.colors["green"],
lw=3,
label="1% threshold",
)
for i in np.arange(len(x)):
plot(x[i], np.abs(np.array(y[i]) - average) / np.array(cov[i]), color=Utils.colors["gray"])
plt.axis([0, plotFeaTimeEnd, 0, 5])
plt.legend()
plt.suptitle("Convergence of Landmark Distance")
axis.set_ylabel("Normalized error [1]")
axis.set_xlabel("Time [s]")
def computeVelocitiesInBodyFrameFromPostionInWorldFrame(self, colPos, colVel, colAtt, a=1, b=0):
colPosIDs = self.getColIDs(colPos)
colVelIDs = self.getColIDs(colVel)
colAttIDs = self.getColIDs(colAtt)
self.computeVectorNDerivative(colPosIDs, colVelIDs, a, b)
self.setCol(Quaternion.q_rotate(self.col(colAttIDs), self.col(colVelIDs)),colVelIDs);
"""
RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td1,posIDs1,attIDs1)
RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td2,posIDs2,attIDs2)
# Add initial x to plot
plotter1.addDataToSubplot(td1, posIDs1[0], 1, 'r', 'td1In x');
plotter1.addDataToSubplot(td2, posIDs2[0], 1, 'b', 'td2In x');
"""
Apply body transform to the second data set. The column start IDs of position(9) and attitutde(1) have to be provided.
We define the rotation through a rotation vector vCB, the exponential represents the corresponding rotation quaternion qCB.
The translation is defined by the translation vector B_r_BC
"""
vCB = np.array([0.1,0.2,0.32])
qCB = Quaternion.q_exp(vCB)
B_r_BC = np.array([1.1,-0.2,0.4])
td2.applyBodyTransform(posIDs2, attIDs2, B_r_BC, qCB)
print('Applying Body Transform:')
print('Rotation Vector ln(qCB):\tvx:' + str(vCB[0]) + '\tvy:' + str(vCB[1]) + '\tvz:' + str(vCB[2]))
print('Translation Vector B_r_BC:\trx:' + str(B_r_BC[0]) + '\try:' + str(B_r_BC[1]) + '\trz:' + str(B_r_BC[2]))
"""
Apply inertial transform to the second data set. Same procedure as with the body transform.
"""
vIJ = np.array([0.2,-0.2,-0.4])
qIJ = Quaternion.q_exp(vIJ)
J_r_JI = np.array([-0.1,0.5,0.1])
td2.applyInertialTransform(posIDs2, attIDs2,J_r_JI,qIJ)
print('Applying Inertial Transform:')
print('Rotation Vector ln(qIJ):\tvx:' + str(vIJ[0]) + '\tvy:' + str(vIJ[1]) + '\tvz:' + str(vIJ[2]))
