def compute_calibration(self):
# check sample counts
if len(self.samples) < HandeyeCalibrator.MIN_SAMPLES:
rospy.logwarn(
"{} more samples needed! Not computing the calibration".format(
HandeyeCalibrator.MIN_SAMPLES - len(self.samples)))
return
hand_world_samples, camera_marker_samples = self.get_visp_samples()
if len(hand_world_samples.transforms) != len(
camera_marker_samples.transforms):
rospy.logerr(
"Different numbers of hand-world and camera-marker samples!")
raise AssertionError
# run calibration
rospy.loginfo("Computing from %g poses..." % len(self.samples))
try:
rospy.loginfo("Eye on hand is: {}".format(self.eye_on_hand))
result = self.calibrate(camera_marker_samples, hand_world_samples)
rospy.loginfo("Computed calibration: {}".format(str(result)))
transl = result.effector_camera.translation
rot = result.effector_camera.rotation
result_tuple = ((transl.x, transl.y, transl.z), (rot.x, rot.y,
rot.z, rot.w))
ret = HandeyeCalibration(self.eye_on_hand, self.robot_base_frame,
self.robot_effector_frame,
self.tracking_base_frame, result_tuple)
return ret
except rospy.ServiceException as ex:
rospy.logerr("Calibration failed: " + str(ex))
return None
def compute_calibration(self, handeye_parameters, samples, algorithm=None):
"""
Computes the calibration through the OpenCV library and returns it.
:rtype: easy_handeye.handeye_calibration.HandeyeCalibration
"""
if algorithm is None:
algorithm = 'Tsai-Lenz'
loginfo(
'OpenCV backend calibrating with algorithm {}'.format(algorithm))
if len(samples) < HandeyeCalibrationBackendOpenCV.MIN_SAMPLES:
logwarn(
"{} more samples needed! Not computing the calibration".format(
HandeyeCalibrationBackendOpenCV.MIN_SAMPLES -
len(samples)))
return
# Update data
opencv_samples = HandeyeCalibrationBackendOpenCV._get_opencv_samples(
samples)
(hand_world_rot, hand_world_tr), (marker_camera_rot,
marker_camera_tr) = opencv_samples
if len(hand_world_rot) != len(marker_camera_rot):
logerr(
"Different numbers of hand-world and camera-marker samples!")
raise AssertionError
loginfo("Computing from %g poses..." % len(samples))
method = HandeyeCalibrationBackendOpenCV.AVAILABLE_ALGORITHMS[
algorithm]
hand_camera_rot, hand_camera_tr = cv2.calibrateHandEye(
hand_world_rot,
hand_world_tr,
marker_camera_rot,
marker_camera_tr,
method=method)
result = tfs.affines.compose(np.squeeze(hand_camera_tr),
hand_camera_rot, [1, 1, 1])
loginfo("Computed calibration: {}".format(str(result)))
(hcqw, hcqx, hcqy,
hcqz) = [float(i) for i in tfs.quaternions.mat2quat(hand_camera_rot)]
(hctx, hcty, hctz) = [float(i) for i in hand_camera_tr]
result_tuple = ((hctx, hcty, hctz), (hcqx, hcqy, hcqz, hcqw))
ret = HandeyeCalibration(calibration_parameters=handeye_parameters,
transformation=result_tuple)
return ret
def compute_calibration(self, handeye_parameters, samples):
"""
Computes the calibration through the ViSP service and returns it.
:rtype: easy_handeye.handeye_calibration.HandeyeCalibration
"""
if len(samples) < HandeyeCalibrationServiceVisp.MIN_SAMPLES:
logwarn("{} more samples needed! Not computing the calibration".format(
HandeyeCalibrationServiceVisp.MIN_SAMPLES - len(samples)))
return
# Update data
hand_world_samples, camera_marker_samples = HandeyeCalibrationServiceVisp.get_visp_samples(samples=samples,
handeye_parameters=handeye_parameters)
if len(hand_world_samples.transforms) != len(camera_marker_samples.transforms):
logerr("Different numbers of hand-world and camera-marker samples!")
raise AssertionError
loginfo("Computing from %g poses..." % len(samples))
try:
result = self.calibrate(camera_marker_samples, hand_world_samples)
loginfo("Computed calibration: {}".format(str(result)))
transl = result.effector_camera.translation
rot = result.effector_camera.rotation
result_tuple = ((transl.x, transl.y, transl.z),
(rot.x, rot.y, rot.z, rot.w))
ret = HandeyeCalibration(calibration_parameters=handeye_parameters,
transformation=result_tuple)
return ret
except ServiceException as ex:
logerr("Calibration failed: " + str(ex))
return None
#!/usr/bin/env python2.7
import rospy
import tf2_ros
import geometry_msgs.msg
from easy_handeye.handeye_calibration import HandeyeCalibration
rospy.init_node('handeye_calibration_publisher')
while rospy.get_time() == 0.0:
pass
inverse = rospy.get_param('inverse')
calib = HandeyeCalibration.from_file(rospy.get_namespace())
if calib.parameters.eye_on_hand:
overriding_robot_effector_frame = rospy.get_param('robot_effector_frame')
if overriding_robot_effector_frame != "":
calib.transformation.header.frame_id = overriding_robot_effector_frame
else:
overriding_robot_base_frame = rospy.get_param('robot_base_frame')
if overriding_robot_base_frame != "":
calib.transformation.header.frame_id = overriding_robot_base_frame
overriding_tracking_base_frame = rospy.get_param('tracking_base_frame')
if overriding_tracking_base_frame != "":
calib.transformation.child_frame_id = overriding_tracking_base_frame
rospy.loginfo('loading calibration parameters into namespace {}'.format(
rospy.get_namespace()))
HandeyeCalibration.store_to_parameter_server(calib)
#!/usr/bin/env python2
import rospy
from tf import TransformBroadcaster, TransformerROS, transformations as tfs
from geometry_msgs.msg import Transform
from easy_handeye.handeye_calibration import HandeyeCalibration
rospy.init_node('handeye_calibration_publisher')
while rospy.get_time() == 0.0:
pass
inverse = rospy.get_param('inverse')
calib = HandeyeCalibration()
calib.from_file()
if calib.eye_on_hand:
overriding_robot_effector_frame = rospy.get_param('robot_effector_frame')
if overriding_robot_effector_frame != "":
calib.transformation.header.frame_id = overriding_robot_effector_frame
else:
overriding_robot_base_frame = rospy.get_param('robot_base_frame')
if overriding_robot_base_frame != "":
calib.transformation.header.frame_id = overriding_robot_base_frame
overriding_tracking_base_frame = rospy.get_param('tracking_base_frame')
if overriding_tracking_base_frame != "":
calib.transformation.child_frame_id = overriding_tracking_base_frame
rospy.loginfo('loading calibration parameters into namespace {}'.format(
rospy.get_namespace()))
calib.to_parameters()
def save_calibration(self, _):
if self.last_calibration:
HandeyeCalibration.to_file(self.last_calibration)
rospy.loginfo('Calibration saved to {}'.format(
self.last_calibration.filename()))
return std_srvs.srv.EmptyResponse()
