PinholeCameraModel::project3dToPixel is correct?

edit

How do you get to that?

[U V W]^T = P * [X Y Z 1]^T   (X=xyz.x, Y=xyz.y ...)

with P something like:

[[ fx 0 0 Tx]
 [ 0 fy 0 Ty]
 [ 0 0  1 0]]

That gives: u = U/W + cx which is what we have in the code no?

The function inverts all points at the midpoint so x and y are inverted. This was at least for me very surprising and I sounds wrong to me. So for anybody having a problem with this function try to invert both axis and see if that helps.

It's possible there's a bug /transcription error. Can you create a unit test and show it's invalid and see if your solution works correctly? If so I'm sure the maintainer would be interested.