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

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

@@ -117,13 +117,17 @@ public:
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;
void 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);
void clearPath();

12
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -149,13 +149,17 @@ public:
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds,
const std::map<int, std::string> & labels) :
const std::map<int, double> & stamps,
const std::map<int, std::string> & labels,
const std::map<int, std::vector<unsigned char> > & userDatas) :
UEvent(0),
_signatures(signatures),
_poses(poses),
_constraints(constraints),
_mapIds(mapIds),
_labels(labels)
_stamps(stamps),
_labels(labels),
_userDatas(userDatas)
{}
virtual ~RtabmapEvent3DMap() {}
@@ -164,7 +168,9 @@ public:
const std::map<int, Transform> & getPoses() const {return _poses;}
const std::multimap<int, Link> & getConstraints() const {return _constraints;}
const std::map<int, int> & getMapIds() const {return _mapIds;}
const std::map<int, double> & getStamps() const {return _stamps;}
const std::map<int, std::string> & getLabels() const {return _labels;}
const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
@@ -173,7 +179,9 @@ private:
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;
};
} // namespace rtabmap

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));
}