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))