i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Gui: fixed default color for scans from odom in MainWindow and constraint view in dbviewer. Recovery/reprocess: fixed triggering new map on covariance >=9999. Reg: repeatOnce not done if it has a child. Statistics: added time proximity by space visual info.

Browse Source
This commit is contained in:
matlabbe
2018-02-20 17:33:48 -05:00
parent 5c04ce257b
commit db1139e89e
8 changed files with 56 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+12 -1
View File
@@ -871,6 +871,7 @@ bool Rtabmap::process(
double timeMemoryUpdate = 0;
double timeNeighborLinkRefining = 0;
double timeProximityByTimeDetection = 0;
double timeProximityBySpaceVisualDetection = 0;
double timeProximityBySpaceDetection = 0;
double timeCleaningNeighbors = 0;
double timeReactivations = 0;
@@ -2009,6 +2010,9 @@ bool Rtabmap::process(
}
}
timeProximityBySpaceVisualDetection = timer.ticks();
ULOGGER_INFO("timeProximityBySpaceVisualDetection=%fs", timeProximityBySpaceVisualDetection);
//
// 2) compare locally with nearest locations by scan matching
//
@@ -2419,6 +2423,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
@@ -3077,6 +3082,11 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
std::map<int, Transform> path;
// select nearest pose and iterate neighbors from there
int nearestId = rtabmap::graph::findNearestNode(poses, target);
if(nearestId == 0)
{
UWARN("Nearest id of %s in %d poses is 0 !? Returning empty path.", target.prettyPrint().c_str(), (int)poses.size());
break;
}
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, true, nodesSet);
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
@@ -3114,7 +3124,8 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
}
else
{
UWARN(uFormat("path.size()=0!? nearestId=%d ids=%d", (int)path.size(), nearestId, (int)ids.size()).c_str());
UWARN(uFormat("path.size()=0!? nearestId=%d ids=%d, aborting...", (int)path.size(), nearestId, (int)ids.size()).c_str());
break;
}
}