Isolated pnp and rodrigues related stuff
CalcTf3D: Made temp variables into member
CalcTf3D: Added documentation
Renamed CalcTf3D to EstimatePose3D