test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -363,7 +363,6 @@ bool AdjustPPK::ConfigMap(double timestamp)
 
 bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
 {
-    static bool fstMatch = true;
     if(!currMatcher_){
         LOG(INFO) << "no currMatcher_!";
         return false;
@@ -389,25 +388,23 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     }
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
         << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
-    if(fstMatch){
-        guessPose.precision_type = MATCH_PRECISION_INIT;
-        fstMatch = false;
-    }else{
-        guessPose.precision_type = MATCH_PRECISION_FAST;
-    }
+    guessPose.precision_type = MATCH_PRECISION_FAST;
     Isometry3f finalPose;
     float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
     if(score < 0){
-        return false;
-    }
-    mapPose_ = finalPose.cast<double>();
-    if(score == 0){
+        guessPose.precision_type = MATCH_PRECISION_INIT;
+        currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
+        mapPose_ = finalPose.cast<double>();
         return true;
     }
+
     guessPose.map_point = finalPose.cast<double>();
     guessPose.precision_type = MATCH_PRECISION_HIGH;
     currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
     mapPose_ = finalPose.cast<double>();
+    PointCloudJ mathedCloud;
+    pcl::transformPointCloud(*cloudInfo.frame, mathedCloud, finalPose);
+    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + "_mathed.pcd", mathedCloud);
     return true;
 }
 
@@ -425,6 +422,12 @@ void AdjustPPK::LoadMap()
         }
         auto &meshInfo = meshVec_.at(meshId);
         if(meshInfo.matcher){
+            for(size_t id = period.startId; id <= period.endId; id += 100){
+                auto pose = localPoseVec_.at(id).pose;
+                if(!meshInfo.matcher->IsMapLoaded(pose.translation())){
+                    meshInfo.matcher->loadArea(pose.translation());
+                }
+            }
             continue;
         }
         string streamPath = outputDir + to_string(meshId) + ".stream";