PnP that can handle points in homogenous coordinates

[lessthanoptimal]

Needs to properly handle homogenous coordinates and not just convert them into 3D points. This way if points are far away they will not screw up the numerics and will effectively just contribute to the orientation estimate. If only points at infinity are found the translation component should be zero. This might require adding a slight bias to keeping it at (0,0,0).