コード例 #1
0
ファイル: test_so2.py プロジェクト: tussedrotten/pylie
def test_construct_from_matrix():
    theta = np.pi / 3
    R = SO2(theta).to_matrix()
    so2 = SO2.from_matrix(R)
    np.testing.assert_equal(so2.angle, theta)
    np.testing.assert_array_equal(so2.to_matrix(), R)

    # Matrices not elements of SO(2) should be fitted to SO(2)
    R_noisy = R + 0.1 + np.random.rand(2)
    so2_fitted = SO2.from_matrix(R_noisy)
    R_fitted = so2_fitted.to_matrix()

    assert np.any(np.not_equal(R_fitted, R_noisy))
    np.testing.assert_almost_equal(np.linalg.det(R_fitted), 1, 14)
    np.testing.assert_almost_equal((R_fitted.T @ R_fitted), np.identity(2), 14)
コード例 #2
0
    def from_matrix(cls, T):
        """Construct an SE(2) element corresponding from a pose matrix.
        The rotation is fitted to the closest rotation matrix, the bottom row of the 3x3 matrix is ignored.

        :param T: 3x3 or 2x3 pose matrix.
        :return: The SE(2) element.
        """

        return cls((SO2.from_matrix(T[:2, :2]), T[:2, 2:3]))