3D Mapping Database Aided GNSS Based Collaborative Positioning Using Factor Graph Optimization