Add SLAM pose graph optimization with GPS prior

Core SLAM lib changes

Add Pose Graph Optimization (PGO) to SLAM to be able to optimize the SLAM trajectory and maps with GPS positions and covariances.

Enable logging to save previous computed SLAM poses, covariances or frames keypoints.

Add command to enable/disable SLAM map update if we want to run localization only or also build keypoints maps.

Clean and improve SLAM algorithm and RollingGrid for more clarity and faster speed.

ROS wrapping changes

Add PGO support into LidarSlamNode.

Replace old "empty msgs topics publication" to send commands to SLAM by a single topic with enum.

Add GPS heading computation from movement if it is not already provided.

Remove SLAM logging from LidarSlamNode (this is done internally by SLAM algorithm).