test

libs/localize/voxel_map/options.h

@@ -17,8 +17,8 @@ struct CeresVoxelMatcherOption
 struct FastVoxelMatcherOption
 {
     float imu_height = 1.3f;                    // divide clouds into ground & above_ground
-    float init_XYZ_scale = 8.f;                 // INIT_MODE, enlarge XYZ search window by times
-    float init_Yaw_scale = 4.f;                 // INIT_MODE, enlarge yaw search window by times
+    float init_XYZ_scale = 2.f;                 // INIT_MODE, enlarge XYZ search window by times
+    float init_Yaw_scale = 1.f;                 // INIT_MODE, enlarge yaw search window by times
     float XY_search_window = 0.7f;              // HARD LIMIT of search window in xy, m, < 9cm * 8
     float Z_search_window = 0.7f;               // HARD LIMIT of search window in z, m < 9cm * 8
     float Yaw_search_window = 1.f / 180 * M_PI; // HARD LIMIT of search window in yaw, rad

libs/voxel_map/libs/area/mesharea.cpp

@@ -28,6 +28,11 @@ MeshArea::MeshArea(const string &meshPath)
                 loadNeighbors_.push_back({x, y});
             }
         }
+    window_size = {1, 1};
+    for(int x = -window_size.x(); x <= window_size.x(); x++)
+        for(int y = -window_size.y(); y <= window_size.y(); y++){
+            checkGridNeighbors_.push_back({x, y});
+        }
 }
 
 void MeshArea::init()
@@ -165,12 +170,15 @@ double MeshArea::AreaAvgGridCnt(const Vector3d &pose)
     double total = 0;
     Vector2i shiftedIndexToLoad =
             calShiftedIndex(pose.x(), pose.y());
-    const BlockContent &blockContent = blockAreaTable_[shiftedIndexToLoad.x()][shiftedIndexToLoad.y()];
-    if(blockContent.blockState == CurrMesh &&
-            blockContent.block->isLoad()){
-        total = blockContent.block->GetGridSize();
+    for(unsigned int i = 0; i < checkGridNeighbors_.size(); i++){
+        Vector2i curr_index = shiftedIndexToLoad + checkGridNeighbors_[i];
+        const BlockContent &blockContent = blockAreaTable_[curr_index.x()][curr_index.y()];
+        if(blockContent.blockState == CurrMesh &&
+                blockContent.block->isLoad()){
+            total += blockContent.block->GetGridSize();
+        }
     }
-    return total;
+    return total / checkGridNeighbors_.size();
 }
 
 void MeshArea::loadArea(const Vector2d &bl)

libs/voxel_map/libs/area/mesharea.h

@@ -312,6 +312,8 @@ private:
 
     vector<Vector2i> loadNeighbors_;
 
+    vector<Vector2i> checkGridNeighbors_;
+
     mutable ulong neighBorMeshId_;
 
     bool fstLoadMap = true;