Skip to content
Snippets Groups Projects
Unverified Commit 2d6e50d9 authored by Tixiao Shan's avatar Tixiao Shan Committed by GitHub
Browse files

Merge pull request #205 from SpriteLin-ZJU/master

Fix keyframe indices downsample bug
parents 944cda4b 9b8c3e95
No related branches found
No related tags found
No related merge requests found
......@@ -463,6 +463,11 @@ public:
downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
for(auto& pt : globalMapKeyPosesDS->points)
{
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){
......@@ -868,6 +873,11 @@ public:
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
for(auto& pt : surroundingKeyPosesDS->points)
{
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();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment