def test_cov(self):
config, robot_params, P = self.load()
camera_points = [ImagePoint(0, 0), ImagePoint(1, 0)]
sensor = CameraChainSensor(
config,
CameraMeasurement(camera_id="camA",
cam_info=CameraInfo(P=P),
image_points=camera_points),
ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0])))
sensor.update_config(robot_params)
target = matrix([[1, 2], [0, 0], [1, 1], [1, 1]])
cov = sensor.compute_cov(target)
print "\ncov:\n", cov
self.assertAlmostEqual(cov[0, 0], 1.0, 6)
self.assertAlmostEqual(cov[1, 0], 0.0, 6)
self.assertAlmostEqual(cov[2, 0], 0.0, 6)
self.assertAlmostEqual(cov[3, 0], 0.0, 6)
self.assertAlmostEqual(cov[1, 1], 2.0, 6)
self.assertAlmostEqual(cov[3, 3], 5.0, 6)
def test_update(self):
config, robot_params, P = self.load()
camera_points = [
ImagePoint(0, 0),
ImagePoint(1, 0),
ImagePoint(0, 1),
ImagePoint(1, 1)
]
block = CameraChainSensor(
config,
CameraMeasurement(camera_id="camA",
cam_info=CameraInfo(P=P),
image_points=camera_points),
ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0])))
block.update_config(robot_params)
target = matrix([[1, 2, 1, 2], [0, 0, 1, 1], [1, 1, 1, 1],
[1, 1, 1, 1]])
h = block.compute_expected(target)
z = block.get_measurement()
r = block.compute_residual(target)
self.assertAlmostEqual(
linalg.norm(h - matrix([[0, 1, 0, 1], [0, 0, 1, 1]]).T), 0.0, 6)
self.assertAlmostEqual(
linalg.norm(z - matrix([[0, 1, 0, 1], [0, 0, 1, 1]]).T), 0.0, 6)
self.assertAlmostEqual(linalg.norm(r), 0.0, 6)
# Test Sparsity
sparsity = block.build_sparsity_dict()
self.assertEqual(sparsity['transforms']['transformA'],
[1, 1, 1, 1, 1, 1])
self.assertEqual(sparsity['transforms']['transformB'],
[1, 1, 1, 1, 1, 1])
self.assertEqual(sparsity['chains']['chainA']['transforms'],
[[1, 1, 1, 1, 1, 1]])
self.assertEqual(sparsity['rectified_cams']['camA']['baseline_shift'],
1)
