cpp implementation of robotics algorithms including localization, mapping, SLAM, path planning and control