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