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