def rotate3DQuaternion(cartesianPt, rotType, angle):
axis = (1, 0, 0)
angle = -(3.14159265359 * angle) / 180
q = Quaternion(math.cos(angle / 2.0),
math.sin(angle / 2.0) * axis[0],
math.sin(angle / 2.0) * axis[1],
math.sin(angle / 2.0) * axis[2])
quaternionPt = Quaternion(0, cartesianPt.x, cartesianPt.y, cartesianPt.z)
qstar = quaternionConjugate(q)
quaternionPt = quaternionMult(q, quaternionPt)
quaternionPt = quaternionMult(quaternionPt, qstar)
t = (quaternionPt.x, quaternionPt.y, quaternionPt.z)
return t
def transform_ra_dec_list(self, l): t = Quaternion.from_ra_dec_pair([self.prec_ra, self.prec_dec], [self.ra, self.dec]) #print "transform_ra_dec_list", t.transform_ra_dec([self.ra, self.dec]) res = [] for rd in l: res.append(t.transform_ra_dec(rd)) return res
def quaternionMult(quat1, quat2):
u = Quaternion(
quat1.s*quat2.s - quat1.x*quat2.x - quat1.y*quat2.y - quat1.z*quat2.z, \
quat2.s*quat1.x + quat1.s*quat2.x + quat1.y*quat2.z - quat1.z*quat2.y, \
quat2.s*quat1.y + quat1.s*quat2.y + quat1.z*quat2.x - quat1.x*quat2.z, \
quat2.s*quat1.z + quat1.s*quat2.z + quat1.x*quat2.y - quat1.y*quat2.x)
return u
def camera_position(self, ci, noise = 2): if len(self.campos[ci]) < 2: return None avg = Quaternion.average(self.campos[ci]) d = np.array([avg.distance_metric(q) for q in self.campos[ci]]) d2 = d**2 var = np.mean(d2) #print var * noise**2 #print d2 #print np.where(d2 < var * noise**2)[0] self.campos[ci] = [self.campos[ci][i] for i in np.where(d2 < var * noise**2)[0]] avg = Quaternion.average(self.campos[ci]) self.campos_avg[ci] = avg return avg
def rotate3DQuaternionApprox(cartesianPt, rotType, angle, normalize=False):
# TODO : rotType ka use karke quaternion define karna h
# print(normalize, threshold)
axis = (1, 0, 0)
d = -(3.14159265359 * angle) / 180
q = Quaternion(1, (d / 2.0) * axis[0], (d / 2.0) * axis[1],
(d / 2.0) * axis[2])
if threshold and normalize:
# print("here")
# print(q.s, q.x, q.y, q.z)
q = mod_q(q)
# print(q.s, q.x, q.y, q.z)
quaternionPt = Quaternion(0, cartesianPt.x, cartesianPt.y, cartesianPt.z)
qstar = quaternionConjugate(q)
quaternionPt = quaternionMult(q, quaternionPt)
quaternionPt = quaternionMult(quaternionPt, qstar)
t = (quaternionPt.x, quaternionPt.y, quaternionPt.z)
return t
def mode_solve_set_pos(self, ra, dec, roll, t, camera): ci = self.cameras[camera] pos_orig = Quaternion([ra, dec, roll]) self.campos_adjust[ci] = (pos_orig, t) if self.t0 is None: self.t0 = t ha = (t - self.t0) / 240.0 qha = self.prec_q * Quaternion([-ha, 0, 0]) / self.prec_q #print "qha", quat_axis_to_ra_dec(qha), "prec", self.prec_ra, self.prec_dec #print Quaternion([ra, dec, roll]).to_euler(), (qha * Quaternion([ra, dec, roll])).to_euler() pos = qha * pos_orig self.pos[ci].append((pos, t)) if ci > 0 and len(self.pos[0]) > 0: prev_pos, prev_t = self.campos_adjust[0] if abs(t - prev_t) < 10: self.campos[ci].append(pos_orig.inv() * prev_pos) elif ci == 0: for i in range(1, len(self.pos)): if len(self.pos[i]) > 0: prev_pos, prev_t = self.campos_adjust[i] if abs(t - prev_t) < 10: self.campos[i].append(prev_pos.inv() * pos_orig)
def plot2(self, size = 960, area = 0.1): ha = celestial_rot() + self.status['gps'][1] qha = Quaternion([90-ha, 0, 0]) img = np.zeros((size, size, 3), dtype=np.uint8) c = size / 2 scale = size / area if self.ra is not None and self.dec is not None: t = Quaternion.from_ra_dec_pair([self.ra, self.dec], [self.prec_ra, self.prec_dec]) else: t = Quaternion.from_ra_dec_pair([0.0, 90.0], [self.prec_ra, self.prec_dec]) polaris = [37.9529, 89.2642] polaris_target = self.prec_q.transform_ra_dec(polaris) prec = t.transform_ra_dec([0, 90]) polaris_real = t.transform_ra_dec(polaris_target) polaris_target = qha.transform_ra_dec(polaris_target) prec = qha.transform_ra_dec(prec) polaris_real = qha.transform_ra_dec(polaris_real) polaris_target_xyz = ra_dec_to_xyz(polaris_target) polaris_r = (polaris_target_xyz[0] ** 2 + polaris_target_xyz[1] ** 2)**0.5 prec_xyz = ra_dec_to_xyz(prec) polaris_real_xyz = ra_dec_to_xyz(polaris_real) cv2.circle(img, (c,c), int(polaris_r * scale), (0, 255, 0), 1) for i in range (0, 24): a = np.deg2rad([i * 360.0 / 24.0]) sa = math.sin(a) ca = math.cos(a) cv2.line(img, (int(c + sa * polaris_r * scale), int(c + ca * polaris_r * scale)), (int(c + sa * (polaris_r * scale + 8)), int(c + ca * (polaris_r * scale + 8))), (0, 255, 0), 1) cv2.circle(img, (int(c + polaris_target_xyz[0] * scale), int(c + polaris_target_xyz[1] * scale)), 4, (0, 255, 0), 2) cv2.line(img, (0, c), (size, c), (0, 255, 0), 1) cv2.line(img, (c, 0), (c, size), (0, 255, 0), 1) if self.ra is not None and self.dec is not None: cv2.circle(img, (int(c + prec_xyz[0] * scale), int(c + prec_xyz[1] * scale)), 4, (255, 255, 255), 2) cv2.circle(img, (int(c + polaris_real_xyz[0] * scale), int(c + polaris_real_xyz[1] * scale)), 4, (255, 255, 255), 2) pole_dist = (prec_xyz[0] ** 2 + prec_xyz[1] ** 2) ** 0.5 if pole_dist >= area / 2: cv2.putText(img, "%0.1fdeg" % (90 - prec[1]), (int(c + prec_xyz[0] / pole_dist * area / 5 * scale - 50), int(c + prec_xyz[1] / pole_dist * area / 5 * scale)), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 255), 2) cv2.arrowedLine(img, (int(c + prec_xyz[0] / pole_dist * area / 3 * scale), int(c + prec_xyz[1] / pole_dist * area / 3 * scale)), (int(c + prec_xyz[0] / pole_dist * area / 2 * scale), int(c + prec_xyz[1] / pole_dist * area / 2 * scale)), (255, 255, 255), 2) return img
def quaternionConjugate(q):
return Quaternion(q.s, -1 * q.x, -1 * q.y, -1 * q.z)
def mod_q(q):
r = math.sqrt(q.s * q.s + q.x * q.x + q.y * q.y + q.z * q.z)
return Quaternion(q.s / r, q.x / r, q.y / r, q.z / r)
def parseDotScene(self):
# TODO: check DTD to make sure you get all nodes/attributes
# TODO: Use the userData for sound/physics
for node in self.root:
if node.nodeType == Node.ELEMENT_NODE and node.nodeName == 'node':
attachMe = None
realName = node.attributes['name'].nodeValue
# create new scene node
newNode = self.rootNode.createChildSceneNode(
) # self.prefix + realName)
#position it
pos = self.findNodes(node, 'position')[0].attributes
newNode.position = (float(pos['x'].nodeValue),
float(pos['y'].nodeValue),
float(pos['z'].nodeValue))
# rotate it
try:
rot = self.findNodes(node, 'rotation')[0].attributes
newNode.orientation = Quaternion(
float(rot['qw'].nodeValue), float(rot['qx'].nodeValue),
float(rot['qy'].nodeValue), float(rot['qz'].nodeValue))
# print float(rot['qw'].nodeValue), float(rot['qx'].nodeValue), float(rot['qy'].nodeValue),float(rot['qz'].nodeValue)
except IndexError: # probably doesn't have rotation attribute
rot = self.findNodes(node, 'quaternion')[0].attributes
newNode.orientation = Quaternion(float(rot['w'].nodeValue),
float(rot['x'].nodeValue),
float(rot['y'].nodeValue),
float(rot['z'].nodeValue))
# print float(rot['w'].nodeValue), float(rot['x'].nodeValue), float(rot['y'].nodeValue), float(rot['z'].nodeValue)
# scale it
scale = self.findNodes(node, 'scale')[0].attributes
newNode.scale = (float(scale['x'].nodeValue),
float(scale['y'].nodeValue),
float(scale['z'].nodeValue))
# is it a light?
#try:
# thingy = self.findNodes(node, 'light')[0].attributes
# name = str(thingy['name'].nodeValue)
#attachMe = self.sceneManager.createLight(name)
#ltypes={'point':ogre.Light.LT_POINT,'directional':ogre.Light.LT_DIRECTIONAL,'spot':ogre.Light.LT_SPOTLIGHT,'radPoint':ogre.Light.LT_POINT}
#try:
# attachMe.type = ltypes[thingy['type'].nodeValue]
#except IndexError:
# pass
# lightNode = self.findNodes(node, 'light')[0]
# try:
# tempnode = self.findNodes(lightNode, 'colourSpecular')[0]
# attachMe.specularColour = (float(tempnode.attributes['r'].nodeValue), float(tempnode.attributes['g'].nodeValue), float(tempnode.attributes['b'].nodeValue), 1.0)
# except IndexError:
# pass
# try:
# tempnode = self.findNodes(lightNode, 'colourDiffuse')[0]
# attachMe.diffuseColour = (float(tempnode.attributes['r'].nodeValue), float(tempnode.attributes['g'].nodeValue), float(tempnode.attributes['b'].nodeValue), 1.0)
# except IndexError:
# pass
# try:
# tempnode = self.findNodes(lightNode, 'colourDiffuse')[0]
# attachMe.diffuseColour = (float(tempnode.attributes['r'].nodeValue), float(tempnode.attributes['g'].nodeValue), float(tempnode.attributes['b'].nodeValue), 1.0)
# except IndexError:
# pass
# self.lights.append( attachMe )
# print 'added light: "%s"' % name
#except IndexError:
# pass
# is it an entity?
try:
thingy = self.findNodes(node, 'entity')[0].attributes
name = str(thingy['name'].nodeValue)
mesh = str(thingy['meshFile'].nodeValue)
attachMe = self.sceneManager.createEntity(name, mesh)
print 'added entity: "%s" %s' % (name, mesh)
except IndexError:
pass
#except: #ogre.OgreFileNotFoundException: # mesh is missing
# print "Missing Mesh:",mesh
# pass
# is it a camera?
# TODO: there are other attributes I need in here
try:
thingy = self.findNodes(node, 'camera')[0].attributes
name = str(thingy['name'].nodeValue)
fov = float(thingy['fov'].nodeValue)
projectionType = str(thingy['projectionType'].nodeValue)
attachMe = self.sceneManager.createCamera(name)
try:
tempnode = self.findNodes(node, 'clipping')[0]
attachMe.nearClipDistance = float(
tempnode.attributes['near'].nodeValue)
attachMe.farClipDistance = float(
tempnode.attributes['far'].nodeValue)
except IndexError:
pass
##attachMe.setFOVy ( ogre.Radian( fov ) ) #fOVy = fov
self.cameras.append(attachMe)
print 'added camera: "%s" fov: %f type: %s clipping: %f,%f' % (
name, fov, projectionType, attachMe.nearClipDistance,
attachMe.farClipDistance)
except IndexError:
pass
# attach it to the scene
#try:
if attachMe is not None:
newNode.attachObject(attachMe)
imu_val[0, :] = Ax_raw * Acc_scale_factor[0] + Acc_bias[0]
imu_val[1, :] = Ay_raw * Acc_scale_factor[1] + Acc_bias[1]
imu_val[2, :] = Az_raw * Acc_scale_factor[2] + Acc_bias[2]
imu_val[3, :] = (Wx_raw - Wx_bias) * Gyro_scale_factor
imu_val[4, :] = (Wy_raw - Wy_bias) * Gyro_scale_factor
imu_val[5, :] = (Wz_raw - Wz_bias) * Gyro_scale_factor
#initialization
xt = np.matrix([1, 0, 0, 0, 0, 0, 0]).T
Pt = np.diag([0.01, 0.01, 0.01, 0.01, 0.01, 0.01])
Q = np.diag([78.97, 78.97, 78.97, 6.5, 6.5, 6.5])
R = np.diag([0.2, 0.2, 0.2, 3, 3, 3])
#gravity
g = Quaternion(0.0, 0.0, 0.0, 1.0)
# Sigma Point 2n
n = 6
#store Rot output
rot_UKF = np.zeros((3, 3, ts.shape[1]))
#rot_UKF[:,:,0] = np.eye(3)
#iteration
for i in range(ts.shape[1]):
if i == 0:
dt = 0.01
else:
dt = ts[0, i] - ts[0, i - 1]
#Cholesky decomposition
#find orientation based on acc
Acc_rpy = np.zeros([3, imu_val.shape[1]])
for i in range(imu_val.shape[1]):
v = imu_val[0:3, i]
Acc_rpy[0, i] = -np.arctan2(v[1], (v[0]**2 + v[2]**2)**0.5)
Acc_rpy[1, i] = np.arctan2(v[0], (v[1] ** 2 + v[2] ** 2) ** 0.5)
Acc_rpy[2, i] = 0
#find orientation based on gyro
Gyro_rot = np.zeros([3, 3, imu_val.shape[1]])
Gyro_rpy = np.zeros([3, imu_val.shape[1]])
q = Quaternion(1,0,0,0)
for i in range(imu_val.shape[1]):
# Time increment
if i == 0:
dt = 0.01
else:
dt = ts[0, i] - ts[0, i - 1]
#find angles and axis
v = imu_val[3:6, i]
v_norm = (v[0]**2 + v[1]**2 + v[2]**2)**0.5
angle = v_norm*dt
axis = v / v_norm
qt = qu.angle_axis2quat(angle,axis)
q *= qt
R = q.q_rot()
def solve(self, noise=2): if self.mode == 'adjust': return self.ra, self.dec if len(self.pos[0]) < 3: return None, None weights = np.matrix(np.zeros((2, 2))) wsum = np.zeros((2,1)) for ci in range(0, len(self.pos)): if len(self.pos[ci]) < 3: continue try: qlist = [ p for (p, t) in self.pos[ci]] avg = Quaternion.average(qlist).inv() alist = [] for q in qlist: q0 = q * avg if abs(q0.a[0]) < 0.7: ax, roll = q0.to_axis_roll() q0 = Quaternion.from_axis_roll(ax, roll - 180) a = q0.a[1:4] / q0.a[0] alist.append(a) aa = np.array(alist) d2 = np.sum(aa[:, 0:2] ** 2, axis = 1) var = np.mean(d2) aa = aa[np.where(d2 < var * 4)] line = cv2.fitLine(aa / aa[0,2] * 100, cv2.DIST_L2, 0.001, 0.000001, 0.000001)[0:3].reshape(3) line2d = line[0:2] / line[2] aa2d = aa[:, 0:2] - np.outer(aa[:, 2], line2d) cov =np.cov(aa2d.T) #print cov w = np.matrix(cov).I #print ci, "w", w weights += w wsum += w * line2d.reshape((2,1)) #print ci, "weights", weights #print ci, "wsum", wsum except: continue try: line2d = weights.I * wsum #print "res", line2d line = np.array([line2d[0,0], line2d[1,0], 1]) ra, dec = xyz_to_ra_dec(line) if dec < 0: dec = -dec ra -= 180 if ra < 0.0 : ra += 360 self.ra = ra self.dec = dec self.solved = True except: return None, None #print "rotation center", ra, dec #print "prec", self.prec_ra, self.prec_dec return ra, dec
poslist = [] for i in range(0, len(self.pos)): if self.campos_adjust[i] is None: continue prev_pos, prev_t = self.campos_adjust[i] if abs(t - prev_t) > 10: continue if i > 0 and self.campos_avg[i] is None: self.camera_position(i) if i > 0 and self.campos_avg[i] is not None: prev_pos = prev_pos * self.campos_avg[i] poslist.append(prev_pos) print [p.to_euler() for p in poslist] pos = Quaternion.average(poslist) if self.p2_from is None: self.mode_adjust_set_ref_pos() self.p2_from = pos self.save() t = pos / self.p2_from print t.to_euler() self.ra, self.dec = t.transform_ra_dec([self.ref_ra, self.ref_dec]) def plot2(self, size = 960, area = 0.1): ha = celestial_rot() + self.status['gps'][1] qha = Quaternion([90-ha, 0, 0])
