I'm trying to write my own implementation of cv2.projectPoints just for practice, and my implementation is not behaving as expected.
I'm assuming a pinhole camera with no distortion, so all I'm doing is applying the following equation to all of my planar object points.
Let's say in some world space (that both a plane and the camera exist in), the plane is 2 units away. So, in world coordinate space, all the points on the plane have z=2. However, in the plane's own coordinate space, all the points on the plane have z=0. So, the vector [X Y Z 1] is actually [X Y 0 1] for all of the coordinates I'm trying to project. Let's say I find, in my pose estimation, that the object is translated -2 in the z direction. After multiplying this vector by the found pose, I get the vector...
r11X + r12Y + t1
r21X + r22Y + t2
r31X + r32Y + 2
But now, the projected point no longer has a z = 2 because of r31 and r32.