Robust Planar Pose Estimation implemented in C++