class chain_test():
def __init__(self, name):
path = "/u/calibration/git/care-o-bot/cob_calibration/cob_calibration_config/cob3-3/"
with open(path + "autogenerated/system.yaml", "r") as f:
system = yaml.safe_load(f)
with open(path + "user_defined/sensors.yaml", "r") as f:
sensors = yaml.safe_load(f)
r = RobotParams()
r.configure(system)
self.full_chain = FullChainRobotParams(
sensors["camera_chains"][0]["chain"], sensors)
self.full_chain.update_config(r)
self.calc_block = self.full_chain.calc_block
self.state = None
self.name = name
def callback(self, data):
self.state = data
self.state.header.frame_id = "torso_chain"
def test(self):
rospy.Subscriber("/torso_controller/state",
JointTrajectoryControllerState, self.callback)
listener = tf.TransformListener()
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
if self.state is None:
rospy.logwarn("No Chainstate received")
rate.sleep()
continue
try:
(trans,
rot) = listener.lookupTransform('/base_link',
'/head_color_camera_l_link',
rospy.Time(0))
T1 = array(self.calc_block.fk([self.state]))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
m = array(tf.transformations.quaternion_matrix(rot))
m[0:3, 3] = trans
print "--------------------%s----------------------" % self.name
#print m
#print T1
#print "Diff: "
#print m-T1
#print numpy.linalg.norm(m-T1)
# offset in mm
trans_tf = array(m[0:3, 3])
trans_fc = array(T1[0:3, 3])
delta_trans = trans_tf - trans_fc
delta = sqrt(sum(delta_trans**2))
print "distance_diff: ", delta
a_fc, _, _ = tf.transformations.rotation_from_matrix(T1)
a_tf, _, _ = tf.transformations.rotation_from_matrix(m)
print "angle_diff: ", (a_tf - a_fc) * 180 / pi
rate.sleep()
class CameraChainSensor:
def __init__(self, config_dict, M_cam, M_chain):
"""
Generates a single sensor block for a single configuration
Inputs:
- config_dict: The configuration for this specific sensor. This looks exactly like
a single element from the valid_configs list passed into CameraChainBundler.__init__
- M_cam: The camera measurement of type calibration_msgs/CameraMeasurement
- M_chain: The chain measurement of type calibration_msgs/ChainMeasurement
"""
self.sensor_type = "camera"
self.sensor_id = config_dict["camera_id"]
self._config_dict = config_dict
self._M_cam = M_cam
self._M_chain = M_chain
self._chain = FullChainRobotParams(config_dict["chain"])
self.terms_per_sample = 2
def update_config(self, robot_params):
"""
On each optimization cycle the set of system parameters that we're optimizing over will change. Thus,
after each change in parameters we must call update_config to ensure that our calculated residual reflects
the newest set of system parameters.
"""
self._camera = robot_params.rectified_cams[ self._config_dict["camera_id"] ]
if self._chain is not None:
self._chain.update_config(robot_params)
def compute_residual(self, target_pts):
"""
Computes the measurement residual for the current set of system parameters and target points
Input:
- target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
Output:
- r: 2N long vector, storing pixel residuals for the target points in the form [u1, v1, u2, v2, ..., uN, vN]
"""
z_mat = self.get_measurement()
h_mat = self.compute_expected(target_pts)
assert(z_mat.shape[1] == 2)
assert(h_mat.shape[1] == 2)
assert(z_mat.shape[0] == z_mat.shape[0])
r = array(reshape(h_mat - z_mat, [-1,1]))[:,0]
return r
def compute_residual_scaled(self, target_pts):
"""
Computes the residual, and then scales it by sqrt(Gamma), where Gamma
is the information matrix for this measurement (Cov^-1).
"""
r = self.compute_residual(target_pts)
gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
r_scaled = gamma_sqrt * matrix(r).T
return array(r_scaled.T)[0]
def compute_marginal_gamma_sqrt(self, target_pts):
"""
Calculates the square root of the information matrix for the measurement of the
current set of system parameters at the passed in set of target points.
"""
import scipy.linalg
cov = self.compute_cov(target_pts)
gamma = matrix(zeros(cov.shape))
num_pts = self.get_residual_length()/2
for k in range(num_pts):
#print "k=%u" % k
first = 2*k
last = 2*k+2
sub_cov = matrix(cov[first:last, first:last])
sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
sub_gamma_sqrt = real(sub_gamma_sqrt_full)
assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
gamma[first:last, first:last] = sub_gamma_sqrt
return gamma
def get_residual_length(self):
N = len(self._M_cam.image_points)
return N*2
# Get the observed measurement in a Nx2 Matrix
def get_measurement(self):
"""
Get the target's pixel coordinates as measured by the actual sensor
"""
camera_pix = numpy.matrix([[pt.x, pt.y] for pt in self._M_cam.image_points])
return camera_pix
def compute_expected(self, target_pts):
"""
Compute the expected pixel coordinates for a set of target points.
target_pts: 4xN matrix, storing feature points of the target, in homogeneous coords
Returns: target points projected into pixel coordinates, in a Nx2 matrix
"""
if self._M_chain is not None:
return self._compute_expected(self._M_chain.chain_state, target_pts)
else:
return self._compute_expected(None, target_pts)
def _compute_expected(self, chain_state, target_pts):
"""
Compute what measurement we would expect to see, based on the current system parameters
and the specified target point locations.
Inputs:
- chain_state: The joint angles of this sensor's chain of type sensor_msgs/JointState.
- target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
Returns: target points projected into pixel coordinates, in a Nx2 matrix
"""
# Camera pose in root frame
camera_pose_root = self._chain.calc_block.fk(chain_state)
cam_frame_pts = camera_pose_root.I * target_pts
# Do the camera projection
pixel_pts = self._camera.project(self._M_cam.cam_info.P, cam_frame_pts)
return pixel_pts.T
def compute_expected_J(self, target_pts):
"""
The output Jacobian J shows how moving target_pts in cartesian space affects
the expected measurement in (u,v) camera coordinates.
For n points in target_pts, J is a 2nx3n matrix
Note: This doesn't seem to be used anywhere, except maybe in some drawing code
"""
epsilon = 1e-8
N = len(self._M_cam.image_points)
Jt = zeros([N*3, N*2])
for k in range(N):
# Compute jacobian for point k
sub_Jt = zeros([3,2])
x = target_pts[:,k].copy()
f0 = self.compute_expected(x)
for i in [0,1,2]:
x[i,0] += epsilon
fTest = self.compute_expected(x)
sub_Jt[i,:] = array((fTest - f0) / epsilon)
x[i,0] -= epsilon
Jt[k*3:(k+1)*3, k*2:(k+1)*2] = sub_Jt
return Jt.T
def compute_cov(self, target_pts):
'''
Computes the measurement covariance in pixel coordinates for the given
set of target points (target_pts)
Input:
- target_pts: 4xN matrix, storing N feature points of the target, in homogeneous coords
'''
epsilon = 1e-8
if self._M_chain is not None:
num_joints = len(self._M_chain.chain_state.position)
Jt = zeros([num_joints, self.get_residual_length()])
x = JointState()
x.position = self._M_chain.chain_state.position[:]
# Compute the Jacobian from the chain's joint angles to pixel residuals
f0 = reshape(array(self._compute_expected(x, target_pts)), [-1])
for i in range(num_joints):
x.position = [cur_pos for cur_pos in self._M_chain.chain_state.position]
x.position[i] += epsilon
fTest = reshape(array(self._compute_expected(x, target_pts)), [-1])
Jt[i] = (fTest - f0)/epsilon
cov_angles = [x*x for x in self._chain.calc_block._chain._cov_dict['joint_angles']]
# Transform the chain's covariance from joint angle space into pixel space using the just calculated jacobian
chain_cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
cam_cov = matrix(zeros([self.get_residual_length(), self.get_residual_length()]))
# Convert StdDev into variance
var_u = self._camera._cov_dict['u'] * self._camera._cov_dict['u']
var_v = self._camera._cov_dict['v'] * self._camera._cov_dict['v']
for k in range(cam_cov.shape[0]/2):
cam_cov[2*k , 2*k] = var_u
cam_cov[2*k+1, 2*k+1] = var_v
# Both chain and camera covariances are now in measurement space, so we can simply add them together
if self._M_chain is not None:
cov = chain_cov + cam_cov
else:
cov = cam_cov
return cov
def build_sparsity_dict(self):
"""
Build a dictionary that defines which parameters will in fact affect this measurement.
Returned dictionary should be of the following form:
transforms:
my_transformA: [1, 1, 1, 1, 1, 1]
my_transformB: [1, 1, 1, 1, 1, 1]
dh_chains:
my_chain:
dh:
- [1, 1, 1, 1]
- [1, 1, 1, 1]
|
- [1, 1, 1, 1]
gearing: [1, 1, ---, 1]
rectified_cams:
my_cam:
baseline_shift: 1
f_shift: 1
cx_shift: 1
cy_shift: 1
"""
sparsity = dict()
sparsity['transforms'] = {}
for cur_transform_name in ( self._config_dict['chain']['before_chain'] + self._config_dict['chain']['after_chain'] ):
sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
sparsity['dh_chains'] = {}
if self._M_chain is not None:
chain_id = self._config_dict['chain']['chain_id']
num_links = self._chain.calc_block._chain._M
assert(num_links == len(self._M_chain.chain_state.position))
sparsity['dh_chains'][chain_id] = {}
sparsity['dh_chains'][chain_id]['dh'] = [ [1,1,1,1] ] * num_links
sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
sparsity['rectified_cams'] = {}
sparsity['rectified_cams'][self.sensor_id] = dict( [(x,1) for x in self._camera.get_param_names()] )
return sparsity
class ChainSensor:
def __init__(self, config_dict, M_chain, target_id, config):
self.sensor_type = "chain"
self.sensor_id = config_dict["sensor_id"]
self._config_dict = config_dict
self._config = config
self._M_chain = M_chain
self._target_id = target_id
self._full_chain = FullChainRobotParams(
self._config_dict, self._config)
self.terms_per_sample = 3
def update_config(self, robot_params):
self._full_chain.update_config(robot_params)
self._checkerboard = robot_params.checkerboards[self._target_id]
def compute_residual(self, target_pts):
h_mat = self.compute_expected(target_pts)
z_mat = self.get_measurement()
assert(h_mat.shape == z_mat.shape)
assert(h_mat.shape[0] == 4)
r_mat = h_mat[0:3, :] - z_mat[0:3, :]
r = array(reshape(r_mat.T, [-1, 1]))[:, 0]
return r
def compute_residual_scaled(self, target_pts):
"""
Computes the residual, and then scales it by sqrt(Gamma), where Gamma
is the information matrix for this measurement (Cov^-1).
"""
r = self.compute_residual(target_pts)
gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
r_scaled = gamma_sqrt * matrix(r).T
return array(r_scaled.T)[0]
def compute_marginal_gamma_sqrt(self, target_pts):
"""
Calculates the square root of the information matrix for the measurement of the
current set of system parameters at the passed in set of target points.
"""
import scipy.linalg
# ----- Populate Here -----
cov = self.compute_cov(target_pts)
gamma = matrix(zeros(cov.shape))
num_pts = self.get_residual_length() / self.terms_per_sample
#code.interact(local=locals())
for k in range(num_pts):
#print "k=%u" % k
first = self.terms_per_sample * k
last = self.terms_per_sample * k + self.terms_per_sample
sub_cov = matrix(cov[first:last, first:last])
sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
sub_gamma_sqrt = real(sub_gamma_sqrt_full)
assert(scipy.linalg.norm(
sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
gamma[first:last, first:last] = sub_gamma_sqrt
return gamma
def compute_cov(self, target_pts):
'''
Computes the measurement covariance in pixel coordinates for the given
set of target points (target_pts)
Input:
- target_pts: 4xN matrix, storing N feature points of the target, in homogeneous coords
'''
_ones = ones([7, self.get_residual_length()])
cov = matrix(diag([0.01] * self.get_residual_length()))
epsilon = 1e-8
i_joints = []
num_joints = 0
for i,c in enumerate(self._M_chain):
l = len(c.actual.positions)
i_joints.extend([i] * l)
num_joints+=l
#num_joints = sum(num_joints)
#num_joints = len(self._M_chain.actual.position)
Jt = zeros([num_joints, self.get_residual_length()])
x = deepcopy(self._M_chain)
#x.actual.position = self._M_chain.actual.position[:]
f0 = reshape(array(self._calc_fk_target_pts(x)[0:3, :].T), [-1])
for i in range(num_joints):
x[i_joints[i]].header.frame_id = self._M_chain[i_joints[i]].header.frame_id
x[i_joints[i]].actual.positions = list(self._M_chain[i_joints[i]].actual.positions[:])
x[i_joints[i]].actual.positions[i] += epsilon
fTest = reshape(array(self._calc_fk_target_pts(x)[0:3, :].T), [-1])
Jt[i] = (fTest - f0) / epsilon
cov_angles = []
for c in self._full_chain.calc_block._chains:
cov_angles.extend([ x * x for x in c._chain._cov_dict])
#cov_angles = [x * x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']]
#import code; code.interact(local=locals())
cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
return cov
def get_residual_length(self):
pts = self._checkerboard.generate_points()
N = pts.shape[1]
return N * 3
def get_measurement(self):
'''
Returns a 4xN matrix with the locations of the checkerboard points in homogenous coords,
as per the forward kinematics of the chain
'''
return self._calc_fk_target_pts()
def _calc_fk_target_pts(self, M_chain=None):
#code.interact(local=locals())
M_chain = self._M_chain if M_chain is None else M_chain
# Get the target's model points in the frame of the tip of the target chain
target_pts_tip = self._checkerboard.generate_points()
# Target pose in root frame
target_pose_root = self._full_chain.calc_block.fk(M_chain)
# Transform points into the root frame
target_pts_root = target_pose_root * target_pts_tip
return target_pts_root
def compute_expected(self, target_pts):
return target_pts
def build_sparsity_dict(self):
"""
Build a dictionary that defines which parameters will in fact affect this measurement.
Returned dictionary should be of the following form:
transforms:
my_transformA: [1, 1, 1, 1, 1, 1]
my_transformB: [1, 1, 1, 1, 1, 1]
dh_chains:
my_chain:
dh:
- [1, 1, 1, 1]
- [1, 1, 1, 1]
|
- [1, 1, 1, 1]
gearing: [1, 1, ---, 1]
rectified_cams:
my_cam:
baseline_shift: 1
f_shift: 1
cx_shift: 1
cy_shift: 1
"""
sparsity = dict()
sparsity['transforms'] = {}
chain_transform_names = [chain['before_chain'] + chain['after_chain'] for chain in self._config['chains']
if chain['chain_id'] in self._config_dict['chains']][0]
for cur_transform_name in (self._config_dict['before_chain'] + self._config_dict['after_chain'] + chain_transform_names):
sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
sparsity['dh_chains'] = {}
chain_ids = self._config_dict['chains']
for chain in self._full_chain.calc_block._chains:
chain_id = chain._config_dict["chain_id"]
num_links = chain._chain._M
#num_links = self._full_chain.calc_block._chains._M
#assert(num_links == len(self._M_chain.chain_state.position))
sparsity['dh_chains'][chain_id] = {}
sparsity['dh_chains'][chain_id]['dh'] = [[1, 1, 1, 1,1,1]] * num_links
sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
sparsity['checkerboards'] = {}
sparsity['checkerboards'][self._target_id] = {'spacing_x': 1,
'spacing_y': 1}
return sparsity
class CameraChainSensor:
def __init__(self, config_dict, M_cam, M_chain):
"""
Generates a single sensor block for a single configuration
Inputs:
- config_dict: The configuration for this specific sensor. This looks exactly like
a single element from the valid_configs list passed into CameraChainBundler.__init__
- M_cam: The camera measurement of type calibration_msgs/CameraMeasurement
- M_chain: The chain measurement of type calibration_msgs/ChainMeasurement
"""
self.sensor_type = "camera"
self.sensor_id = config_dict["camera_id"]
self._config_dict = config_dict
self._M_cam = M_cam
self._M_chain = M_chain
self._chain = FullChainRobotParams(config_dict["chain"])
self.terms_per_sample = 2
def update_config(self, robot_params):
"""
On each optimization cycle the set of system parameters that we're optimizing over will change. Thus,
after each change in parameters we must call update_config to ensure that our calculated residual reflects
the newest set of system parameters.
"""
self._camera = robot_params.rectified_cams[
self._config_dict["camera_id"]]
if self._chain is not None:
self._chain.update_config(robot_params)
def compute_residual(self, target_pts):
"""
Computes the measurement residual for the current set of system parameters and target points
Input:
- target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
Output:
- r: 2N long vector, storing pixel residuals for the target points in the form [u1, v1, u2, v2, ..., uN, vN]
"""
z_mat = self.get_measurement()
h_mat = self.compute_expected(target_pts)
assert (z_mat.shape[1] == 2)
assert (h_mat.shape[1] == 2)
assert (z_mat.shape[0] == z_mat.shape[0])
r = array(reshape(h_mat - z_mat, [-1, 1]))[:, 0]
return r
def compute_residual_scaled(self, target_pts):
"""
Computes the residual, and then scales it by sqrt(Gamma), where Gamma
is the information matrix for this measurement (Cov^-1).
"""
r = self.compute_residual(target_pts)
gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
r_scaled = gamma_sqrt * matrix(r).T
return array(r_scaled.T)[0]
def compute_marginal_gamma_sqrt(self, target_pts):
"""
Calculates the square root of the information matrix for the measurement of the
current set of system parameters at the passed in set of target points.
"""
import scipy.linalg
cov = self.compute_cov(target_pts)
gamma = matrix(zeros(cov.shape))
num_pts = self.get_residual_length() / 2
for k in range(num_pts):
#print "k=%u" % k
first = 2 * k
last = 2 * k + 2
sub_cov = matrix(cov[first:last, first:last])
sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
sub_gamma_sqrt = real(sub_gamma_sqrt_full)
assert (scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) <
1e-6)
gamma[first:last, first:last] = sub_gamma_sqrt
return gamma
def get_residual_length(self):
N = len(self._M_cam.image_points)
return N * 2
# Get the observed measurement in a Nx2 Matrix
def get_measurement(self):
"""
Get the target's pixel coordinates as measured by the actual sensor
"""
camera_pix = numpy.matrix([[pt.x, pt.y]
for pt in self._M_cam.image_points])
return camera_pix
def compute_expected(self, target_pts):
"""
Compute the expected pixel coordinates for a set of target points.
target_pts: 4xN matrix, storing feature points of the target, in homogeneous coords
Returns: target points projected into pixel coordinates, in a Nx2 matrix
"""
if self._M_chain is not None:
return self._compute_expected(self._M_chain.chain_state,
target_pts)
else:
return self._compute_expected(None, target_pts)
def _compute_expected(self, chain_state, target_pts):
"""
Compute what measurement we would expect to see, based on the current system parameters
and the specified target point locations.
Inputs:
- chain_state: The joint angles of this sensor's chain of type sensor_msgs/JointState.
- target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
Returns: target points projected into pixel coordinates, in a Nx2 matrix
"""
# Camera pose in root frame
camera_pose_root = self._chain.calc_block.fk(chain_state)
cam_frame_pts = camera_pose_root.I * target_pts
# Do the camera projection
pixel_pts = self._camera.project(self._M_cam.cam_info.P, cam_frame_pts)
return pixel_pts.T
def compute_expected_J(self, target_pts):
"""
The output Jacobian J shows how moving target_pts in cartesian space affects
the expected measurement in (u,v) camera coordinates.
For n points in target_pts, J is a 2nx3n matrix
Note: This doesn't seem to be used anywhere, except maybe in some drawing code
"""
epsilon = 1e-8
N = len(self._M_cam.image_points)
Jt = zeros([N * 3, N * 2])
for k in range(N):
# Compute jacobian for point k
sub_Jt = zeros([3, 2])
x = target_pts[:, k].copy()
f0 = self.compute_expected(x)
for i in [0, 1, 2]:
x[i, 0] += epsilon
fTest = self.compute_expected(x)
sub_Jt[i, :] = array((fTest - f0) / epsilon)
x[i, 0] -= epsilon
Jt[k * 3:(k + 1) * 3, k * 2:(k + 1) * 2] = sub_Jt
return Jt.T
def compute_cov(self, target_pts):
'''
Computes the measurement covariance in pixel coordinates for the given
set of target points (target_pts)
Input:
- target_pts: 4xN matrix, storing N feature points of the target, in homogeneous coords
'''
epsilon = 1e-8
if self._M_chain is not None:
num_joints = len(self._M_chain.chain_state.position)
Jt = zeros([num_joints, self.get_residual_length()])
x = JointState()
x.position = self._M_chain.chain_state.position[:]
# Compute the Jacobian from the chain's joint angles to pixel residuals
f0 = reshape(array(self._compute_expected(x, target_pts)), [-1])
for i in range(num_joints):
x.position = [
cur_pos for cur_pos in self._M_chain.chain_state.position
]
x.position[i] += epsilon
fTest = reshape(array(self._compute_expected(x, target_pts)),
[-1])
Jt[i] = (fTest - f0) / epsilon
cov_angles = [
x * x for x in
self._chain.calc_block._chain._cov_dict['joint_angles']
]
# Transform the chain's covariance from joint angle space into pixel space using the just calculated jacobian
chain_cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
cam_cov = matrix(
zeros([self.get_residual_length(),
self.get_residual_length()]))
# Convert StdDev into variance
var_u = self._camera._cov_dict['u'] * self._camera._cov_dict['u']
var_v = self._camera._cov_dict['v'] * self._camera._cov_dict['v']
for k in range(cam_cov.shape[0] / 2):
cam_cov[2 * k, 2 * k] = var_u
cam_cov[2 * k + 1, 2 * k + 1] = var_v
# Both chain and camera covariances are now in measurement space, so we can simply add them together
if self._M_chain is not None:
cov = chain_cov + cam_cov
else:
cov = cam_cov
return cov
def build_sparsity_dict(self):
"""
Build a dictionary that defines which parameters will in fact affect this measurement.
Returned dictionary should be of the following form:
transforms:
my_transformA: [1, 1, 1, 1, 1, 1]
my_transformB: [1, 1, 1, 1, 1, 1]
dh_chains:
my_chain:
dh:
- [1, 1, 1, 1]
- [1, 1, 1, 1]
|
- [1, 1, 1, 1]
gearing: [1, 1, ---, 1]
rectified_cams:
my_cam:
baseline_shift: 1
f_shift: 1
cx_shift: 1
cy_shift: 1
"""
sparsity = dict()
sparsity['transforms'] = {}
for cur_transform_name in (self._config_dict['chain']['before_chain'] +
self._config_dict['chain']['after_chain']):
sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
sparsity['dh_chains'] = {}
if self._M_chain is not None:
chain_id = self._config_dict['chain']['chain_id']
num_links = self._chain.calc_block._chain._M
assert (num_links == len(self._M_chain.chain_state.position))
sparsity['dh_chains'][chain_id] = {}
sparsity['dh_chains'][chain_id]['dh'] = [[1, 1, 1, 1]] * num_links
sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
sparsity['rectified_cams'] = {}
sparsity['rectified_cams'][self.sensor_id] = dict([
(x, 1) for x in self._camera.get_param_names()
])
return sparsity
class CameraChainSensor():
def __init__(self, config_dict, M_cam, M_chain, config):
"""
Generates a single sensor block for a single configuration
Inputs:
- config_dict: The configuration for this specific sensor. This looks exactly like
a single element from the valid_configs list passed into CameraChainBundler.__init__
- M_cam: The camera measurement of type calibration_msgs/CameraMeasurement
- M_chain: The chain measurement of type calibration_msgs/ChainMeasurement
"""
self.sensor_type = "camera"
self.sensor_id = config_dict["camera_id"]
self._config_dict = config_dict
self._config = config
self._M_cam = M_cam
self._M_chain = M_chain
self._chain = FullChainRobotParams(config_dict["chain"], self._config)
self.camera_info_name = None
self.info_used = self.sensor_id in ['left', 'right', 'kinect_rgb']
path = rospy.get_param(
'/calibration_config/camera_parameter') + self.sensor_id + '.yaml'
with open(path) as f:
self._yaml = yaml.load(f)
self._distortion = self._yaml['distortion_coefficients']['data']
self._camera_matrix = self._yaml['camera_matrix']['data']
self.terms_per_sample = 2
def update_config(self, robot_params):
"""
On each optimization cycle the set of system parameters that we're optimizing over will change. Thus,
after each change in parameters we must call update_config to ensure that our calculated residual reflects
the newest set of system parameters.
"""
self._camera = robot_params.rectified_cams[
self._config_dict["camera_id"]]
if self._chain is not None:
self._chain.update_config(robot_params)
def compute_residual(self, target_pts):
"""
Computes the measurement residual for the current set of system parameters and target points
Input:
- target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
Output:
- r: 2N long vector, storing pixel residuals for the target points in the form [u1, v1, u2, v2, ..., uN, vN]
"""
z_mat = self.get_measurement()[:, 0, :]
h_mat = self.compute_expected(target_pts)
#code.interact(local=locals())
assert (z_mat.shape[1] == 2)
assert (h_mat.shape[1] == 2)
assert (z_mat.shape[0] == z_mat.shape[0])
r = array(reshape(h_mat - z_mat, [-1, 1]))[:, 0]
return r
def compute_residual_scaled(self, target_pts):
"""
Computes the residual, and then scales it by sqrt(Gamma), where Gamma
is the information matrix for this measurement (Cov^-1).
"""
r = self.compute_residual(target_pts)
gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
r_scaled = gamma_sqrt * matrix(r).T
return array(r_scaled.T)[0]
def compute_marginal_gamma_sqrt(self, target_pts):
"""
Calculates the square root of the information matrix for the measurement of the
current set of system parameters at the passed in set of target points.
"""
import scipy.linalg
cov = self.compute_cov(target_pts)
gamma = matrix(zeros(cov.shape))
num_pts = self.get_residual_length() / 2
for k in range(num_pts):
#print "k=%u" % k
first = 2 * k
last = 2 * k + 2
sub_cov = matrix(cov[first:last, first:last])
sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
sub_gamma_sqrt = real(sub_gamma_sqrt_full)
assert (scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) <
1e-6)
gamma[first:last, first:last] = sub_gamma_sqrt
return gamma
def get_residual_length(self):
N = len(self._M_cam.image_points)
return N * 2
# Get the observed measurement in a Nx2 Matrix
def get_measurement(self):
"""
Get the target's pixel coordinates as measured by the actual sensor
"""
cm = cv.CreateMat(3, 3, cv.CV_32F)
cv.Set(cm, 0)
cm_t = reshape(matrix(self._camera_matrix, float64), (3, 3))[:3, :3]
for x in range(3):
for y in range(3):
cm[x, y] = cm_t[x, y]
dc = cv.CreateMat(5, 1, cv.CV_32F)
for i in range(len(self._distortion)):
dc[i, 0] = self._distortion[i]
camera_pix = cv.fromarray(
float64([[[pt.x, pt.y]] for pt in self._M_cam.image_points]))
dst = cv.CreateMat(camera_pix.rows, camera_pix.cols, camera_pix.type)
cv.UndistortPoints(camera_pix, dst, cm, dc, P=cm)
return asarray(dst)
def compute_expected(self, target_pts):
"""
Compute the expected pixel coordinates for a set of target points.
target_pts: 4xN matrix, storing feature points of the target, in homogeneous coords
Returns: target points projected into pixel coordinates, in a Nx2 matrix
"""
if self._M_chain is not None:
return self._compute_expected(target_pts)
else:
return self._compute_expected(target_pts, None)
def _compute_expected(self, target_pts, M_chain=None):
"""
Compute what measurement we would expect to see, based on the current system parameters
and the specified target point locations.
Inputs:
- chain_state: The joint angles of this sensor's chain of type sensor_msgs/JointState.
- target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
Returns: target points projected into pixel coordinates, in a Nx2 matrix
"""
M_chain = self._M_chain if M_chain is None else M_chain
# Camera pose in root frame
camera_pose_root = self._chain.calc_block.fk(M_chain)
cam_frame_pts = camera_pose_root.I * target_pts
# Do the camera projection
pixel_pts = self._camera.project(self._camera_matrix, cam_frame_pts)
#code.interact(local=locals())
return pixel_pts.T
def compute_expected_J(self, target_pts):
"""
The output Jacobian J shows how moving target_pts in cartesian space affects
the expected measurement in (u,v) camera coordinates.
For n points in target_pts, J is a 2nx3n matrix
Note: This doesn't seem to be used anywhere, except maybe in some drawing code
"""
epsilon = 1e-8
N = len(self._M_cam.image_points)
Jt = zeros([N * 3, N * 2])
for k in range(N):
# Compute jacobian for point k
sub_Jt = zeros([3, 2])
x = target_pts[:, k].copy()
f0 = self.compute_expected(x)
for i in [0, 1, 2]:
x[i, 0] += epsilon
fTest = self.compute_expected(x)
sub_Jt[i, :] = array((fTest - f0) / epsilon)
x[i, 0] -= epsilon
Jt[k * 3:(k + 1) * 3, k * 2:(k + 1) * 2] = sub_Jt
return Jt.T
def compute_cov(self, target_pts):
'''
Computes the measurement covariance in pixel coordinates for the given
set of target points (target_pts)
Input:
- target_pts: 4xN matrix, storing N feature points of the target, in homogeneous coords
'''
#chain cov
chain_cov = self.compute_chain_cov(target_pts)
cam_cov = matrix(
zeros([self.get_residual_length(),
self.get_residual_length()]))
# Convert StdDev into variance
var_u = self._camera._cov_dict['u'] * self._camera._cov_dict['u']
var_v = self._camera._cov_dict['v'] * self._camera._cov_dict['v']
for k in range(cam_cov.shape[0] / 2):
cam_cov[2 * k, 2 * k] = var_u
cam_cov[2 * k + 1, 2 * k + 1] = var_v
# Both chain and camera covariances are now in measurement space, so we can simply add them together
if self._M_chain is not None:
cov = chain_cov + cam_cov
else:
cov = cam_cov
return cov
def compute_chain_cov(self, target_pts):
_ones = ones([7, self.get_residual_length()])
cov = matrix(diag([0.01] * self.get_residual_length()))
epsilon = 1e-8
chain_cov = None
if self._M_chain is not None:
i_joints = []
num_joints = 0
for i, c in enumerate(self._M_chain):
l = len(c.actual.positions)
i_joints.extend([i] * l)
num_joints += l
#num_joints = sum(num_joints)
#num_joints = len(self._M_chain.actual.position)
Jt = zeros([num_joints, self.get_residual_length()])
x = deepcopy(self._M_chain)
# Compute the Jacobian from the chain's joint angles to pixel residuals
f0 = reshape(array(self._compute_expected(target_pts, x)), [-1])
for i in range(num_joints):
x[i_joints[i]].header.frame_id = self._M_chain[
i_joints[i]].header.frame_id
x[i_joints[i]].actual.positions = list(
self._M_chain[i_joints[i]].actual.positions[:])
x[i_joints[i]].actual.positions[i] += epsilon
fTest = reshape(array(self._compute_expected(target_pts, x)),
[-1])
Jt[i] = (fTest - f0) / epsilon
cov_angles = []
for c in self._chain.calc_block._chains:
cov_angles.extend([x * x for x in c._chain._cov_dict])
# Transform the chain's covariance from joint angle space into pixel space using the just calculated jacobian
chain_cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
return chain_cov
def build_sparsity_dict(self):
"""
Build a dictionary that defines which parameters will in fact affect this measurement.
Returned dictionary should be of the following form:
transforms:
my_transformA: [1, 1, 1, 1, 1, 1]
my_transformB: [1, 1, 1, 1, 1, 1]
dh_chains:
my_chain:
dh:
- [1, 1, 1, 1]
- [1, 1, 1, 1]
|
- [1, 1, 1, 1]
gearing: [1, 1, ---, 1]
rectified_cams:
my_cam:
baseline_shift: 1
f_shift: 1
cx_shift: 1
cy_shift: 1
"""
sparsity = dict()
sparsity['transforms'] = {}
chain_transform_names = [
chain['before_chain'] + chain['after_chain']
for chain in self._config['chains']
if chain['chain_id'] in self._config_dict['chain']['chains']
][0]
for cur_transform_name in (self._config_dict['chain']['before_chain'] +
self._config_dict['chain']['after_chain'] +
chain_transform_names):
sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
sparsity['dh_chains'] = {}
chain_ids = self._config_dict["chain"]['chains']
for chain in self._chain.calc_block._chains:
chain_id = chain._config_dict["chain_id"]
num_links = chain._chain._M
#num_links = self._full_chain.calc_block._chains._M
#assert(num_links == len(self._M_chain.chain_state.position))
sparsity['dh_chains'][chain_id] = {}
sparsity['dh_chains'][chain_id]['dh'] = [[1, 1, 1, 1, 1, 1]
] * num_links
sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
sparsity['rectified_cams'] = {}
sparsity['rectified_cams'][self.sensor_id] = dict([
(x, 1) for x in self._camera.get_param_names()
])
return sparsity
class ChainSensor:
def __init__(self, config_dict, M_chain, target_id):
self.sensor_type = "chain"
self.sensor_id = config_dict["chain_id"]
self._config_dict = config_dict
self._M_chain = M_chain
self._target_id = target_id
self._full_chain = FullChainRobotParams(self._config_dict)
self.terms_per_sample = 3
def update_config(self, robot_params):
self._full_chain.update_config(robot_params)
self._checkerboard = robot_params.checkerboards[ self._target_id ]
def compute_residual(self, target_pts):
h_mat = self.compute_expected(target_pts)
z_mat = self.get_measurement()
assert(h_mat.shape == z_mat.shape)
assert(h_mat.shape[0] == 4)
r_mat = h_mat[0:3,:] - z_mat[0:3,:]
r = array(reshape(r_mat.T, [-1,1]))[:,0]
return r
def compute_residual_scaled(self, target_pts):
r = self.compute_residual(target_pts)
gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
r_scaled = gamma_sqrt * matrix(r).T
return array(r_scaled.T)[0]
def compute_marginal_gamma_sqrt(self, target_pts):
import scipy.linalg
# ----- Populate Here -----
cov = self.compute_cov(target_pts)
gamma = matrix(zeros(cov.shape))
num_pts = self.get_residual_length()/3
for k in range(num_pts):
#print "k=%u" % k
first = 3*k
last = 3*k+3
sub_cov = matrix(cov[first:last, first:last])
sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
sub_gamma_sqrt = real(sub_gamma_sqrt_full)
assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
gamma[first:last, first:last] = sub_gamma_sqrt
return gamma
def compute_cov(self, target_pts):
epsilon = 1e-8
num_joints = len(self._M_chain.chain_state.position)
Jt = zeros([num_joints, self.get_residual_length()])
x = JointState()
x.position = self._M_chain.chain_state.position[:]
f0 = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
for i in range(num_joints):
x.position = list(self._M_chain.chain_state.position[:])
x.position[i] += epsilon
fTest = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
Jt[i] = (fTest - f0)/epsilon
cov_angles = [x*x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']]
#import code; code.interact(local=locals())
cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
return cov
def get_residual_length(self):
pts = self._checkerboard.generate_points()
N = pts.shape[1]
return N*3
def get_measurement(self):
'''
Returns a 4xN matrix with the locations of the checkerboard points in homogenous coords,
as per the forward kinematics of the chain
'''
return self._calc_fk_target_pts(self._M_chain.chain_state)
def _calc_fk_target_pts(self, chain_state):
# Get the target's model points in the frame of the tip of the target chain
target_pts_tip = self._checkerboard.generate_points()
# Target pose in root frame
target_pose_root = self._full_chain.calc_block.fk(chain_state)
# Transform points into the root frame
target_pts_root = target_pose_root * target_pts_tip
return target_pts_root
def compute_expected(self, target_pts):
return target_pts
# Build a dictionary that defines which parameters will in fact affect this measurement
def build_sparsity_dict(self):
sparsity = dict()
sparsity['transforms'] = {}
for cur_transform_name in ( self._config_dict['before_chain'] + self._config_dict['after_chain'] ):
sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
sparsity['dh_chains'] = {}
chain_id = self._config_dict['chain_id']
num_links = self._full_chain.calc_block._chain._M
assert(num_links == len(self._M_chain.chain_state.position))
sparsity['dh_chains'][chain_id] = {}
sparsity['dh_chains'][chain_id]['dh'] = [ [1,1,1,1] ] * num_links
sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
sparsity['checkerboards'] = {}
sparsity['checkerboards'][self._target_id] = { 'spacing_x': 1,
'spacing_y': 1 }
return sparsity
class ChainSensor:
def __init__(self, config_dict, M_chain, target_id):
self.sensor_type = "chain"
self.sensor_id = config_dict["chain_id"]
self._config_dict = config_dict
self._M_chain = M_chain
self._target_id = target_id
self._full_chain = FullChainRobotParams(self._config_dict)
self.terms_per_sample = 3
def update_config(self, robot_params):
self._full_chain.update_config(robot_params)
self._checkerboard = robot_params.checkerboards[self._target_id]
def compute_residual(self, target_pts):
h_mat = self.compute_expected(target_pts)
z_mat = self.get_measurement()
assert (h_mat.shape == z_mat.shape)
assert (h_mat.shape[0] == 4)
r_mat = h_mat[0:3, :] - z_mat[0:3, :]
r = array(reshape(r_mat.T, [-1, 1]))[:, 0]
return r
def compute_residual_scaled(self, target_pts):
r = self.compute_residual(target_pts)
gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
r_scaled = gamma_sqrt * matrix(r).T
return array(r_scaled.T)[0]
def compute_marginal_gamma_sqrt(self, target_pts):
import scipy.linalg
# ----- Populate Here -----
cov = self.compute_cov(target_pts)
gamma = matrix(zeros(cov.shape))
num_pts = self.get_residual_length() / 3
for k in range(num_pts):
#print "k=%u" % k
first = 3 * k
last = 3 * k + 3
sub_cov = matrix(cov[first:last, first:last])
sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
sub_gamma_sqrt = real(sub_gamma_sqrt_full)
assert (scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) <
1e-6)
gamma[first:last, first:last] = sub_gamma_sqrt
return gamma
def compute_cov(self, target_pts):
epsilon = 1e-8
num_joints = len(self._M_chain.chain_state.position)
Jt = zeros([num_joints, self.get_residual_length()])
x = JointState()
x.position = self._M_chain.chain_state.position[:]
f0 = reshape(array(self._calc_fk_target_pts(x)[0:3, :].T), [-1])
for i in range(num_joints):
x.position = list(self._M_chain.chain_state.position[:])
x.position[i] += epsilon
fTest = reshape(array(self._calc_fk_target_pts(x)[0:3, :].T), [-1])
Jt[i] = (fTest - f0) / epsilon
cov_angles = [
x * x for x in
self._full_chain.calc_block._chain._cov_dict['joint_angles']
]
#import code; code.interact(local=locals())
cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
return cov
def get_residual_length(self):
pts = self._checkerboard.generate_points()
N = pts.shape[1]
return N * 3
def get_measurement(self):
'''
Returns a 4xN matrix with the locations of the checkerboard points in homogenous coords,
as per the forward kinematics of the chain
'''
return self._calc_fk_target_pts(self._M_chain.chain_state)
def _calc_fk_target_pts(self, chain_state):
# Get the target's model points in the frame of the tip of the target chain
target_pts_tip = self._checkerboard.generate_points()
# Target pose in root frame
target_pose_root = self._full_chain.calc_block.fk(chain_state)
# Transform points into the root frame
target_pts_root = target_pose_root * target_pts_tip
return target_pts_root
def compute_expected(self, target_pts):
return target_pts
# Build a dictionary that defines which parameters will in fact affect this measurement
def build_sparsity_dict(self):
sparsity = dict()
sparsity['transforms'] = {}
for cur_transform_name in (self._config_dict['before_chain'] +
self._config_dict['after_chain']):
sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
sparsity['dh_chains'] = {}
chain_id = self._config_dict['chain_id']
num_links = self._full_chain.calc_block._chain._M
assert (num_links == len(self._M_chain.chain_state.position))
sparsity['dh_chains'][chain_id] = {}
sparsity['dh_chains'][chain_id]['dh'] = [[1, 1, 1, 1]] * num_links
sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
sparsity['checkerboards'] = {}
sparsity['checkerboards'][self._target_id] = {
'spacing_x': 1,
'spacing_y': 1
}
return sparsity