mpkuse/solve_keyframe_pose_graph

A kidnap-aware multi-threaded node to solve 6DOF posegraph slam. Needs poses at each node (subscribes to) and relative positions at edges. Maintains an optimized pose graph. Has support for recovery from kidnap