def quatAngleDiff(rvec1, rvec2):
rot1, jac1 = cv2.Rodrigues(rvec1)
rot2, jac2 = cv2.Rodrigues(rvec2)
quat1 = tf.quaternion_from_matrix(rot1)
quat2 = tf.quaternion_from_matrix(rot2)
dtheta = math.acos(2 * (np.dot(quat1, quat2)**2) - 1)
return math.degrees(dtheta)
def quatAngleDiff(rot1, rot2):
quat1 = tf.quaternion_from_matrix(rot1)
quat2 = tf.quaternion_from_matrix(rot2)
dtheta = math.acos(2 * (np.dot(quat1, quat2)**2) - 1)
return math.degrees(dtheta)
print('file:' + fullpath)
model = mujoco_py.MjModel(fullpath)
viewer = mujoco_py.MjViewer(visible=True)
viewer.start()
viewer.set_model(model)
cam_pos = np.array([1.0, 0.0, 0.7, 0.5, -45, 180])
# cam_pos = np.array([0.1, 0.0, 0.7, 0.01, -45., 0.])
set_cam_position(viewer, cam_pos)
ctrl = model.data.ctrl.copy()
# ctrl[-1] = 10.0
model.data.mocap_pos = model.data.site_xpos[3] #np.array([[0.6, 0., 0.]])
model.data.mocap_quat = quaternion_from_matrix(
model.data.site_xmat[3].reshape(3, 3))
# set_trace()
jt1 = np.array([
-0.65597575, -0.77090292, -0.85008688, 1.69193828, 0.70414396,
0.98353719, -0.98110516
]).reshape(1, 7)
jt2 = np.array([
-0.57907814, -0.55066804, -0.67528671, 1.76927182, 0.94890188,
0.71474327, -1.00205739
]).reshape(1, 7)
jt3 = np.array([
-0.84808738, -0.3352765, -0.84940643, 1.42527668, 1.04024461,
0.96507808, -1.19109601
]).reshape(1, 7)
xEnd += 8
pixelR = r[0, 0]
pixelG = g[0, 0]
pixelB = b[0, 0]
quat = quaternion.quaternion(0, pixelR, pixelG, pixelB)
quat2 = np.array([0, pixelR, pixelG, pixelB])
test = scipy.fftpack.dct(quat2,
type=2,
n=None,
axis=-1,
norm=None,
overwrite_x=False)
bMatrix = TF.rotation_matrix(0.0, [r[0][0], g[0][0], b[0][0]])
quat = TF.quaternion_from_matrix([r[0][0], g[0][0], b[0][0]])
quaternionOfR = quaternion.as_quat_array(r)
quaternionOfG = quaternion.as_quat_array(g)
quaternionOfB = quaternion.as_quat_array(b)
arrayOfColors = np.concatenate(
(quaternionOfR, quaternionOfG, quaternionOfB), axis=0)
print quaternionOfB
qdct_transformR = scipy.fftpack.dct(arrayOfColors,
type=2,
n=None,
axis=-1,
norm=None,
overwrite_x=False)
# quaternionOfColors = quaternion.as_quat_array(arrayOfColors)
#qdct_transform = scipy.fftpack.dct(quaternionOfColors, type=2, n=None, axis=-1, norm=None, overwrite_x=False)
def fuse_transform(marker,
rgb_image,
depth_image,
camera_intrinsics,
frame_id,
ax=None):
I = np.array(camera_intrinsics).reshape(3, 3) # camera intrinsics
# fx = I.item(0,0)
# fy = I.item(1,1)
# px = I.item(0,2)
# py = I.item(1,2)
detection_id = marker.id
tag_corners = marker.corners2d
tag_size = marker.tag_size
tag_radius = tag_size / 2
corners0_r = tag_corners[0].x
corners0_c = tag_corners[0].y
corners1_r = tag_corners[1].x
corners1_c = tag_corners[1].y
corners2_r = tag_corners[2].x
corners2_c = tag_corners[2].y
corners3_r = tag_corners[3].x
corners3_c = tag_corners[3].y
position_x = marker.pose.position.x
position_y = marker.pose.position.y
position_z = marker.pose.position.z
rotation_x = marker.pose.orientation.x
rotation_y = marker.pose.orientation.y
rotation_z = marker.pose.orientation.z
rotation_w = marker.pose.orientation.w
# x_array = [corners0_x, corners1_x, corners2_x, corners3_x]
# y_array = [corners0_y, corners1_y, corners2_y, corners3_y]
# x_array = sorted(x_array)
# y_array = sorted(y_array)
# x_min = int(x_array[1]) + 0
# x_max = int(x_array[2]) - 0
# y_min = int(y_array[1]) + 0
# y_max = int(y_array[2]) - 0
im_pt1 = [corners0_r, corners0_c]
im_pt2 = [corners1_r, corners1_c]
im_pt3 = [corners2_r, corners2_c]
im_pt4 = [corners3_r, corners3_c]
im_pts = im_pt1 + im_pt2 + im_pt3 + im_pt4
image_pts = np.array(im_pts).reshape(4, 2)
ob_pt1 = [-tag_radius, -tag_radius, 0.0]
ob_pt2 = [tag_radius, -tag_radius, 0.0]
ob_pt3 = [tag_radius, tag_radius, 0.0]
ob_pt4 = [-tag_radius, tag_radius, 0.0]
ob_pts = ob_pt1 + ob_pt2 + ob_pt3 + ob_pt4
object_pts = np.array(ob_pts).reshape(4, 3)
D = np.zeros((5, 1))
rvec, tvec = solvePnP_RGBD(rgb_image, depth_image, object_pts, image_pts,
I, D)
rotation, jacob = cv2.Rodrigues(rvec)
M_r = np.zeros((3, 4))
M_r[0:3, 0:3] = rotation
M_r[0:3, 3] = tvec.reshape(3)
print M_r
R_fused = M_r[0:3, 0:3]
T_fused = M_r[0:3, 3]
quart_fused = tf.quaternion_from_matrix(R_fused)
fused_marker = copy_module.deepcopy(marker)
fused_marker.pose.position.x = T_fused[0]
fused_marker.pose.position.y = T_fused[1]
fused_marker.pose.position.z = T_fused[2]
fused_marker.pose.orientation.x = quart_fused[1]
fused_marker.pose.orientation.y = quart_fused[2]
fused_marker.pose.orientation.z = quart_fused[3]
fused_marker.pose.orientation.w = quart_fused[0]
transformed_marker = generate_marker(detection_id, R_fused, T_fused,
tag_size, fused_marker, frame_id)
return fused_marker, transformed_marker