Class RigidMotion

plib/rigid_motion.py:14–196

class RigidMotion:
    def __init__(self, R: np.ndarray = None, t: np.ndarray = None, H: np.ndarray = None):
        if H is not None:
            R = H[:3, :3]
            t = H[:3, 3]
        if R is None:
            R = np.eye(3)
        if t is None:
            t = np.zeros(3)

        self.R = copy.deepcopy(R)  # 3*3 rotation matrix
        self.t = copy.deepcopy(np.reshape(t, (3, 1)))  # 3*1 position

    def homogeneous_matrix(self):
        """returns the 4*4 homogeneous matrix."""
        H = np.zeros((4, 4))
        H[0:3, 0:3] = self.R
        H[0:3, 3:4] = self.t
        H[3, 3] = 1
        return H

    @staticmethod
    def invert_homogeneous_matrix(H: np.ndarray) -> np.ndarray:
        """returns the inverse of a homegeneous matrix."""
        H_rm = RigidMotion(H=H)
        return RigidMotion.inverse(H_rm).homogeneous_matrix()

    @staticmethod
    def inverse(H):
        """Returns H^-1 given RigidMotion H"""
        inv_R = (H.R).T
        inv_t = -1.0 * (inv_R @ H.t)
        return RigidMotion(R=inv_R, t=inv_t)

    @staticmethod
    def exp_skew_symmetric(S: np.ndarray, t: float = 1.0, theta: float = None) -> np.ndarray:
        """Returns exp(t*S) of a 3*3 skew symmetric matrix to get a rotation matrix."""

        if (S ** 2).sum() < 1e-8:
            # S is all zero
            return np.eye(3)

        if theta is None:
            s = np.array([S[2, 1], S[0, 2], S[1, 0]])
            theta = np.sqrt(np.sum(s ** 2.0))

        angle = t * theta
        theta_square = theta * theta

        R = np.eye(3) + np.sin(angle) / theta * S + (1 - np.cos(angle)) / theta_square * (S @ S)
        return R

    @staticmethod
    def log_rotation(R: np.ndarray):
        """Return the log(R) where R is a rotation matrix.  So it returns a skew symmetric matrix."""
        arg = 0.5 * (R[0, 0] + R[1, 1] + R[2, 2] - 1)  # in [-1, 1]
        if arg > -1:
            if arg < 1: