fix keyframe downsample bug in global visualization

hitleon · Feb 23, 2021 · 10dc976 · 10dc976
1 parent 944cda4
commit 10dc976
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp
@@ -463,6 +463,13 @@ class mapOptimization : public ParamServer
         downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
         downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
         downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
+        for(auto& pt : globalMapKeyPosesDS->points)
+        {
+            pointSearchIndGlobalMap.resize(1);
+            pointSearchSqDisGlobalMap.resize(1);
+            kdtreeGlobalMap->nearestKSearch(pt, 1, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);
+            pt.intensity = cloudKeyPoses3D->points[pointSearchIndGlobalMap[0]].intensity;
+        }
 
         // extract visualized and downsampled key frames
         for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i){