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

updated some events with user_data/stamp

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-25 14:26:25 -04:00
parent 186dc60a49
commit 19d6da9035
5 changed files with 63 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
corelib/src/Rtabmap.cpp
View File

@@ -1733,10 +1733,7 @@ bool Rtabmap::process(const SensorData & data)
mapIds.insert(std::make_pair(iter->first, mapId));
labels.insert(std::make_pair(iter->first, label));
stamps.insert(std::make_pair(iter->first, stamp));
if(userData.size())
{
userDatas.insert(std::make_pair(iter->first, userData));
}
userDatas.insert(std::make_pair(iter->first, userData));
}
statistics_.setPoses(poses);
statistics_.setConstraints(constraints);
@@ -1932,10 +1929,7 @@ bool Rtabmap::process(const SensorData & data)
mapIds.insert(std::make_pair(iter->first, mapId));
labels.insert(std::make_pair(iter->first, label));
stamps.insert(std::make_pair(iter->first, stamp));
if(userData.size())
{
userDatas.insert(std::make_pair(iter->first, userData));
}
userDatas.insert(std::make_pair(iter->first, userData));
}
statistics_.setPoses(_optimizedPoses);
statistics_.setConstraints(_constraints);
@@ -2381,7 +2375,9 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global) const
{
@@ -2417,7 +2413,9 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
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));
}
@@ -2456,7 +2454,9 @@ void Rtabmap::getGraph(
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global)
{
@@ -2491,7 +2491,9 @@ void Rtabmap::getGraph(
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));
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
@@ -2603,8 +2605,10 @@ bool Rtabmap::computePath(int targetNode, bool global)
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
this->getGraph(nodes, constraints, mapIds, labels, true, global);
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
if(computePath(targetNode, nodes, constraints))
@@ -2632,8 +2636,10 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
this->getGraph(nodes, constraints, mapIds, labels, true, global);
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);

16
corelib/src/RtabmapThread.cpp
View File

@@ -117,13 +117,17 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
_rtabmap->get3DMap(signatures,
poses,
constraints,
mapIds,
stamps,
labels,
userDatas,
optimized,
full);
@@ -131,7 +135,9 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
poses,
constraints,
mapIds,
labels));
stamps,
labels,
userDatas));
}
void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
@@ -140,12 +146,16 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
_rtabmap->getGraph(poses,
constraints,
mapIds,
stamps,
labels,
userDatas,
optimized,
full);
@@ -153,7 +163,9 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
poses,
constraints,
mapIds,
labels));
stamps,
labels,
userDatas));
}