def jac_function(theta): body_to_c = SE3.group_from_algebra( se3.algebra_from_vector(theta[:6])) # tag_in_board_offset = theta[6:] t_in_board = tag_in_board.copy() # t_in_board[:3,3] += tag_in_board_offset # t_in_board = np.dot(t_in_board, SE3.group_from_algebra( # se3.algebra_from_vector(tag_in_board_offset))) error = 0 img_count = 0 jacs = [] for measurement, body_to_world, board_to_world, tag_in_cam in data_tuples: 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(t_in_board, tag_pts)) tag_pts_in_body = np.dot(np.linalg.inv( body_to_world), tag_pts_in_world) tag_pts_in_cam = np.dot(body_to_c, tag_pts_in_body) projections, jac = cv2.projectPoints( tag_pts_in_body.T[:, :3], theta[:3], theta[3:6], K, np.zeros((1, 4))) jacs.append(jac[:, :6]) return np.vstack(np.array(jacs))
def cost_function_tuple_offset( params): cam_to_body = SE3.group_from_algebra(se3.algebra_from_vector(params[:6])) tuple_offset = int(params[6]*100) # Use a central region of data tuples +- 100 # The offset determines the start of the measurement offset # pdb.set_trace() # measurements_offset = data_tuples[buffer_size + tuple_offset: -buffer_size + tuple_offset, 0] # bodys_to_world_tuple_offset = data_tuples[buffer_size:-buffer_size, 1] # boards_to_world_tuple_offset = data_tuples[buffer_size:-buffer_size, 2] # offset_tuples = np.concatenate(measurements_offset, bodys_to_world_offset, boards_to_world_offset, axis=1) error = 0 for i in range(len(data_tuples) - buffer_size*2): measurement = data_tuples[i + buffer_size + tuple_offset][0] body_to_world = data_tuples[i + buffer_size][1] board_to_world = data_tuples[i + buffer_size][2] cam_to_world = np.dot(body_to_world, cam_to_body) tag_pts = np.array([[-s, -s, 0, 1], [s, -s, 0, 1], [s, s, 0, 1], [-s, s, 0, 1]]).transpose() tag_in_board = np.array( [[0, -1, 0, s], [1, 0, 0, s], [0, 0, 1, 0], [0, 0, 0, 1]]) 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() error += np.linalg.norm(measurement - projections) return error
def cost_function(theta): body_to_c = SE3.group_from_algebra( se3.algebra_from_vector(theta[:6])) # tag_in_board_offset = theta[6:] t_in_board = tag_in_board.copy() # t_in_board[:3,3] += tag_in_board_offset # t_in_board = np.dot(t_in_board, SE3.group_from_algebra( # se3.algebra_from_vector(tag_in_board_offset))) error = 0 img_count = 0 residual_vectors = [] for measurement, body_to_world, board_to_world, tag_in_cam in data_tuples: 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(t_in_board, tag_pts)) tag_pts_in_body = np.dot(np.linalg.inv( body_to_world), tag_pts_in_world) tag_pts_in_cam = np.dot(body_to_c, tag_pts_in_body) projections, jac = cv2.projectPoints( tag_pts_in_body.T[:, :3], theta[:3], theta[3:6], K, np.zeros((1, 4))) projections = projections.astype(np.float32) # cv2.drawChessboardCorners(debug_img, (rows, cols), projections, True) projections.shape = (projections.shape[0], projections.shape[-1]) measurement.shape = (measurement.shape[0], measurement.shape[-1]) if img_count == 0: debug_img = img_tuples[img_count].copy() for i in range(projections.shape[0]): cv2.circle( debug_img, (projections[i, 0], projections[i, 1]), 2, (255, 0, 0), 1) cv2.circle( debug_img, (measurement[i, 0], measurement[i, 1]), 2, (0, 0, 255), 1) cv2.line(debug_img, (measurement[i, 0], measurement[i, 1]), (projections[i, 0], projections[i, 1]), (0, 255, 0)) cv2.imshow('cost function img', debug_img) cv2.waitKey(10) # pdb.set_trace() img_count += 1 residual_vectors.append((measurement - projections).ravel()) # np.linalg.norm(measurement - projections) # error += np.sum((measurement - projections), axis=0) return np.array(residual_vectors).ravel()
def cost_function( cam_to_body_log): cam_to_body = SE3.group_from_algebra(se3.algebra_from_vector(cam_to_body_log)) error = 0 for measurement, body_to_world, board_to_world in data_tuples: cam_to_world = np.dot(body_to_world, cam_to_body) tag_pts = np.array([[-s, -s, 0, 1], [s, -s, 0, 1], [s, s, 0, 1], [-s, s, 0, 1]]).transpose() tag_in_board = np.array( [[0, -1, 0, s], [1, 0, 0, s], [0, 0, 1, 0], [0, 0, 0, 1]]) 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() error += np.linalg.norm(measurement - projections) return error
axes_to_plot = range(6) for i in [cam_id]: # range(num_images): pose_cpp = poses[i] depth_img_cpp = depth_imgs[i] sampled_poses = [] ray_likelihoods = [[]] * 6 for j in axes_to_plot: print '!!Evaluating for axis ' + labels[j] likes = [] for k in range(num_poses): print "Axis::{0} Pose::{1}".format(j, k) delta_vector = np.zeros(6) delta_vector[j] = deltas[k] T = SE3.group_from_algebra( se3.algebra_from_vector(delta_vector)) pose = np.dot(pose_cpp, T) sampled_poses.append(pose) # print 'pose is '+str(pose) mrfmap.inference.set_pose(i, pose.astype(dtype)) # pdb.set_trace() sum = mrfmap.inference.compute_likelihood(i) likes.append(sum) ray_likelihoods[j] = np.array(likes) # viewer.visualize_poses(sampled_poses, resolution, scaled_dims) # viewer.show() f, ax = plt.subplots(6, 1) handles = []
if use_bag: with rosbag.Bag(path, 'r') as bag: counter = 0 for topic, msg, t in bag.read_messages(topics_to_parse): if topic in topics_to_parse: index = topics_to_parse.index(topic) subs[index].signalMessage(msg) counter += 1 if counter%1000 == 0: print 'Read {0} tuples'.format(counter) # Try to use a black box optimizer print 'Starting optimization...' from scipy.optimize import minimize initial_guess = np.array([0,0,0,0,0,0,-0.1]) # Since initial guess is pretty close to unity result = minimize(cost_function_tuple_offset, initial_guess, bounds=np.array([[-1,1],[-1,1],[-1,1],[-1,1],[-1,1],[-1,1], [-1, 1]])) print 'Done, results is' print result print SE3.group_from_algebra(se3.algebra_from_vector(result.x[:6])) print result.x[6] pdb.set_trace() else: rospy.Subscriber(topics_to_parse[0], Image, lambda msg: subs[0].signalMessage(msg)) rospy.Subscriber(topics_to_parse[1], Odometry, lambda msg: subs[1].signalMessage(msg)) rospy.Subscriber(topics_to_parse[2], Odometry, lambda msg: subs[2].signalMessage(msg)) rospy.spin()