quaternion.py 文件源码

def _transform2quat(self):
        """Construct quaternion from the transform/rotation matrix 

        :returns: quaternion formed from transform matrix
        :rtype: numpy array
        """

        # Code was copied from perl PDL code that uses backwards index ordering
        T = self.transform.transpose()  
        den = np.array([ 1.0 + T[0, 0] - T[1, 1] - T[2, 2],
                              1.0 - T[0, 0] + T[1, 1] - T[2, 2],
                              1.0 - T[0, 0] - T[1, 1] + T[2, 2],
                              1.0 + T[0, 0] + T[1, 1] + T[2, 2]])

        max_idx = np.flatnonzero(den == max(den))[0]

        q = np.zeros(4)
        q[max_idx] = 0.5 * sqrt(max(den))
        denom = 4.0 * q[max_idx]
        if (max_idx == 0):
            q[1] = (T[1, 0] + T[0, 1]) / denom 
            q[2] = (T[2, 0] + T[0, 2]) / denom 
            q[3] = -(T[2, 1] - T[1, 2]) / denom 
        if (max_idx == 1):
            q[0] = (T[1, 0] + T[0, 1]) / denom 
            q[2] = (T[2, 1] + T[1, 2]) / denom 
            q[3] = -(T[0, 2] - T[2, 0]) / denom 
        if (max_idx == 2):
            q[0] = (T[2, 0] + T[0, 2]) / denom 
            q[1] = (T[2, 1] + T[1, 2]) / denom 
            q[3] = -(T[1, 0] - T[0, 1]) / denom 
        if (max_idx == 3):
            q[0] = -(T[2, 1] - T[1, 2]) / denom 
            q[1] = -(T[0, 2] - T[2, 0]) / denom 
            q[2] = -(T[1, 0] - T[0, 1]) / denom 

        return q