test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -242,22 +242,23 @@ void AdjustPPK::CalPPKPeriod()
     ppkPeriodVec_.reserve(1000);
     PPKPeriod period;
     period.meshId = 0;
-    period.period.reserve(10000);
-    for(const IERawData& rawPPK : rawData_){
+    for(size_t i = 0; i < rawData_.size(); i++){
+        const IERawData& rawPPK = rawData_.at(i);
         ulong meshId = GetMeshFromPosition(rawPPK.longitude, rawPPK.latitude);
         if(meshId != period.meshId){
-            if(period.period.size()){
+            if(period.startTime > 0){
                 period.endTime = rawPPK.unix_time;
+                period.endId = i - 1;
                 ppkPeriodVec_.emplace_back(period);
-                period.meshId = meshId;
                 LOG(INFO) << "period.meshId: " << period.meshId;
-                period.period.resize(0);
             }
             period.startTime = rawPPK.unix_time;
+            period.startId = i;
+            period.meshId = meshId;
         }
-        period.period.emplace_back(rawPPK);
     }
     period.endTime = rawData_.back().unix_time;
+    period.endId = rawData_.size() - 1;
     ppkPeriodVec_.emplace_back(period);
     LOG(INFO) << "ppkPeriodVec_.size(): " << ppkPeriodVec_.size();
 }
@@ -309,7 +310,7 @@ bool AdjustPPK::Undisort(
     PointCloudJ mappedFrame;
     CloudPreprocess::Instance()->Undisort(
                             frame, undisortedFrame, &mappedFrame, rawFrame.cloud.back().time, &periodPose, framePose, increasePose);
-//    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(rawFrame.timestamp) + "_mapped.pcd", mappedFrame);
+    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(rawFrame.timestamp) + "_mapped.pcd", mappedFrame);
     cloudInfo.frame = undisortedFrame;
     cloudInfo.filterPoseInfo.increasePose = increasePose;
     cloudInfo.timestamp = rawFrame.timestamp;
@@ -373,10 +374,11 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
     guessPose.rotation = fabs(increaseRpy.z());
     guessPose.use_gnss = false;
-    LOG(INFO) << "currMatcher_ ptr: " << currMatcher_.get();
     if(!currMatcher_->IsMapLoaded(mapPose_.translation().cast<double>())){
         currMatcher_->loadArea(mapPose_.translation().cast<double>());
     }
+    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_);
     if(score < 0.45){
@@ -385,8 +387,6 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     guessPose.map_point = mapPose_.cast<double>();
     guessPose.precision_type = MATCH_PRECISION_HIGH;
     currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_);
-    ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
-        << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
     return true;
 }
 
@@ -415,9 +415,12 @@ void AdjustPPK::LoadMap()
             LOG(INFO) << "mesh exist: " << meshId;
         }
 
-        LoadMesh(streamPath, meshInfo.matcher);
-        LOG(INFO) << "mesh loaded: " << meshId
-                  << " ptr: " << meshInfo.matcher.get();
+        if(LoadMesh(streamPath, meshInfo.matcher)){
+            LOG(INFO) << "mesh loaded: " << meshId;
+            for(size_t id = period.startId; id <= period.endId; id += 100){
+                meshInfo.matcher->loadArea(localPoseVec_.at(id).pose.translation());
+            }
+        }
     }
 }
 
@@ -444,11 +447,11 @@ void AdjustPPK::Compress(const vector<string> &pcdPathVec,
     blockSerializer.configSerialization();
 }
 
-void AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMatcher> &matcher)
+bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMatcher> &matcher)
 {
     if(!boost::filesystem::exists(streamPath)){
         LOG(WARNING) << "streamPath not exists: " << streamPath;
-        return;
+        return false;
     }
 
     LOG(INFO) << "mesh to load: " << streamPath;
@@ -459,6 +462,7 @@ void AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
     option.fast_option.accepted_low_score = 0.55;
     matcher.reset(new VoxelMapMatcher(option));
     matcher->SetMapPath(streamPath);
+    return true;
 }
 
 } // end of namespace

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -14,10 +14,11 @@
 #include "serialize/blockserializer.h"
 
 struct PPKPeriod{
-    vector<IERawData> period;
     ulong meshId;
     double startTime = -1;
     double endTime = -1;
+    uint32_t startId = 0;
+    uint32_t endId = 0;
 
     int InPeriod(double timestamp){
         LOG_EVERY_N(INFO, 99) << setprecision(15) << "timestamp " << timestamp
@@ -78,7 +79,7 @@ private:
     void Compress(const vector<string> &pcdPathVec,
                   const string &streamPath);
 
-    void LoadMesh(const string &streamPath,
+    bool LoadMesh(const string &streamPath,
                   boost::shared_ptr<VoxelMapMatcher> &matcher);
 private:
     string ieBaseDir_;

libs/voxel_map/libs/area/maparea.h

@@ -88,7 +88,7 @@ public:
     }
     inline bool IsMapLoaded(const Vector3d& pose)
     {
-        LOG(INFO) << "pose: " << pose.transpose();
+//        LOG(INFO) << "pose: " << pose.transpose();
         return currMesh_->IsAreaLoaded(pose);
     }
 

libs/voxel_map/libs/area/mesharea.cpp

@@ -271,7 +271,6 @@ void MeshArea::loadBlockAtArea(const Vector3d &position)
     Vector2i centerIndex =
             calShiftedIndex(position.x(), position.y());
     LOG(INFO) << "shiftedIndexToLoad: " << centerIndex.transpose();
-    LOG(INFO) << "offset_: " << offset_.transpose();
     vector<Vector2i> neighbors;
     if(fstLoadMap){
         neighbors = firstLoadNeighbors_;

libs/voxel_map/libs/area/mesharea.h

@@ -73,8 +73,8 @@ public:
         }
         Vector2i shiftedIndexToLoad =
                 calShiftedIndex(pose.x(), pose.y());
-        LOG(INFO) << "pose: " << pose.transpose()
-                  << " shiftedIndexToLoad: " << shiftedIndexToLoad.transpose();
+//        LOG(INFO) << "pose: " << pose.transpose()
+//                  << " shiftedIndexToLoad: " << shiftedIndexToLoad.transpose();
         return checkBlockAtAreaToLoad(shiftedIndexToLoad);
     }