def test_setup_moving(self):
calibrator = HandEyeCalibrator(setup=Setup.Moving)
for Q, Pinv in self.moving_setup_samples:
calibrator.assess_tcp_pose(Q)
calibrator.add_sample(Q, Pinv)
for solver_class in self.solver_classes:
print 'Testing fixed setup with solver: {0}'.format(solver_class)
Xest = calibrator.solve(method=solver_class)
# Given that we don't have any noise, Xest == self.Xm must be true
np.testing.assert_allclose(Xest, self.Xm)
# Reprojection error should be zero
rot_rmse, trans_rmse = calibrator.compute_reprojection_error(Xest)
np.testing.assert_allclose([rot_rmse, trans_rmse], [0, 0],
atol=1e-08)
self.assertEqual(calibrator.get_num_samples(), self.num_samples)
def test_setup(self):
# Setup specified with strings
HandEyeCalibrator('Fixed')
HandEyeCalibrator('FiXed')
HandEyeCalibrator('fixed')
HandEyeCalibrator('Moving')
HandEyeCalibrator('moving')
# Invalid setup string
try:
HandEyeCalibrator('invalid.setup.str')
raises = False
except KeyError:
raises = True
self.assertTrue(raises)
# Setup specified with int
HandEyeCalibrator(setup=1)
HandEyeCalibrator(setup=2)
# Invalid setup int
try:
HandEyeCalibrator(setup=666)
raises = False
except ValueError:
raises = True
self.assertTrue(raises)
def test_num_samples(self):
# Test invalid number
calibrator = HandEyeCalibrator(Setup.Fixed)
for num_samples in range(calibrator.min_samples_required):
calibrator.reset()
# Ground-truth values
X = tTo = br.transform.random(max_position=0.1)
bTc = br.transform.random(max_position=1.2)
# Samples without any noise
samples = self.fixed_setup_samples[:num_samples]
[calibrator.add_sample(Q, P) for Q, P in samples]
try:
calibrator.solve()
raises = False
except Exception:
raises = True
self.assertTrue(raises)
def test_noisy_synthetic_data(self):
calibrator = HandEyeCalibrator(setup=Setup.Fixed)
# Add noise to the samples
for Q, Pinv in self.fixed_setup_samples:
Qnoisy = add_relative_noise(Q, rot_noise=0.1e-2, trans_noise=1e-2)
Pinv_noisy = add_relative_noise(
Pinv, rot_noise=0.1e-2, trans_noise=1e-2)
calibrator.add_sample(Qnoisy, Pinv_noisy)
for solver_class in self.solver_classes:
print("Testing: {0}".format(solver_class))
Xest = calibrator.solve(method=solver_class)
# Given that we have noise, Xest != X
self.assertFalse(br.transform.are_equal(Xest, self.Xf))
# Reprojection error should be bigger than zero
rot_rmse, trans_rmse = calibrator.compute_reprojection_error(Xest)
self.assertTrue(rot_rmse > br._FLOAT_EPS)
self.assertTrue(trans_rmse > br._FLOAT_EPS)
def calibrate_simulation(cto_poses, bte_poses):
calibrator = HandEyeCalibrator(setup=Setup.Fixed)
for cto_pose, bte_pose in zip(cto_poses, bte_poses):
bte = tr.pose_to_transform(bte_pose)
cto = tr.pose_to_transform(cto_pose)
calibrator.assess_tcp_pose(bte)
calibrator.add_sample(bte, cto)
Xest = calibrator.solve(method=solver.Daniilidis1999)
Xpose = tr.pose_quaternion_from_matrix(Xest)
print("via Daniilidis1999:", Xpose)
rot_rmse, trans_rmse = calibrator.compute_reprojection_error(Xest)
assert (rot_rmse > br._FLOAT_EPS)
assert (trans_rmse > br._FLOAT_EPS)
rospy.init_node('ur3_force_control')
tfbr = tf.TransformBroadcaster()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
tfbr.sendTransform(Xpose[:3], Xpose[3:], rospy.Time.now(),
"camera_es_link", "base_link")
rate.sleep()
def calibration_cb(req):
response = CalibrateHandEyeResponse()
solver_class = get_class_from_str(req.solver)
# Check the given solver can be imported
if solver_class is None:
rospy.logwarn('Failed to find HandEye solver: {}'.format(req.solver))
return response
# Check the given solver inherits from handeye.solver.SolverBase
if handeye.solver.SolverBase not in inspect.getmro(solver_class):
rospy.logwarn('The HandEye solver {} does not inherit from' +
'handeye.solver.SolverBase'.format(req.solver))
return response
# Check the number of poses are consistent
num_object_poses = len(req.object_wrt_sensor.poses)
num_effector_poses = len(req.effector_wrt_world.poses)
if num_object_poses != num_effector_poses:
rospy.logwarn(
'Number of poses must be equal: ' +
'{0} != {1}'.format(num_effector_poses, num_object_poses))
return response
# Check a valid setup has been specified
try:
setup = handeye.calibrator.Setup[req.setup.capitalize()]
except KeyError:
rospy.logwarn('Invalid req.setup was specified: {}'.format(req.setup))
return response
# Run the HandEye calibration
try:
calibrator = HandEyeCalibrator(setup)
except NotImplementedError:
rospy.logwarn('Only Moving setup is supported')
return response
for i in range(num_object_poses):
Q = cu.conversions.from_pose(req.effector_wrt_world.poses[i])
P = cu.conversions.from_pose(req.object_wrt_sensor.poses[i])
calibrator.add_sample(Q, P)
Xhat = calibrator.solve(method=solver_class)
# Populate the response
response.success = True
rotation_rmse, translation_rmse = calibrator.compute_reprojection_error(
Xhat)
response.rotation_rmse = rotation_rmse
response.translation_rmse = translation_rmse
response.sensor_frame = cu.conversions.to_pose(Xhat)
return response