test_imu_state.py 文件源码

def test_J(self):
        # Setup
        cam_q_CI = np.array([0.0, 0.0, 0.0, 1.0])
        cam_p_IC = np.array([1.0, 1.0, 1.0])
        q_hat_IG = np.array([0.0, 0.0, 0.0, 1.0])
        N = 1
        J = self.imu_state.J(cam_q_CI, cam_p_IC, q_hat_IG, N)

        # Assert
        C_CI = C(cam_q_CI)
        C_IG = C(q_hat_IG)
        # -- First row --
        self.assertTrue(np_equal(J[0:3, 0:3], C_CI))
        # -- Second row --
        self.assertTrue(np_equal(J[3:6, 0:3], skew(dot(C_IG.T, cam_p_IC))))
        # -- Third row --
        self.assertTrue(np_equal(J[3:6, 12:15], I(3)))

        # Plot matrix
        if self.debug:
            ax = plt.subplot(111)
            ax.matshow(J)
            plt.show()