def test_free(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
free_list = st.calc_free([0, 0, 1, 1, 0, 0])
self.assertEqual(free_list[0], False)
self.assertEqual(free_list[1], False)
self.assertEqual(free_list[2], True)
self.assertEqual(free_list[3], True)
self.assertEqual(free_list[4], False)
self.assertEqual(free_list[5], False)
def test_inflate(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
st.inflate(reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1)))
expected = numpy.matrix(
[[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], float)
print ""
print st.transform
self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0,
6)
def test_free(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
free_list = st.calc_free( [0, 0, 1, 1, 0, 0] )
self.assertEqual(free_list[0], False)
self.assertEqual(free_list[1], False)
self.assertEqual(free_list[2], True)
self.assertEqual(free_list[3], True)
self.assertEqual(free_list[4], False)
self.assertEqual(free_list[5], False)
def test_inflate(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
st.inflate( reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) ))
expected = numpy.matrix( [[ 1, 0, 0, 1],
[ 0, 1, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]], float )
print ""
print st.transform
self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
def calculate_error(self, opt_all_vec):
print "x ",
sys.stdout.flush()
opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
full_param_vec = self.calculate_full_param_vec(opt_param_vec)
# Update the primitives with the new set of parameters
self._robot_params.inflate(full_param_vec)
# Update all the blocks' configs
for multisensor in self._multisensors:
multisensor.update_config(self._robot_params)
r_list = []
for multisensor, cb_pose_vec in zip(self._multisensors,
list(full_pose_arr)):
# Process cb pose
cb_points = SingleTransform(
cb_pose_vec).transform * self._robot_params.checkerboards[
multisensor.checkerboard].generate_points()
if (self._use_cov):
r_list.append(multisensor.compute_residual_scaled(cb_points))
else:
r_list.append(multisensor.compute_residual(cb_points))
r_vec = concatenate(r_list)
rms_error = numpy.sqrt(numpy.mean(r_vec**2))
print "%.3f " % rms_error,
sys.stdout.flush()
return array(r_vec)
def test_hard(self):
sample_nums = range(1, 6)
dir_name = os.path.dirname(os.path.realpath(__file__))
params_filenames = [
os.path.join(dir_name, 'data', 'single_transform_data',
'params_%02u.txt' % n) for n in sample_nums
]
transform_filenames = [
os.path.join(dir_name, 'data', 'single_transform_data',
'transform_%02u.txt' % n) for n in sample_nums
]
for params_filename, transform_filename in zip(params_filenames,
transform_filenames):
f_params = open(params_filename)
f_transforms = open(transform_filename)
param_elems = [float(x) for x in f_params.read().split()]
transform_elems = [float(x) for x in f_transforms.read().split()]
st = SingleTransform(param_elems)
expected_result = numpy.matrix(transform_elems)
expected_result.shape = 4, 4
self.assertAlmostEqual(
numpy.linalg.norm(st.transform - expected_result), 0.0, 4,
"Failed on %s" % params_filename)
def test_hard(self):
sample_nums = range(1, 6)
params_filenames = [
"test/data/single_transform_data/params_%02u.txt" % n
for n in sample_nums
]
transform_filenames = [
"test/data/single_transform_data/transform_%02u.txt" % n
for n in sample_nums
]
for params_filename, transform_filename in zip(params_filenames,
transform_filenames):
f_params = open(params_filename)
f_transforms = open(transform_filename)
param_elems = [float(x) for x in f_params.read().split()]
transform_elems = [float(x) for x in f_transforms.read().split()]
st = SingleTransform(param_elems)
expected_result = numpy.matrix(transform_elems)
expected_result.shape = 4, 4
self.assertAlmostEqual(
numpy.linalg.norm(st.transform - expected_result), 0.0, 4,
"Failed on %s" % params_filename)
def multisensor_pose_jacobian(self, opt_param_vec, pose_param_vec,
multisensor):
"""
Generates the jacobian from a target pose to the multisensor's residual.
Input:
- opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing)
- pose_param_vec: Vector of length 6 encoding the target's pose 0:3=translation 3:6=rotation_axis
- multisensor: The actual multisensor definition.
Output:
- J_scaled: An mx6 jacobian, where m is the length of multisensor's residual. There are 6 columns since the
target's pose is encoded using 6 parameters.
If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma
is the information matrix for this measurement.
"""
# Update the primitives with the new set of parameters
full_param_vec = self.calculate_full_param_vec(opt_param_vec)
self._robot_params.inflate(full_param_vec)
multisensor.update_config(self._robot_params)
cb_model = self._robot_params.checkerboards[multisensor.checkerboard]
local_cb_points = cb_model.generate_points()
# based on code from scipy.slsqp
x0 = pose_param_vec
epsilon = 1e-6
world_pts = SingleTransform(x0).transform * local_cb_points
f0 = multisensor.compute_residual(world_pts)
if (self._use_cov):
gamma_sqrt = multisensor.compute_marginal_gamma_sqrt(world_pts)
Jt = numpy.zeros([len(x0), len(f0)])
dx = numpy.zeros(len(x0))
for i in range(len(x0)):
dx[i] = epsilon
test_vec = x0 + dx
fTest = multisensor.compute_residual(
SingleTransform(test_vec).transform * local_cb_points)
Jt[i] = (fTest - f0) / epsilon
#import code; code.interact(local=locals())
dx[i] = 0.0
J = Jt.transpose()
if (self._use_cov):
J_scaled = gamma_sqrt * J
else:
J_scaled = J
return J_scaled
def test_easy_rot_1(self):
st = SingleTransform([0, 0, 0, 0, 0, pi / 2])
expected = numpy.matrix(
[[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], float)
print ""
print st.transform
self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0,
6)
def setUp(self):
print ""
config = '''
root: x
tip: y
joints: [j1, j2, j3]
active_joints: [j1, j2, j3]
axis: [6, 6, 6]
gearing: [1, 1, 1]
cov:
joint_angles: [1, 1, 1]
'''
config_dict = yaml.load(config)
config_dict['transforms'] = { 'j1': SingleTransform([1, 0, 0, 0, 0, 0]),
'j2': SingleTransform([1, 0, 0, 0, 0, 0]),
'j3': SingleTransform([1, 0, 2, 0, 0, 0]) }
self.chain = JointChain(config_dict)
def test_easy_trans_1(self):
st = SingleTransform([1, 2, 3, 0, 0, 0])
expected = numpy.matrix(
[[1, 0, 0, 1], [0, 1, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]], float)
print ""
print st.transform
self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0,
6)
def compute_pose(self, joint_pos):
pose = matrix(numpy.eye(4))
for before_chain_T in self._before_chain_Ts:
pose = pose * before_chain_T.transform
pose = pose * SingleTransform([
0, 0, 0, 0, joint_pos[0] * self._gearing, 0
]).transform # TODO: remove assumption of Y axis
for after_chain_T in self._after_chain_Ts:
pose = pose * after_chain_T.transform
return pose
def compute_errors_breakdown(error_calc, multisensors, opt_pose_arr):
errors_dict = {}
# Compute the error for each sensor type
for ms, k in zip(multisensors, range(len(multisensors))):
cb = error_calc._robot_params.checkerboards[ms.checkerboard]
cb_pose_vec = opt_pose_arr[k]
target_pts = SingleTransform(cb_pose_vec).transform * cb.generate_points()
for sensor in ms.sensors:
r_sensor = sensor.compute_residual(target_pts) * numpy.sqrt(sensor.terms_per_sample) # terms per sample is a hack to find rms distance, instead of a pure rms, based on each term
if sensor.sensor_id not in errors_dict.keys():
errors_dict[sensor.sensor_id] = []
errors_dict[sensor.sensor_id].append(r_sensor)
return errors_dict
def calculate_error(self, opt_all_vec):
opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
full_param_vec = self.calculate_full_param_vec(opt_param_vec)
# Update the primitives with the new set of parameters
self._robot_params.inflate(full_param_vec)
# Update all the blocks' configs
for multisensor in self._multisensors:
multisensor.update_config(self._robot_params)
r_list = []
id = 0
for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)):
# Process cb pose
cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points()
if (self._use_cov):
r_list.append(multisensor.compute_residual_scaled(cb_points))
else:
r_list.append(multisensor.compute_residual(cb_points))
cb_points_msgs = [ geometry_msgs.msg.Point(cur_pt[0,0], cur_pt[0,1], cur_pt[0,2]) for cur_pt in cb_points.T]
m = Marker()
m.header.frame_id = self._robot_params.base_link
m.ns = "points_3d"
m.id = id
m.type = Marker.SPHERE_LIST
m.action = Marker.MODIFY
m.points = cb_points_msgs
m.color.r = 1.0
m.color.g = 0.0
m.color.b = 1.0
m.color.a = 1.0
m.scale.x = 0.01
m.scale.y = 0.01
m.scale.z = 0.01
self.marker_pub.publish(m)
id += 1
r_vec = concatenate(r_list)
rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
print "\t\t\t\t\tRMS error: %.3f \r" % rms_error,
sys.stdout.flush()
return array(r_vec)
def test_params_to_config(self):
st = SingleTransform()
p = reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1))
config = st.params_to_config(p)
self.assertAlmostEqual(config[0], 1, 6)
self.assertAlmostEqual(config[1], 0, 6)
def test_deflate(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
p = reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1))
st.inflate(p)
result = st.deflate()
self.assertAlmostEqual(numpy.linalg.norm(p - result), 0.0, 6)
def calculate_jacobian(self, opt_all_vec):
"""
Full Jacobian:
The resulting returned jacobian can be thought of as a set of several concatenated jacobians as follows:
[ J_multisensor_0 ]
J = [ J_multisensor_1 ]
[ : ]
[ J_multisensor_M ]
where M is the number of multisensors, which is generally the number of calibration samples collected.
Multisensor Jacobian:
The Jacobian for a given multisensor is created by concatenating jacobians for each sensor
[ J_sensor_m_0 ]
J_multisensor_m = [ J_sensor_m_1 ]
[ : ]
[ J_sensor_m_S ]
where S is the number of sensors in this multisensor.
Sensor Jacobian:
A single sensor jacobian is created by concatenating jacobians for the system parameters and checkerboards
J_sensor_m_s = [ J_params_m_s | J_m_s_pose0 | J_m_s_pose1 | --- | J_m_s_CB_poseC ]
The number of rows in J_sensor_m_s is equal to the number of rows in this sensor's residual.
J_params_m_s shows how modifying the free system parameters affects the residual.
Each J_m_s_pose_c is 6 columns wide and shows how moving a certain target affects the residual. All
of the J_m_s_pose blocks are zero, except J_sensor_pose_m, since target m is the only target that
was viewed by the sensors in this multisensor.
"""
self._j_count += 1;
sys.stdout.write(" \r")
sys.stdout.write("Computing Jacobian.. (cycle #%d)\r" % self._j_count)
sys.stdout.flush()
#import scipy.optimize.slsqp.approx_jacobian as approx_jacobian
#J = approx_jacobian(opt_param_vec, self.calculate_error, 1e-6)
opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
# Allocate the full jacobian matrix
ms_r_len = [ms.get_residual_length() for ms in self._multisensors]
J = zeros([sum(ms_r_len), len(opt_all_vec)])
# Calculate at which row each multisensor starts and ends
ms_end_row = list(cumsum(ms_r_len))
ms_start_row = [0] + ms_end_row[:-1]
# Calculate at which column each multisensor starts and ends
ms_end_col = list(cumsum([6]*len(self._multisensors)) + len(opt_param_vec))
ms_start_col = [x-6 for x in ms_end_col]
for i,ms in enumerate(self._multisensors):
# Populate the parameter section for this multisensor
J_ms_params = J[ ms_start_row[i]:ms_end_row[i],
0:len(opt_param_vec) ]
s_r_len = [s.get_residual_length() for s in ms.sensors]
s_end_row = list(cumsum(s_r_len))
s_start_row = [0] + s_end_row[:-1]
target_pose_T = SingleTransform(full_pose_arr[i,:]).transform
# Fill in parameter section one sensor at a time
for k,s in enumerate(ms.sensors):
J_s_params = J_ms_params[ s_start_row[k]:s_end_row[k], :]
J_s_params[:,:] = self.single_sensor_params_jacobian(opt_param_vec, target_pose_T, ms.checkerboard, s)
# Populate the pose section for this multisensor
J_ms_pose = J[ ms_start_row[i]:ms_end_row[i],
ms_start_col[i]:ms_end_col[i]]
assert(J_ms_pose.shape[1] == 6)
J_ms_pose[:,:] = self.multisensor_pose_jacobian(opt_param_vec, full_pose_arr[i,:], ms)
sys.stdout.flush()
return J
def test_length(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
self.assertEqual(st.get_length(), 6)
print " Sample %d: %.6f" % (i, rms_error)
i += 1
error_cat = numpy.concatenate(error_list)
rms_error = numpy.sqrt(numpy.mean(error_cat**2))
print " Total: %.6f" % rms_error
# Calculate loop errors
chain_sensors = [[
s for s in ms.sensors if s.sensor_id == cur_loop['3d']
][0] for ms in multisensors_pruned]
cam_sensors = [[
s for s in ms.sensors if s.sensor_id == cur_loop['cam']
][0] for ms in multisensors_pruned]
fk_points = [s.get_measurement() for s in chain_sensors]
cb_points = [
SingleTransform(pose).transform *
system_def.checkerboards[ms.checkerboard].generate_points()
for pose, ms in zip(cb_poses_pruned, multisensors_pruned)
]
points_list_fk = [
geometry_msgs.msg.Point(cur_pt[0, 0], cur_pt[0, 1], cur_pt[0, 2])
for cur_pt in list(numpy.concatenate(fk_points, 1).T)
]
points_list_guess = [
geometry_msgs.msg.Point(cur_pt[0, 0], cur_pt[0, 1], cur_pt[0, 2])
for cur_pt in list(numpy.concatenate(cb_points, 1).T)
]
m = Marker()
m.header.frame_id = system_def.base_link
def test_params_to_config(self):
st = SingleTransform()
p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )
config = st.params_to_config(p)
self.assertAlmostEqual( config[0], 1, 6)
self.assertAlmostEqual( config[1], 0, 6)
def test_length(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
self.assertEqual(st.get_length(), 6)
def configure(self, urdf, config_dict):
self.urdf = urdf
self.fakes = list() # list of fake joints which must be later removed.
# set base_link to which all measurements are based
try:
self.base_link = config_dict['base_link']
except:
self.base_link = 'base_link'
transforms = config_dict['transforms']
checkerboards = config_dict["checkerboards"]
config_dict = config_dict['sensors']
# length of parameter vector
cur_index = 0
# clean up joints
for joint_name in urdf.joints.keys():
j = urdf.joints[joint_name]
if j.origin == None:
j.origin = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0])
if j.origin.rotation == None:
j.origin.rotation = [0.0, 0.0, 0.0]
if j.origin.position == None:
j.origin.position = [0.0, 0.0, 0.0]
# build our transforms
self.transforms = dict()
for joint_name in urdf.joints.keys():
joint_name = str(joint_name)
j = urdf.joints[joint_name]
rot = j.origin.rotation
rot = RPY_to_angle_axis(rot)
p = j.origin.position + rot
self.transforms[joint_name] = SingleTransform(p, joint_name)
self.transforms[joint_name].start = cur_index
self.transforms[joint_name].end = cur_index + self.transforms[
joint_name].get_length()
cur_index = self.transforms[joint_name].end
for name, transform in transforms.items():
transform = [eval(str(x)) for x in transform]
transform[3:6] = RPY_to_angle_axis(transform[3:6])
try:
self.transforms[name].inflate(
reshape(matrix(transform, float), (-1, 1)))
except:
rospy.logwarn("Transform not found in URDF %s", name)
self.transforms[name] = SingleTransform(transform, name)
self.transforms[name].start = cur_index
self.transforms[
name].end = cur_index + self.transforms[name].get_length()
cur_index = self.transforms[name].end
# build our chains
self.chains = dict()
chain_dict = config_dict['chains']
for chain_name in config_dict['chains']:
# create configuration
this_config = config_dict['chains'][chain_name]
this_config['joints'] = urdf.get_chain(this_config['root'],
this_config['tip'],
links=False)
this_config['active_joints'] = list()
this_config['axis'] = list()
this_config['transforms'] = dict()
this_config['gearing'] = config_dict['chains'][chain_name][
'gearing'] # TODO: This always defaults to 1.0?
this_config['cov'] = config_dict['chains'][chain_name]['cov']
for joint_name in this_config["joints"]:
this_config['transforms'][joint_name] = self.transforms[
joint_name]
if urdf.joints[joint_name].joint_type in [
'revolute', 'continuous'
]:
this_config["active_joints"].append(joint_name)
axis = list(urdf.joints[joint_name].axis.split())
this_config["axis"].append(
sum([i[0] * int(i[1]) for i in zip([4, 5, 6], axis)]))
# we can handle limited rotations here
rot = urdf.joints[joint_name].origin.rotation
if rot != None and (
sum([abs(x) for x in rot]) -
rot[abs(this_config["axis"][-1]) - 4]) > 0.001:
print 'Joint origin is rotated, calibration will fail: ', joint_name
elif urdf.joints[joint_name].joint_type == 'prismatic':
this_config["active_joints"].append(joint_name)
axis = list(urdf.joints[joint_name].axis.split())
this_config["axis"].append(
sum([i[0] * int(i[1]) for i in zip([1, 2, 3], axis)]))
elif urdf.joints[joint_name].joint_type != 'fixed':
print 'Unknown joint type:', urdf.joints[
joint_name].joint_type
# put a checkerboard in it's hand
self.urdf.add_link(Link(chain_name + "_cb_link"))
self.urdf.add_joint(
Joint(chain_name + "_cb",
this_config['tip'],
chain_name + "_cb_link",
"fixed",
origin=Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0])))
self.fakes += [chain_name + "_cb_link", chain_name + "_cb"]
self.chains[chain_name] = JointChain(this_config)
self.chains[chain_name].start = cur_index
self.chains[chain_name].end = cur_index + self.chains[
chain_name].get_length()
cur_index = self.chains[chain_name].end
# build our lasers:
self.tilting_lasers = dict()
if 'tilting_lasers' in config_dict.keys():
self.tilting_lasers, cur_index = init_primitive_dict(
cur_index, config_dict["tilting_lasers"], TiltingLaser)
self.rectified_cams, cur_index = init_primitive_dict(
cur_index, config_dict["rectified_cams"], RectifiedCamera)
self.checkerboards, cur_index = init_primitive_dict(
cur_index, checkerboards, Checkerboard)
self.length = cur_index
def test_deflate(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )
st.inflate(p)
result = st.deflate()
self.assertAlmostEqual(numpy.linalg.norm(p-result), 0.0, 6)
