Camera pose by solvePnP

Question

Goal:

I need to get the position and orientation of the camera (using OpenCV / Python).

Definition:

Angles of relationship are determined by:

Yaw is the general orientation of the camera when it lies on a horizontal plane: north = 0, east = 90 °, south = 180 °, west = 270 °, etc.

Pitch - orientation of the "nose" of the camera: 0 ° = horizontal orientation at a point on the horizon, -90 ° = vertical observation, + 90 ° = vertical observation, 45 ° = watch at an angle of 45 ° from the horizon, etc.

Roll if the camera is tilted left or right when in your hands (so that it always looks at a point on the horizon when this angle changes): + 45 ° = 45 ° tilt clockwise when you capture the camera, thus + 90 ° (and -90 °) will be the angle needed for a portrait image, for example, etc.

World reference system:

My world coordinate system is oriented like this:

East = + X
North = + Y
Up to the sky = + Z

In this coordinate system points of my objects of the world are indicated.

Camera frame:

According to the document, the camera’s reference camera is oriented like this:

What to achieve:

Now, from cv2.solvepnp() over a bunch of image points and their corresponding world coordinates, I calculated both rvec and tvec . But according to the document: http://docs.opencv.org/trunk/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d , they are:

rvec ; A vector of vector rotation (see Rodrigues() ), which, together with tvec outputs points from the model coordinate system to the camera coordinate system.
tvec ; Output translation vector.

these vectors are shown go to the reference frame of the camera.
I need to do the exact reverse operation, thus getting the position and position of the camera relative to world coordinates.

Camera position:

So, I calculated the rotation matrix from rvec using Rodrigues() :

 rmat = cv2.Rodrigues(rvec)[0] 

And if I am here, then the position of the camera, expressed in the world coordinate system, is determined:

 camera_position = -np.matrix(rmat).T * np.matrix(tvec) 

(src: Camera position in world coordinate from cv :: solvePnP )
It looks pretty good.

Camera Ratio (Yaw, Pitch and Roll):

But how to get the appropriate position angles (yaw, pitch and roll, as described above) from the point of view of the camera (as if it were mainly in your hand)?

I tried to implement this: http://planning.cs.uiuc.edu/node102.html#eqn:yprmat in a function:

 def rotation_matrix_to_attitude_angles(R): import math import numpy as np cos_beta = math.sqrt(R[2,1] * R[2,1] + R[2,2] * R[2,2]) validity = cos_beta < 1e-6 if not validity: alpha = math.atan2(R[1,0], R[0,0]) # yaw [z] beta = math.atan2(-R[2,0], cos_beta) # pitch [y] gamma = math.atan2(R[2,1], R[2,2]) # roll [x] else: alpha = math.atan2(R[1,0], R[0,0]) # yaw [z] beta = math.atan2(-R[2,0], cos_beta) # pitch [y] gamma = 0 # roll [x] return np.array([alpha, beta, gamma]) 

But the results are not consistent with what I want. For example, I have a tilt angle of ~ -90 °, but the camera is horizontal, so it should be around 0.

The pitch angle is about 0, so it seems to have been correctly identified, but I don’t really understand why it is about 0, since the Z-axis of the camera’s reference frame is in a horizontal position, so that it was already tilted from 90 ° from the vertical axis of the world reference systems. I would expect a value of -90 ° or + 270 ° here. Anyway.

And yaw seems good. Mostly.

Am I missing something with a tilt angle?