Calculate camera world position with OpenCV Python

Question

I want to calculate my camera's position in world coordinates. This should be fairly easy, but I don't get the results I expect. I believe I've read everything on this topic, but my code isn't working. Here's what I do:

I have a camera looking at an area.

1) I drew a map of the area.

2) I calculated the homography by matching 4 image points to 4 points on my map using cv2.getPerspectiveTransform

3) The H homography transforms every world coordinate to camera coordinate; this is working properly

4) To calculate the camera matrix I followed this:

translation = np.zeros((3,1)) 
translation[:,0] = homography[:,2]

rotation = np.zeros((3,3))
rotation[:,0] = homography[:,0]
rotation[:,1] = homography[:,1]
rotation[:,2] = np.cross(homography[0:3,0],homography[0:3,1])

cameraMatrix = np.zeros((3,4))
cameraMatrix[:,0:3] = rotation
cameraMatrix[:,3] = homography[:,2]

cameraMatrix = cameraMatrix/cameraMatrix[2][3] #normalize the matrix

5) According to this, the camera's position should be calculated like this:

x,y,z = np.dot(-np.transpose(rotation),translation)

The coordinates I'm getting are totally wrong. The problem should be somewhere in step 4 or 5 I guess. What's wrong with my method?

Jig

do you know the world coordinates of those 4 points? – Lutestring

yes, I know the world coordinates – Jig

if you are already using opencv can you just use solvePnP instead? it would give you your world position directly... – Lutestring

I've just tried the solvePnP, it returns a 3-element rotation and a 3-element translation vector, but I'm not sure what to do with them. – Jig

those represent the world position of your camera, isn't that what you want? Are you having trouble turning them into a 3x4 matrix? – Lutestring

Yes I want that, but they're not even close to what I'm expecting. – Jig

I would recommend asking a question about your problem, solvePnP is much better suited to finding your world position than getPerspectiveTransform – Lutestring

Answer 1

I think I've got it now. The problem was with the method described in step 4. The camera position cannot be calculated from the homography matrix alone. The camera intrinsics matrix is also necessary. So, the correct procedure is the following:

1) draw a map of the area

2) calibrate the camera using the chessboard image with cv2.findChessboardCorners this yields the camera matrix and the distortion coefficients

3) solvePnP with the world coordinates (3D) and image coordinates (2D). The solvePnP returns the object's origo in the camera's coordinate system given the 4 corresponding points and the camera matrix.

4) Now I need to calculate the camera's position in world coordinates. The rotation matrix is: rotM = cv2.Rodrigues(rvec)[0]

5) The x,y,z position of the camera is: cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)

Jig

I have the same situation as you, but I'm a little stuck on 5). Struggling to find the finer details of numpy to convert to c/++... is your final line akin to... TransMatrix = inverse( rotM.Transpose() ) .PostMult ( TransMatrix ) or is the - something other than inverse? – Lipkin

this is the position in the world space right? for use cameraPosition and rotM as a view matrix in opengl I have to take the inverse, right? – Clifford

@Jig What exactly is the "world coordinates (3D)" that we have to pass to the solvePnP function? Is it known? How can I find that? – Colporteur

@Jig what is the size of rotM in order to calculate cameraPosition? In rotM you assign the value cv2.Rodrigues(rvec)[0] which refers to the first row of the Rodrigues produced 3x3 rotation matrix (so it has 1x3 size)? I stuck on this and I am trying to calculate the rotation and position of the camera in world coordinates. I'm using OpenCV(C++) in combination with Unity3D (C#) – Minervamines