How to Convert a Rodrigues Vector to a Rotation Matrix without OpenCV using Python

user_920689 0
Viewed 1724

I'm looking for a code snippet equivalent to 

import cv2

def rodrigues_vec_to_rotation_mat(rodrigues_vec): 
     return cv2.Rodrigues(rodrigues_vec)[0]

without importing/using the OpenCV library.

opencv python
1 Answers
import math
import sys
import numpy as np

def rodrigues_vec_to_rotation_mat(rodrigues_vec):
    theta = np.linalg.norm(rodrigues_vec)
    if theta < sys.float_info.epsilon:              
        rotation_mat = np.eye(3, dtype=float)
    else:
        r = rodrigues_vec / theta
        I = np.eye(3, dtype=float)
        r_rT = np.array([
            [r[0]*r[0], r[0]*r[1], r[0]*r[2]],
            [r[1]*r[0], r[1]*r[1], r[1]*r[2]],
            [r[2]*r[0], r[2]*r[1], r[2]*r[2]]
        ])
        r_cross = np.array([
            [0, -r[2], r[1]],
            [r[2], 0, -r[0]],
            [-r[1], r[0], 0]
        ])
        rotation_mat = math.cos(theta) * I + (1 - math.cos(theta)) * r_rT + math.sin(theta) * r_cross
    return rotation_mat

Sources:
The documentation of the Rodrigues() function.
The source code of the cvRodrigues2() function.

user_9206890
Related