test_msckf.py 文件源码

def test_H(self):
        # Setup feature track
        track_id = 0
        frame_id = 3
        data0 = KeyPoint(np.array([0.0, 0.0]), 21)
        data1 = KeyPoint(np.array([0.0, 0.0]), 21)
        track = FeatureTrack(track_id, frame_id, data0, data1)

        # Setup track cam states
        self.msckf.augment_state()
        self.msckf.augment_state()
        self.msckf.augment_state()
        self.msckf.augment_state()
        track_cam_states = self.msckf.track_cam_states(track)

        # Feature position
        p_G_f = np.array([[1.0], [2.0], [3.0]])

        # Test
        H_f_j, H_x_j = self.msckf.H(track, track_cam_states, p_G_f)

        # Assert
        self.assertEqual(H_f_j.shape, (4, 3))
        self.assertEqual(H_x_j.shape, (4, 45))

        # Plot matrix
        # debug = True
        debug = False
        if debug:
            ax = plt.subplot(211)
            ax.matshow(H_f_j)
            ax = plt.subplot(212)
            ax.matshow(H_x_j)
            plt.show()