Heuristic-Based Laser Scan Matching for Outdoor 6D SLAM -- A Probabilistic Multimodal