Add keyframing and submapping
Finish coarse registration and GTSAM PGO
finish depth registration option, tested