def update_PROJ(self, ned, yaw_rad, pitch_rad, roll_rad):
cam_yaw, cam_pitch, cam_roll = self.get_ypr()
body2cam = transformations.quaternion_from_euler(
cam_yaw * d2r, cam_pitch * d2r, cam_roll * d2r, 'rzyx')
# this function modifies the parameters you pass in so, avoid
# getting our data changed out from under us, by forcing copies
# (a = b, wasn't sufficient, but a = float(b) forced a copy.
tmp_yaw = float(yaw_rad)
tmp_pitch = float(pitch_rad)
tmp_roll = float(roll_rad)
ned2body = transformations.quaternion_from_euler(
tmp_yaw, tmp_pitch, tmp_roll, 'rzyx')
#body2ned = transformations.quaternion_inverse(ned2body)
#print 'ned2body(q):', ned2body
ned2cam_q = transformations.quaternion_multiply(ned2body, body2cam)
ned2cam = np.matrix(
transformations.quaternion_matrix(np.array(ned2cam_q))[:3, :3]).T
#print 'ned2cam:', ned2cam
R = ned2proj.dot(ned2cam)
rvec, jac = cv2.Rodrigues(R)
tvec = -np.matrix(R) * np.matrix(ned).T
R, jac = cv2.Rodrigues(rvec)
# is this R the same as the earlier R?
self.PROJ = np.concatenate((R, tvec), axis=1)
#print 'PROJ:', PROJ
#print lat_deg, lon_deg, altitude, ref[0], ref[1], ref[2]
#print ned
return self.PROJ
def ladder_helper(self, q0, a0, a1):
q1 = transformations.quaternion_from_euler(-a1 * d2r, -a0 * d2r, 0.0,
'rzyx')
q = transformations.quaternion_multiply(q1, q0)
v = transformations.quaternion_transform(q, [1.0, 0.0, 0.0])
uv = self.project_point(
[self.ned[0] + v[0], self.ned[1] + v[1], self.ned[2] + v[2]])
return uv
def draw_nose(self):
ned2body = transformations.quaternion_from_euler(
self.psi_rad, self.the_rad, self.phi_rad, 'rzyx')
body2ned = transformations.quaternion_inverse(ned2body)
vec = transformations.quaternion_transform(body2ned, [1.0, 0.0, 0.0])
uv = self.project_point(
[self.ned[0] + vec[0], self.ned[1] + vec[1], self.ned[2] + vec[2]])
r1 = int(round(self.render_h / 80))
r2 = int(round(self.render_h / 40))
if uv != None:
cv2.circle(self.frame, uv, r1, self.color, self.line_width,
cv2.LINE_AA)
cv2.circle(self.frame, uv, r2, self.color, self.line_width,
cv2.LINE_AA)
q = averageQuaternions(np.array(quats))
q /= transformations.vector_norm(q)
print('average transform:', q)
(yaw_rad, pitch_rad,
roll_rad) = transformations.euler_from_quaternion(q, 'rzyx')
print('euler transform (ypr):', yaw_rad * r2d, pitch_rad * r2d, roll_rad * r2d)
mount_node = getNode('/config/camera/mount', True)
camera_yaw = mount_node.getFloat('yaw_deg')
camera_pitch = mount_node.getFloat('pitch_deg')
camera_roll = mount_node.getFloat('roll_deg')
print('camera:', camera_yaw, camera_pitch, camera_roll)
body2cam = transformations.quaternion_from_euler(camera_yaw * d2r,
camera_pitch * d2r,
camera_roll * d2r, 'rzyx')
print('body2cam:', body2cam)
(yaw_rad, pitch_rad,
roll_rad) = transformations.euler_from_quaternion(body2cam, 'rzyx')
print('euler body2cam (ypr):', yaw_rad * r2d, pitch_rad * r2d, roll_rad * r2d)
cam2body = transformations.quaternion_inverse(body2cam)
print('cam2body:', cam2body)
(yaw_rad, pitch_rad,
roll_rad) = transformations.euler_from_quaternion(cam2body, 'rzyx')
print('euler cam2body (ypr):', yaw_rad * r2d, pitch_rad * r2d, roll_rad * r2d)
tot = transformations.quaternion_multiply(q, body2cam)
(yaw_rad, pitch_rad,
roll_rad) = transformations.euler_from_quaternion(tot, 'rzyx')
flight_mode = 'auto'
else:
flight_mode = 'auto'
if 'mission' in data:
excite_mode = mission['excite']
test_index = mission['test_index']
record = iter.next()
hud1.update_task(record)
while 'imu' in record and record['imu']['time'] < time:
record = iter.next()
hud1.update_task(record)
body2cam = transformations.quaternion_from_euler( cam_yaw * d2r,
cam_pitch * d2r,
cam_roll * d2r,
'rzyx')
# this function modifies the parameters you pass in so, avoid
# getting our data changed out from under us, by forcing copies (a
# = b, wasn't sufficient, but a = float(b) forced a copy.
tmp_yaw = float(yaw_rad)
tmp_pitch = float(pitch_rad)
tmp_roll = float(roll_rad)
ned2body = transformations.quaternion_from_euler(tmp_yaw,
tmp_pitch,
tmp_roll,
'rzyx')
body2ned = transformations.quaternion_inverse(ned2body)
#print 'ned2body(q):', ned2body
