OpenCV Structure from Motion Reprojection Issue

edit

I am currently facing an issue with my Structure from Motion program based on OpenCv. I'm gonna try to depict you what it does, and what it is supposed to do.

This program lies on the classic "structure from motion" method.

The basic idea is to take a pair of images, detect their keypoints and compute the descriptors of those keypoints. Then, the keypoints matching is done, with a certain number of tests to insure the result is good. That part works perfectly.

Once this is done, the following computations are performed : fundamental matrix, essential matrix, SVD decomposition of the essential matrix, camera matrix computation and finally, triangulation.

The result for a pair of images is a set of 3D coordinates, giving us points to be drawn in a 3D viewer. This works perfectly, for a pair.

Indeed, here is my problem : for a pair of images, the 3D points coordinates are calculated in the coordinate system of the first image of the image pair, taken as the reference image. When working with more than two images, which is the objective of my program, I have to reproject the 3D points computed in the coordinate system of the very first image, in order to get a consistent result.

My question is : How do I reproject 3D points coordinate given in a camera related system, into an other camera related system ? With the camera matrices ?

My idea was to take the 3D point coordinates, and to multiply them by the inverse of each camera matrix before.

I clarify :

Suppose I am working on the third and fourth image (hence, the third pair of images, because I am working like 1-2 / 2-3 / 3-4 and so on).

I get my 3D point coordinates in the coordinate system of the third image, how do I do to reproject them properly in the very first image coordinate system ?

I would have done the following :

Get the 3D points coordinates matrix, apply the inverse of the camera matrix for image 2 to 3, and then apply the inverse of the camera matrix for image 1 to 2. Is that even correct ?

Because those camera matrices are non square matrices, and I can't inverse them.

I am surely mistaking somewhere, and I would be grateful if someone could enlighten me, I am pretty sure this is a relative easy one, but I am obviously missing something...

Thanks a lot for reading :)