init rp search

libs/localize/map_matcher_interface.cc

@@ -45,6 +45,13 @@ bool MapMatcherInterface::IsMapLoaded(const Eigen::Vector3d &pose)
     return map_ != NULL && map_->IsMapLoaded(pose);
 }
 
+double MapMatcherInterface::AreaAvgGridCnt(const Eigen::Vector3d& pose){
+    if(map_ != NULL){
+        return map_->AreaAvgGridCnt(pose);
+    }
+    return 0;
+}
+
 float MapMatcherInterface::MatchPointCloud(const PointCloudJ::Ptr pcloud, const GuessPose guess_pose,
                                            GnssPoint &final_pose, Eigen::Isometry3f &local_pose)
 {

libs/localize/map_matcher_interface.h

@@ -33,6 +33,8 @@ public:
 
     bool IsMapLoaded(const Eigen::Vector3d &pose);
 
+    double AreaAvgGridCnt(const Vector3d& pose);
+
     float MatchPointCloud(const PointCloudJ::Ptr pcloud, const GnssPoint init_pose,
                           GnssPoint &final_pose, Eigen::Isometry3f &local_pose) {
         GuessPose guess_pose;

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -393,6 +393,11 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     guessPose.use_gnss = false;
     if(!currMatcher_->IsMapLoaded(mapPose_.translation())){
         currMatcher_->loadArea(mapPose_.translation());
+        double avgGridCnt = currMatcher_->AreaAvgGridCnt(mapPose_.translation());
+        LOG(INFO) << "avgGridCnt: " << avgGridCnt;
+        if(avgGridCnt < 5){
+            return false;
+        }
     }
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
         << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;

libs/voxel_map/interface/voxel_map.cc

@@ -34,6 +34,10 @@ bool VoxelMap::IsMapLoaded(
     return mapArea_->IsMapLoaded(pose);
 }
 
+double VoxelMap::AreaAvgGridCnt(const Eigen::Vector3d& pose){
+    return currMesh_->AreaAvgGridCnt(pose);
+}
+
 bool VoxelMap::GuessPosition(
         const GnssPoint &init_pose,
         Isometry3d &pose_in_map) {

libs/voxel_map/interface/voxel_map.h

@@ -21,6 +21,7 @@ public:
     void loadArea(const Eigen::Vector3d pose);
     bool IsMapLoaded(const GnssPoint blh_pose);
     bool IsMapLoaded(const Eigen::Vector3d pose);
+    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);
 

libs/voxel_map/libs/area/blockarea.h

@@ -67,6 +67,10 @@ public:
                   << " rZ: " << rZ;
         return true;
     }
+
+    inline size_t GetGridSize(){
+        return hashMap_.size();
+    }
 private:
     void parseBitmapBlock(const BlockInfo &blockInfo);
 

libs/voxel_map/libs/area/maparea.h

@@ -92,6 +92,10 @@ public:
         return currMesh_->IsAreaLoaded(pose);
     }
 
+    inline bool AreaAvgGridCnt(const Vector3d& pose){
+        return currMesh_->AreaAvgGridCnt(pose);
+    }
+
     template <typename T>
     Eigen::Matrix<T, 3, 1> RotationQuaternionToEulerVector(
         const Eigen::Quaternion<T>& quaternion) {

libs/voxel_map/libs/area/mesharea.cpp

@@ -157,6 +157,30 @@ void MeshArea::backendFunc(uint8_t type, Vector2d point)
     }
 }
 
+double MeshArea::AreaAvgGridCnt(const Vector3d &pose)
+{
+    if(!inited_){
+        return false;
+    }
+    double total = 0;
+    size_t cnt = 0;
+    Vector2i shiftedIndexToLoad =
+            calShiftedIndex(pose.x(), pose.y());
+    for(unsigned int i = 0; i < loadNeighbors_.size(); i++){
+        Vector2i curr_index = shiftedIndexToLoad + loadNeighbors_[i];
+        const BlockContent &blockContent = blockAreaTable_[curr_index.x()][curr_index.y()];
+        if(blockContent.blockState == CurrMesh &&
+                blockContent.block->isLoad()){
+            total += blockContent.block->GetGridSize();
+            cnt++;
+        }
+    }
+    if(0 == cnt){
+        return 0;
+    }
+    return total / cnt;
+}
+
 void MeshArea::loadArea(const Vector2d &bl)
 {
     if(!inited_){

libs/voxel_map/libs/area/mesharea.h

@@ -78,6 +78,8 @@ public:
         return checkBlockAtAreaToLoad(shiftedIndexToLoad);
     }
 
+    inline double AreaAvgGridCnt(const Vector3d &pose);
+
     void setMeshId(ulong meshId){
         meshId_ = meshId;
         meshCenter_ = GetMeshCenter(meshId_);