fix the issue (SteveMacenski#198) that loop closures can rotate the m…

…ap 180° if the lidar is mounted backwards

Set the edges (constraints) based on the robot base frame (base_footprint) instead of sensor frame (laser).

slam_toolbox/lib/karto_sdk/src/Mapper.cpp

@@ -1607,7 +1607,11 @@ namespace karto
     // only attach link information if the edge is new
     if (isNewEdge == true)
     {
-      pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
+      auto corrected_pose=pToScan->GetCorrectedPose();
+      pToScan->SetSensorPose(rMean);
+      auto corrected_mean_pose=pToScan->GetCorrectedPose();
+      pToScan->SetCorrectedPose(corrected_pose);
+      pEdge->SetLabel(new LinkInfo(pFromScan->GetCorrectedPose(), corrected_mean_pose, rCovariance));
       if (m_pMapper->m_pScanOptimizer != NULL)
       {
         m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
@@ -1990,7 +1994,7 @@ namespace karto
         {
           continue;
         }
-        scan->SetSensorPose(iter->second);
+        scan->SetCorrectedPose(iter->second);
       }
 
       pSolver->Clear();