unload block

libs/localize/map_matcher_interface.cc

@@ -130,4 +130,9 @@ float MapMatcherInterface::MatchPointCloud(const PointCloudJ::Ptr pcloud, const
     return score;
 }
 
+void MapMatcherInterface::unLoadAllBlocks()
+{
+
+}
+
 } // namespace juefx

libs/localize/map_matcher_interface.h

@@ -47,6 +47,9 @@ public:
     float MatchPointCloud(const PointCloudJ::Ptr pcloud, const GuessPose guess_pose,
                           GnssPoint &final_pose, Eigen::Isometry3f &local_pose);
 
+
+    void unLoadAllBlocks();
+
 protected:
     virtual float InnerMatch(const VectorCloud &vector_cloud, const GuessPose guess_pose,
                              const Rigid3d &predict_pose, Rigid3d &estimate_pose) = 0;

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -470,6 +470,9 @@ bool AdjustPPK::ConfigMap(double timestamp)
     if(meshVec_.find(meshId) == meshVec_.end()){
         return false;
     }
+    if(currMatcher_){
+        currMatcher_->unLoadAllBlocks();
+    }
     while(!meshVec_.at(meshId).matcher){
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
@@ -504,26 +507,13 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     guessPose.movement = 2;
     guessPose.rotation = 0.01;
     guessPose.use_gnss = false;
-    uint16_t checkCnt = 0;
-    while(!currMatcher_->IsMapLoaded(guess.translation())){
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        if(++checkCnt > 200){
-            LOG(INFO) << "IsMapLoaded fail ++checkCnt > 200! ";
-            return false;
-        }
+    if(!currMatcher_->IsMapLoaded(guess.translation())){
+        currMatcher_->loadArea(guess.translation());
     }
     double avgGridCnt = currMatcher_->AreaAvgGridCnt(guess.translation());
     if(avgGridCnt < 20000){
         LOG(INFO) << "avgGridCnt < 20000! " << "avgGridCnt: " << avgGridCnt;
-        if(ppkIndex_ > 0){
-            double radio = 0.9;
-            const auto &localPPK = localPoseVec_.at(ppkIndex_ - 1);
-            mapPose_.translation() =
-                    localPPK.pose.translation() * radio +
-                    mapPose_.translation() * (1 - radio);
-            mapPose_.linear() =
-                    Quaterniond(mapPose_.linear()).slerp(radio, Quaterniond(localPPK.pose.linear())).toRotationMatrix();
-        }
+        CloseToPPK();
         return false;
     }
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << guess.translation().x()
@@ -535,6 +525,7 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
         guessPose.precision_type = MATCH_PRECISION_INIT;
         currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
         mapPose_ = finalPose.cast<double>();
+        CloseToPPK();
         skipCnt_ = 5;
         return true;
     }
@@ -577,15 +568,15 @@ void AdjustPPK::LoadMap()
                 LOG(INFO) << "mesh loaded: " << meshId;
             }
         }
-        for(size_t id = period.startId; id <= period.endId; id += 100){
-            if(requestQuit_){
-                return;
-            }
-            auto pose = localPoseVec_.at(id).pose;
-            if(!meshInfo.matcher->IsMapLoaded(pose.translation())){
-                meshInfo.matcher->loadArea(pose.translation());
-            }
-        }
+//        for(size_t id = period.startId; id <= period.endId; id += 100){
+//            if(requestQuit_){
+//                return;
+//            }
+//            auto pose = localPoseVec_.at(id).pose;
+//            if(!meshInfo.matcher->IsMapLoaded(pose.translation())){
+//                meshInfo.matcher->loadArea(pose.translation());
+//            }
+//        }
     }
 }
 
@@ -669,4 +660,17 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
     return true;
 }
 
+void AdjustPPK::CloseToPPK()
+{
+    if(ppkIndex_ > 0){
+        double radio = 0.9;
+        const auto &localPPK = localPoseVec_.at(ppkIndex_ - 1);
+        mapPose_.translation() =
+                localPPK.pose.translation() * radio +
+                mapPose_.translation() * (1 - radio);
+        mapPose_.linear() =
+                Quaterniond(mapPose_.linear()).slerp(radio, Quaterniond(localPPK.pose.linear())).toRotationMatrix();
+    }
+}
+
 } // end of namespace

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -89,6 +89,8 @@ private:
 
     bool LoadMesh(const string &streamPath,
                   boost::shared_ptr<VoxelMapMatcher> &matcher);
+
+    void CloseToPPK();
 private:
     string ieBaseDir_;
 

libs/voxel_map/interface/voxel_map.cc

@@ -50,6 +50,11 @@ void VoxelMap::ConfirmPosition(
     mapArea_->ConfirmPosition(pose_in_map, final_pose);
 }
 
+void VoxelMap::unLoadAllBlocks()
+{
+    mapArea_->unLoadAllBlocks();
+}
+
 float VoxelMap::GetResolution(uint8_t type) const
 {
     float resolution = mapArea_->getResolution();

libs/voxel_map/interface/voxel_map.h

@@ -24,6 +24,7 @@ public:
     double AreaAvgGridCnt(const Eigen::Vector3d& pose);
     bool GuessPosition(const GnssPoint &init_pose, Eigen::Isometry3d &pose_in_map);
     void ConfirmPosition(const Eigen::Vector3d &pose_in_map, GnssPoint &final_pose);
+    void unLoadAllBlocks();
 
 public:
     float GetResolution(uint8_t type) const;

libs/voxel_map/libs/area/blockarea.cpp

@@ -61,6 +61,17 @@ void BlockArea::load()
 
 //--------------------------------------------------------------------------
 
+void BlockArea::unLoad()
+{
+    vector<juefx::Distribution>().swap(distributionVec_);
+    for(size_t i = 0; i < 5; i++){
+        vector<vector<juefx::DistributionIndex>>().swap(indexTable_[i]);
+    }
+    loaded_ = false;
+}
+
+//--------------------------------------------------------------------------
+
 juefx::Distribution *BlockArea::getGridDistribution(
         uint8_t layerId, uint8_t shiftedX, uint8_t shiftedY, uint8_t &length) const
 {

libs/voxel_map/libs/area/blockarea.h

@@ -27,6 +27,8 @@ public:
 
     void load();
 
+    void unLoad();
+
     juefx::Distribution* getGridDistribution(
             uint8_t layerId, uint8_t shiftedX, uint8_t shiftedY, uint8_t &length) const;
 

libs/voxel_map/libs/area/maparea.h

@@ -266,6 +266,13 @@ public:
 
     float getResolution();
 
+    void unLoadAllBlocks(){
+        if(!currMesh_){
+            return;
+        }
+        currMesh_->unLoadAllBlocks();
+    }
+
 private:
     void backendThreadFunc();
 

libs/voxel_map/libs/area/mesharea.cpp

@@ -356,7 +356,19 @@ void MeshArea::loadBlockAtArea(const Vector3d &position)
         }
     }
 //    LOG(INFO) << "shiftedIndexToLoad: " << centerIndex.transpose()
-//              << " blockCntLoaded: " <<blockCntLoaded;
+    //              << " blockCntLoaded: " <<blockCntLoaded;
+}
+
+void MeshArea::unLoadAllBlocks()
+{
+    for(size_t x = 0; x < rowItemCnt_; x++){
+        for(size_t y = 0; y < colItemCnt_; y++){
+            BlockContent &blockContent = blockAreaTable_[x][y];
+            if(blockContent.block->isLoad()){
+                blockContent.block->unLoad();
+            }
+        }
+    }
 }
 
 }

libs/voxel_map/libs/area/mesharea.h

@@ -246,6 +246,7 @@ public:
         return offset_;
     }
 
+    void unLoadAllBlocks();
 private:
 
     void checkBlockState();
@@ -271,6 +272,7 @@ private:
 
     void loadBlockAtArea(const Vector3d &position);
 
+
 private:
     string meshPath_;