test

libs/localize/voxel_map/voxel_map_interface.h

@@ -3,7 +3,7 @@
 //#include "voxel_map_stub.h"
 #include "voxel_map/interface/voxel_map.h"
 
-#define VOXEL_MAP_DEBUG
-#define VOXEL_MAP_FAST_DEBUG
+//#define VOXEL_MAP_DEBUG
+//#define VOXEL_MAP_FAST_DEBUG
 //#define VOXEL_MAP_CERES_DEBUG
 //#define VOXEL_MAP_CERES_OUTPUT_NUM 0

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -363,7 +363,8 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
 
     Quaternionf q(mapPose_.linear());
     Vector3f rpy= RotationQuaternionToEulerVector(q);
-    LOG(INFO) << "guessPose: " << mapPose_.translation().transpose()
+    LOG(INFO) << setprecision(15) << "cloud time: " << cloudInfo.timestamp
+              << " guessPose: " << mapPose_.translation().transpose()
               << " rpy: " << rpy.transpose();
 //        pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
     GnssPoint finalGnssPose;
@@ -418,7 +419,10 @@ void AdjustPPK::LoadMap()
         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());
+                auto pose = localPoseVec_.at(id).pose;
+                if(!meshInfo.matcher->IsMapLoaded(pose.translation())){
+                    meshInfo.matcher->loadArea(pose.translation());
+                }
             }
         }
     }

libs/voxel_map/libs/area/mesharea.cpp

@@ -238,7 +238,7 @@ void MeshArea::reSetupNeighBorMesh()
 bool MeshArea::checkBlockAtAreaToLoad(
         const Vector2i &shiftedIndexToCheck)
 {
-    LOG(INFO) << "shiftedIndexToCheck: " << shiftedIndexToCheck.transpose();
+//    LOG(INFO) << "shiftedIndexToCheck: " << shiftedIndexToCheck.transpose();
     Vector2i centerIndex(shiftedIndexToCheck.x(), shiftedIndexToCheck.y());
     for(unsigned int i = 0; i < checkNeighbors_.size(); i++){
         Vector2i curr_index = centerIndex + checkNeighbors_[i];