CODEWITHSUNDEEP
pythonopencveuler-angles
fightstarr20
fightstarr20

Reputation: 12598

Python OpenCV solvePnP convert to euler angles

I am using solvePnP like this..

import cv2
import numpy as np

# Read Image
im = cv2.imread("headPose.jpg");
size = im.shape

#2D image points. If you change the image, you need to change vector
image_points = np.array([
                            (359, 391),     # Nose tip
                            (399, 561),     # Chin
                            (337, 297),     # Left eye left corner
                            (513, 301),     # Right eye right corne
                            (345, 465),     # Left Mouth corner
                            (453, 469)      # Right mouth corner
                        ], dtype="double")

# 3D model points.
model_points = np.array([
                            (0.0, 0.0, 0.0),             # Nose tip
                            (0.0, -330.0, -65.0),        # Chin
                            (-225.0, 170.0, -135.0),     # Left eye left corner
                            (225.0, 170.0, -135.0),      # Right eye right corne
                            (-150.0, -150.0, -125.0),    # Left Mouth corner
                            (150.0, -150.0, -125.0)      # Right mouth corner

                        ])


# Camera internals

focal_length = size[1]
center = (size[1]/2, size[0]/2)
camera_matrix = np.array(
                         [[focal_length, 0, center[0]],
                         [0, focal_length, center[1]],
                         [0, 0, 1]], dtype = "double"
                         )

print "Camera Matrix :\n {0}".format(camera_matrix)

dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.CV_ITERATIVE)

print "Rotation Vector:\n {0}".format(rotation_vector)
print "Translation Vector:\n {0}".format(translation_vector)

I am confused as to what the Rotation Vector and Translation Vector actually are? I think I need to convert these to Euler angles to give me 3 values, for pitch, roll and yaw.

Is this correct? Does anybody have an example of this?

Upvotes: 3

Views: 4214

Answers (1)

Zaw Lin
Zaw Lin

Reputation: 5708

rvecs is axis angle representation of rotation, which usually require 4 numbers, [v,theta], but v is required to be unit vector, and thus it's length is encoded as theta, reducing the required numbers to 3.

for the code, it should be something like this.

def pnp(objectPoints,imgPoints,w,h,f):
    cameraMatrix = np.array([[f,0,w/2.0],
                     [0,f,h/2.0],
                    [0,0,1]])
    distCoeffs = np.zeros((5,1))
    revtval,rvecs, tvecs  =cv2.solvePnP(objectPoints[:,np.newaxis,:], imgPoints[:,np.newaxis,:], cameraMatrix, distCoeffs)#,False,flags=cv2.SOLVEPNP_EPNP)
    return rvecs,tvecs


def rot_params_rv(rvecs):
    from math import pi,atan2,asin
    R = cv2.Rodrigues(rvecs)[0]
    roll = 180*atan2(-R[2][1], R[2][2])/pi
    pitch = 180*asin(R[2][0])/pi
    yaw = 180*atan2(-R[1][0], R[0][0])/pi
    rot_params= [roll,pitch,yaw]
    return rot_params

Upvotes: 8

Related Questions