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

fixed colored loop closure detection rect when no images are in GUI cache. Modified some logged info.

Browse Source
This commit is contained in:
matlabbe
2015-05-27 19:02:27 -04:00
parent d273ce1094
commit ece3915b10
6 changed files with 44 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -128,7 +128,8 @@ public:
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global);
bool global,
bool posesConstraintsOnly = false);
void clearPath();
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose, bool global);

2
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -95,7 +95,7 @@ private:
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void setDataBufferSize(int size);
void publishMap(bool optimized, bool full) const;
void publishTOROGraph(bool optimized, bool full) const;
void publishGraph(bool optimized, bool full) const;
private:
UMutex _stateMutex;

9
corelib/src/DBReader.cpp
View File

@@ -185,8 +185,6 @@ void DBReader::mainLoop()
if(goalId > 0)
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
if(!_ignoreGoalDelay && _currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
@@ -203,12 +201,19 @@ void DBReader::mainLoop()
double delay = stamp - previousStamp;
UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
goalId, delay);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
uSleep(delay*1000);
}
else
{
UWARN("Goal %d detected, posting it!", goalId);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
}
}
else
{
UWARN("Goal %d detected, posting it!", goalId);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
}
}

45
corelib/src/Rtabmap.cpp
View File

@@ -2570,17 +2570,16 @@ void Rtabmap::optimizeCurrentMap(
{
//Optimize the map
optimizedPoses.clear();
UDEBUG("Optimize map: around location %d", id);
UINFO("Optimize map: around location %d", id);
if(_memory && id > 0)
{
UTimer timer;
std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
UDEBUG("get ids=%d", (int)ids.size());
if(!_optimizeFromGraphEnd && ids.size() > 1)
{
id = ids.begin()->first;
}
UINFO("get ids time %f s", timer.ticks());
UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
@@ -2605,7 +2604,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
std::multimap<int, Link> edgeConstraints;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
@@ -2622,6 +2621,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
}
UINFO("Optimization time %f s", timer.ticks());
return optimizedPoses;
}
@@ -2817,7 +2817,8 @@ void Rtabmap::getGraph(
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global)
bool global,
bool posesConstraintsOnly)
{
if(_memory && _memory->getLastWorkingSignature())
{
@@ -2840,19 +2841,22 @@ void Rtabmap::getGraph(
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
if(!posesConstraintsOnly)
{
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
mapIds.insert(std::make_pair(iter->first, mapId));
stamps.insert(std::make_pair(iter->first, stamp));
labels.insert(std::make_pair(iter->first, label));
userDatas.insert(std::make_pair(iter->first, userData));
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
mapIds.insert(std::make_pair(iter->first, mapId));
stamps.insert(std::make_pair(iter->first, stamp));
labels.insert(std::make_pair(iter->first, label));
userDatas.insert(std::make_pair(iter->first, userData));
}
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
@@ -2991,6 +2995,7 @@ bool Rtabmap::computePath(
// return true if path is updated
bool Rtabmap::computePath(int targetNode, bool global)
{
UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
this->clearPath();
if(!_rgbdSlamMode)
@@ -2999,6 +3004,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
return false;
}
UTimer totalTimer;
UTimer timer;
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
@@ -3013,13 +3019,16 @@ bool Rtabmap::computePath(int targetNode, bool global)
{
updateGoalIndex();
}
UINFO("Time computing path = %fs", timer.ticks());
UINFO("Time computing path (A*) = %fs", timer.ticks());
UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
return _path.size()>0;
}
bool Rtabmap::computePath(const Transform & targetPose, bool global)
{
UINFO("Planning a path to pose %s (global=%d)", targetPose.prettyPrint().c_str(), global?1:0);
this->clearPath();
std::list<std::pair<int, Transform> > pathPoses;

6
corelib/src/RtabmapThread.cpp
View File

@@ -141,7 +141,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
userDatas));
}
void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
void RtabmapThread::publishGraph(bool optimized, bool full) const
{
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
@@ -257,10 +257,10 @@ void RtabmapThread::mainLoop()
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStatePublishingTOROGraphLocal:
this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, false);
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
break;
case kStatePublishingTOROGraphGlobal:
this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, true);
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStateTriggeringMap:
_rtabmap->triggerNewMap();