test_msckf.py 文件源码

def test_augment_state(self):
        self.msckf.augment_state()

        N = self.msckf.N()
        self.assertTrue(self.msckf.P_cam is not None)
        self.assertTrue(self.msckf.P_imu_cam is not None)
        self.assertEqual(self.msckf.P_cam.shape, (N * 6, N * 6))
        self.assertEqual(self.msckf.P_imu_cam.shape, (15, N * 6))
        self.assertEqual(self.msckf.N(), 2)

        self.assertTrue(np.array_equal(self.msckf.cam_states[0].q_CG,
                                       self.msckf.ext_q_CI))
        self.assertEqual(self.msckf.counter_frame_id, 2)

        # Plot matrix
        # debug = True
        debug = False
        if debug:
            ax = plt.subplot(111)
            ax.matshow(self.msckf.P())
            plt.show()