Project 3D pts from reconstructScene back to image

I have calibrated my cameras and made a 3D point cloud using reconstructScene(disparityMap, stereoParams). Now all I want to do is project those points back into my two images. You would think that I would just be able to do pts3D*stereoParams.CameraParameters1.IntrinsicMatrix (and then divide by the homogeneous coordinate) to project from 3D to camera 1, but when I plot the result, the projected points are shifted to the left of where they should land. I have tried projecting them onto both the unrectified and rectified image, but the points always are shifted from where they should land. I really just need the inverse function of reconstructScene.

I should note that of course with reconstructScene, you have a NxMx3 point cloud so you know the pixels that each point refers to, but I am actually aligning a different point cloud to this one and projecting it back into the image, so that wouldnt work

Answer 1

[Alberto Puig Rodriguez]

I know this question has been asked a few months ago, but if someone comes across the same problem, heres the solution I found:

MATLAB doesn't take the CameraParameters1 to compute the 3D point cloud, it rather merges the two cameraParameters into a new one, which one can see in pathToYourMatlab\toolbox\vision\vision\+vision\+internal\+calibration\StereoParametersImpl.m (or just debug reconstructScene) on line 274 where it creates this ominous matrix Q. This is actually the projection matrix to convert the coordinates either from 2D to 3D or vice versa as it is written on the comment:

[x, y, disparity, 1] * Q = [X, Y, Z, 1] * w

Thus in order to restore the 2d coordinates one has to compute it like this:

point2D_h = [X Y Z 1] * inv(Q);
point2D = [point2D_h(1)/point2D_h(4) point2D_h(2)/point2D_h(4)];

Of course, you have to save the matrix Q before somewhere

save Q Q

Hope it helps someone, cheers