i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

3
corelib/src/Memory.cpp
View File

@@ -2375,6 +2375,7 @@ Transform Memory::computeTransform(
// make sure feature matching is used instead of optical flow to compute the guess
ParametersMap parameters = parameters_;
uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
uInsert(parameters, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
RegistrationVis regVis(parameters);
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
if(!guess.isNull())
@@ -2475,7 +2476,7 @@ Transform Memory::computeIcpTransformMulti(
UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
UDEBUG("%d -> %d, Guess=%s", fromId, toId, (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
if(ULogger::level() == ULogger::kDebug)
{
std::string ids;

7
corelib/src/Recovery.cpp
View File

@@ -136,9 +136,10 @@ bool databaseRecovery(
Rtabmap rtabmap;
rtabmap.init(parameters, databasePath);
bool odometryIgnored = false;
Parameters::parse(parameters, Parameters::kRGBDEnabled(), odometryIgnored);
DBReader dbReader(backupPath, 0, !odometryIgnored);
bool rgbdEnabled = Parameters::defaultRGBDEnabled();
Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
bool odometryIgnored = !rgbdEnabled;
DBReader dbReader(backupPath, 0, odometryIgnored);
dbReader.init();
CameraInfo info;

40
corelib/src/Registration.cpp
View File

@@ -207,10 +207,32 @@ Transform Registration::computeTransformationMod(
}
Transform t = computeTransformationImpl(from, to, guess, info);
if(repeatOnce_ && guess.isNull() && !t.isNull() && this->canUseGuess())
if(child_)
{
if(!t.isNull())
{
t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
}
else if(!guess.isNull())
{
UDEBUG("This registration approach failed, continue with the guess for the next registration");
t = child_->computeTransformationMod(from, to, guess, &info);
}
}
else if(repeatOnce_ && guess.isNull() && !t.isNull() && this->canUseGuess())
{
// redo with guess to get a more accurate transform
t = computeTransformationImpl(from, to, t, info);
if(!t.isNull() && force3DoF_)
{
t = t.to3DoF();
}
}
else if(!t.isNull() && force3DoF_)
{
t = t.to3DoF();
}
if(info.covariance.empty())
@@ -231,22 +253,6 @@ Transform Registration::computeTransformationMod(
if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
if(child_)
{
if(!t.isNull())
{
t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
}
else if(!guess.isNull())
{
UDEBUG("This registration approach failed, continue with the guess for the next registration");
t = child_->computeTransformationMod(from, to, guess, &info);
}
}
else if(!t.isNull() && force3DoF_)
{
t = t.to3DoF();
}
if(infoOut)
{

13
corelib/src/Rtabmap.cpp
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;
}
}