test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

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