matcher reset and laser filter

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -42,7 +42,7 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
     LOG(INFO) << "calib_: " << calib_.translation().transpose()
               << " rpy: " << rpy.transpose();
     trd_.reset(new boost::thread(boost::bind(&AdjustPPK::LoadMap, this)));
-    skipCnt_ = 20;
+    skipCnt_ = 40;
 }
 
 AdjustPPK::~AdjustPPK()
@@ -56,7 +56,7 @@ void AdjustPPK::ConfigConvert(const string &firetimeFile, const string &correcti
 {
     LOG(INFO) << "correctionFile: " << correctionFile;
     LOG(INFO) << "firetimeFile: " << firetimeFile;
-    convert_.reset(new pandar_pointcloud::Convert(firetimeFile, correctionFile));
+    convert_.reset(new pandar_pointcloud::Convert(firetimeFile, correctionFile, true));
     LOG(INFO) << "ConfigConvert done";
 }
 
@@ -231,6 +231,9 @@ void AdjustPPK::LoadMapInfo()
     directory_iterator stream_iter(outputDir);
     directory_iterator stream_end_iter;
     for(; stream_iter != stream_end_iter; stream_iter++) {
+        if(stream_iter->path().extension().string() != ".stream"){
+            continue;
+        }
         auto fileName = stream_iter->path().stem().string();
         ulong meshId;
         try {
@@ -488,10 +491,19 @@ bool AdjustPPK::ConfigMap(double timestamp)
         return false;
     }
     if(currMatcher_){
-        currMatcher_->unLoadAllBlocks();
+//        currMatcher_->unLoadAllBlocks();
+        currMatcher_.reset();
     }
-    while(!meshVec_.at(meshId).matcher){
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//    while(!meshVec_.at(meshId).matcher){
+//        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//    }
+    auto &meshInfo = meshVec_.at(meshId);
+    if(meshInfo.streamPath != ""){
+        if(LoadMesh(meshInfo.streamPath, meshInfo.matcher)){
+            LOG(INFO) << "mesh loaded: " << meshId;
+        }else{
+            return false;
+        }
     }
     currMatcher_ = meshVec_.at(meshId).matcher;
     LOG(INFO) << "set currMatcher_: " << meshId;
@@ -541,16 +553,16 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     if(score < 0){
         guessPose.precision_type = MATCH_PRECISION_INIT;
         currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
-        mapPose_ = finalPose.cast<double>();
+        mapPose_.translation() = finalPose.translation().cast<double>();
         CloseToPPK(0.1);
-        skipCnt_ = 5;
+        skipCnt_ = 10;
         return true;
     }
-    skipCnt_ = 20;
+    skipCnt_ = 40;
     guessPose.map_point = finalPose.cast<double>();
     guessPose.precision_type = MATCH_PRECISION_HIGH;
     currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
-    mapPose_ = finalPose.cast<double>();
+    mapPose_.translation() = finalPose.translation().cast<double>();
     return true;
 }
 
@@ -581,9 +593,10 @@ void AdjustPPK::LoadMap()
                 LOG(INFO) << "mesh exist: " << meshId;
             }
 
-            if(LoadMesh(streamPath, meshInfo.matcher)){
-                LOG(INFO) << "mesh loaded: " << meshId;
-            }
+            meshInfo.streamPath = streamPath;
+//            if(LoadMesh(streamPath, meshInfo.matcher)){
+//                LOG(INFO) << "mesh loaded: " << meshId;
+//            }
         }
 //        for(size_t id = period.startId; id <= period.endId; id += 100){
 //            if(requestQuit_){
@@ -668,7 +681,7 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
 
     LOG(INFO) << "mesh to load: " << streamPath;
     VoxelMapMatcherOption option;
-    option.option.filter_resolution = 1.2;
+    option.option.filter_resolution = 1.0;
     option.option.cloud_range = 80;
     option.fast_option.accepted_score = 0.3;
     option.fast_option.accepted_low_score = 0.4;

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -45,6 +45,7 @@ struct PCDPathInfo{
 struct MashMap{
     vector<PCDPathInfo> pcdPathVec;
     boost::shared_ptr<VoxelMapMatcher> matcher;
+    string streamPath;
 };
 
 class AdjustPPK