i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

RGBD/NeighborLinkRefining: do one proximity detection by time if intermediate nodes are added. Optimized radiusFiltering(). Updated default parameters of proportionalRadiusFiltering().

Browse Source
This commit is contained in:
matlabbe
2022-01-14 18:47:57 -05:00
parent 51f6b32f47
commit 29a2b64a0c
4 changed files with 39 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/Rtabmap.cpp
View File

@@ -1407,6 +1407,7 @@ bool Rtabmap::process(
// Update optimizedPoses with the newly added node
Transform newPose;
bool intermediateNodeRefining = false;
if(_neighborLinkRefining &&
signature->getLinks().size() &&
signature->getLinks().begin()->second.type() == Link::kNeighbor &&
@@ -1507,9 +1508,8 @@ bool Rtabmap::process(
}
else
{
UWARN("Neighbor link refining is activated but there are intermediate nodes (%d=%d %d=%d), aborting refining...",
signature->id(), signature->getWeight(), oldS->id(), oldS->getWeight());
newPose = _mapCorrection * signature->getPose();
intermediateNodeRefining = true;
}
}
else
@@ -1633,7 +1633,7 @@ bool Rtabmap::process(
//============================================================
// Local loop closure in TIME
//============================================================
if(_proximityByTime &&
if((_proximityByTime || intermediateNodeRefining) &&
rehearsedId == 0 && // don't do it if rehearsal happened
_memory->isIncremental() && // don't do it in localization mode
signature->getWeight()>=0)
@@ -1684,6 +1684,12 @@ bool Rtabmap::process(
UINFO("Local loop closure (time) between %d and %d rejected: %s",
*iter, signature->id(), rejectedMsg.c_str());
}
if(!_proximityByTime && intermediateNodeRefining)
{
// Do it only with the latest non-intermediate node
break;
}
}
}
}

4
corelib/src/util3d_filtering.cpp
View File

@@ -1123,7 +1123,7 @@ pcl::IndicesPtr radiusFilteringImpl(
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
@@ -1141,7 +1141,7 @@ pcl::IndicesPtr radiusFilteringImpl(
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;