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