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)
Example #2
0
    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)
Example #3
0
    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['dh_chains']['chainA']['dh'], [[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['dh_chains']['chainA']['dh'], [[1,1,1,1]])
        self.assertEqual(sparsity['rectified_cams']['camA']['baseline_shift'], 1)