def Interpolate(a, b, time):
ratio = float(time - a.id) / (b.id - a.id)
M0, M1 = a.M, b.M
dm = SE3.algebra_from_group(M1.dot(invT(M0)))
dM = SE3.group_from_algebra(ratio * dm)
Mt = dM.dot(M0)
cov_time = (1 - ratio) * a.cov + ratio * b.cov
return AngleAxisPose.FromM(Mt, id=time, cov=cov_time)
def test_Interpolate():
p1 = AngleAxisPose(np.zeros(3), np.zeros(3), 0,
block_diag(np.eye(3), np.eye(3)))
p2 = AngleAxisPose(np.zeros(3), np.ones(3), 1,
block_diag(np.eye(3), np.eye(3)))
AngleAxisPose.Interpolate(p1, p2, 0)
AngleAxisPose.Interpolate(p1, p2, 0.5)
AngleAxisPose.Interpolate(p1, p2, 1)
def got_tuple(img_msg, cam_odom, board_odom):
img = bridge.imgmsg_to_cv2(img_msg, "bgr8")
img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
body_to_world = transform_matrix_from_odom(cam_odom)
board_to_world = transform_matrix_from_odom(board_odom)
# Get detection from tracker
pixels = []
debug_img = bridge.imgmsg_to_cv2(img_msg, "bgr8")
gray = cv2.cvtColor(debug_img, cv2.COLOR_BGR2GRAY)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
axis = np.float32([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]]).reshape(-1, 3)
ret, corners = cv2.findChessboardCorners(
gray, (cols, rows), None, cv2.CALIB_CB_FILTER_QUADS)
cv2.drawChessboardCorners(debug_img, (cols, rows), corners, ret)
cv2.imshow('PreRefinement', debug_img)
if ret == True:
if K is None:
print 'K not initialized yet!'
return
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)
corners2.shape = (corners2.shape[0], corners2.shape[-1])
# Find the rotation and translation vectors.
ret, rvecs, tvecs = cv2.solvePnP(
objp, corners2, K, np.array([[0, 0, 0, 0]], dtype=np.float32))
if ret == True:
rodRotMat = cv2.Rodrigues(rvecs)
tag_in_cam = np.eye(4)
tag_in_cam[:3, :3] = rodRotMat[0]
tag_in_cam[:3, 3] = tvecs[:, 0]
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(
axis, rvecs, tvecs, K, np.zeros((1, 4)))
if visualize:
cv2.drawChessboardCorners(
debug_img, (cols, rows), corners2, ret)
img_msg = bridge.cv2_to_imgmsg(debug_img)
img_msg.header.frame_id = 'cam'
img_pub.publish(img_msg)
K_msg.header.stamp = img_msg.header.stamp
cam_info_pub.publish(K_msg)
debug_img = draw(debug_img, corners2, imgpts)
print ret
cam_to_world = np.dot(body_to_world, cam_to_body)
broadcaster.sendTransform(body_to_world[:3, 3],
tf.transformations.quaternion_from_matrix(
body_to_world),
rospy.Time.now(),
'body',
"world")
broadcaster.sendTransform(board_to_world[:3, 3],
tf.transformations.quaternion_from_matrix(
board_to_world),
rospy.Time.now(),
'board',
"world")
broadcaster.sendTransform(cam_to_body[:3, 3],
tf.transformations.quaternion_from_matrix(
cam_to_body),
rospy.Time.now(),
'cam',
"body")
broadcaster.sendTransform(tag_in_cam[:3, 3],
tf.transformations.quaternion_from_matrix(
tag_in_cam),
rospy.Time.now(),
'tag',
"cam")
broadcaster.sendTransform(tag_in_board[:3, 3],
tf.transformations.quaternion_from_matrix(
tag_in_board),
rospy.Time.now(),
'tag_gt',
"board")
# tag_in_cam = np.eye(4).astype(datatype)
# # Now see if the 3D points projected make sense.
# tag_pts = np.concatenate((objp, np.ones((objp.shape[0], 1))), axis=1).transpose()
# tag_pts_in_world = np.dot(
# board_to_world, np.dot(tag_in_board, tag_pts))
# tag_pts_in_cam = np.dot(np.linalg.inv(cam_to_world), tag_pts_in_world)
# projections = np.dot(K, tag_pts_in_cam[:3, :])
# projections /= projections[2]
# projections = projections[:2].transpose()
# pixels = []
# Draw these pixels
# pdb.set_trace()
tag_in_cam_mocap_approx = np.dot(np.linalg.inv(
cam_to_world), np.dot(board_to_world, tag_in_board))
diff = np.dot(np.linalg.inv(tag_in_cam), tag_in_cam_mocap_approx)
diff = se3.vector_from_algebra(SE3.algebra_from_group(diff))
diffs_vector.append(diff)
print diff
print np.linalg.norm(diff[:3])
# I'm curious to see the projected mocap frame in the image too
pts = np.eye(4)
pts[3, :] = 1
origin_in_cam = np.dot(tag_in_cam_mocap_approx, pts)
projections = np.dot(K, origin_in_cam[:3, :])
projections /= projections[2]
projections = projections.astype(np.float32)
debug_img = cv2.line(debug_img, tuple(projections[:2, 3]), tuple(
projections[:2, 0]), (0, 0, 127), 1)
debug_img = cv2.line(debug_img, tuple(projections[:2, 3]), tuple(
projections[:2, 1]), (0, 127, 0), 1)
debug_img = cv2.line(debug_img, tuple(projections[:2, 3]), tuple(
projections[:2, 2]), (127, 0, 0), 1)
cv2.imshow('img', debug_img)
cv2.waitKey(10)
data_tuples.append(
[corners2, body_to_world, board_to_world, tag_in_cam])
img_tuples.append(cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR))
