test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -381,13 +381,15 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
         << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
     guessPose.precision_type = MATCH_PRECISION_FAST;
-    float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_.cast<float>());
+    Isometry3f finalPose;
+    float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
     if(score < 0.45){
         return false;
     }
     guessPose.map_point = mapPose_.cast<double>();
     guessPose.precision_type = MATCH_PRECISION_HIGH;
-    currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_.cast<float>());
+    currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
+    mapPose_ = finalPose.cast<double>();
     return true;
 }