def test_cov(self): print "" config, robot_params = loadSystem() joint_points = [ JointState(position=[0, 0, 1]), JointState(position=[pi / 2, 0, 2]) ] sensor = TiltingLaserSensor( config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA", joint_points=joint_points)) sensor.update_config(robot_params) cov = sensor.compute_cov(None) print "Cov:" print cov self.assertAlmostEqual(cov[0, 0], 1.0, 6) self.assertAlmostEqual(cov[1, 1], 1.0, 6) self.assertAlmostEqual(cov[2, 2], 1.0, 6) self.assertAlmostEqual(cov[3, 3], 4.0, 6) self.assertAlmostEqual(cov[4, 4], 4.0, 6) self.assertAlmostEqual(cov[5, 5], 1.0, 6)
def test_tilting_laser_1(self): print "" config, robot_params = loadSystem() joint_points = [ JointState(position=[0, 0, 0]), JointState(position=[0, pi / 2, 1]), JointState(position=[pi / 2, 0, 1]) ] sensor = TiltingLaserSensor( config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA", joint_points=joint_points)) sensor.update_config(robot_params) target_pts = matrix([[0, 0, 0], [0, 1, 0], [0, 0, -1], [1, 1, 1]]) h = sensor.compute_expected(target_pts) z = sensor.get_measurement() r = sensor.compute_residual(target_pts) self.assertAlmostEqual(numpy.linalg.norm(h - target_pts), 0.0, 6) self.assertAlmostEqual(numpy.linalg.norm(z - target_pts), 0.0, 6) self.assertAlmostEqual(numpy.linalg.norm(r), 0.0, 6) # Test Sparsity sparsity = sensor.build_sparsity_dict() self.assertEqual(sparsity['transforms']['j0'], [1, 1, 1, 1, 1, 1]) self.assertEqual(sparsity['transforms']['j1'], [1, 1, 1, 1, 1, 1]) self.assertEqual(sparsity['transforms']['j2'], [1, 1, 1, 1, 1, 1]) self.assertEqual(sparsity['tilting_lasers']['laserA']['gearing'], 1)
def test_gamma(self): print "" config, robot_params = loadSystem() joint_points = [ JointState(position=[0, 0, 1]), JointState(position=[pi / 2, 0, 2]) ] sensor = TiltingLaserSensor( config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA", joint_points=joint_points)) sensor.update_config(robot_params) gamma = sensor.compute_marginal_gamma_sqrt(None) print "Gamma:" print gamma self.assertAlmostEqual(gamma[0, 0], 1.0, 6) self.assertAlmostEqual(gamma[1, 1], 1.0, 6) self.assertAlmostEqual(gamma[2, 2], 1.0, 6) self.assertAlmostEqual(gamma[3, 3], 0.5, 6) self.assertAlmostEqual(gamma[4, 4], 0.5, 6) self.assertAlmostEqual(gamma[5, 5], 1.0, 6)