Systematic Handling of Heterogeneous Geometric Primitives in Graph-SLAM Optimization