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