From 6a8168b5d46584b489ab05adcf13024cbb16092b Mon Sep 17 00:00:00 2001
From: SpriteLin-ZJU <317815056@qq.com>
Date: Tue, 23 Feb 2021 13:39:14 +0800
Subject: [PATCH] fix keyframe id downsample bug in function extractNeatby()
---
src/mapOptmization.cpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp
index e9f54e3..ae17bc9 100644
--- a/src/mapOptmization.cpp
+++ b/src/mapOptmization.cpp
@@ -875,6 +875,13 @@ public:
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
+ for(auto& pt : surroundingKeyPosesDS->points)
+ {
+ pointSearchInd.resize(1);
+ pointSearchSqDis.resize(1);
+ kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
+ pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
+ }
// also extract some latest key frames in case the robot rotates in one position
int numPoses = cloudKeyPoses3D->size();
--
GitLab