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

MsgConversion.h: added conversion of rtabmap_ros/Info messages from/to rtabmap::Statistics

Browse Source 
rtabmapviz: added subscription to stereo.
Updated demo_stereo_outdoor.launch with arguments to choose between rtabmapviz and rviz
Added localPath array in rtabmap_ros/Info message
rtabmap: publishing the local path, uniformized time stamps between published topics at each iteration
This commit is contained in:
Mathieu Labbe
2015-02-03 11:16:06 -05:00
parent a78cbb533c
commit 0b67fe4e3b
8 changed files with 611 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+74
View File
@@ -124,6 +124,80 @@ cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool co
return out;
}
void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
{
stat.setExtended(true); // Extended
// rtabmap_ros::Info
stat.setRefImageId(info.refId);
stat.setLoopClosureId(info.loopClosureId);
stat.setLocalLoopClosureId(info.localLoopClosureId);
stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
//Posterior, likelihood, childCount
std::map<int, float> mapIntFloat;
for(unsigned int i=0; i<info.posteriorKeys.size() && i<info.posteriorValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(info.posteriorKeys.at(i), info.posteriorValues.at(i)));
}
stat.setPosterior(mapIntFloat);
mapIntFloat.clear();
for(unsigned int i=0; i<info.likelihoodKeys.size() && i<info.likelihoodValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(info.likelihoodKeys.at(i), info.likelihoodValues.at(i)));
}
stat.setLikelihood(mapIntFloat);
mapIntFloat.clear();
for(unsigned int i=0; i<info.rawLikelihoodKeys.size() && i<info.rawLikelihoodValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(info.rawLikelihoodKeys.at(i), info.rawLikelihoodValues.at(i)));
}
stat.setRawLikelihood(mapIntFloat);
std::map<int, int> mapIntInt;
for(unsigned int i=0; i<info.weightsKeys.size() && i<info.weightsValues.size(); ++i)
{
mapIntInt.insert(std::pair<int, int>(info.weightsKeys.at(i), info.weightsValues.at(i)));
}
stat.setWeights(mapIntInt);
stat.setLocalPath(info.localPath);
// Statistics data
for(unsigned int i=0; i<info.statsKeys.size() && i<info.statsValues.size(); i++)
{
stat.addStatistic(info.statsKeys.at(i), info.statsValues.at(i));
}
}
void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
{
info.refId = stats.refImageId();
info.loopClosureId = stats.loopClosureId();
info.localLoopClosureId = stats.localLoopClosureId();
rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform);
// Detailed info
if(stats.extended())
{
//Posterior, likelihood, childCount
info.posteriorKeys = uKeys(stats.posterior());
info.posteriorValues = uValues(stats.posterior());
info.likelihoodKeys = uKeys(stats.likelihood());
info.likelihoodValues = uValues(stats.likelihood());
info.rawLikelihoodKeys = uKeys(stats.rawLikelihood());
info.rawLikelihoodValues = uValues(stats.rawLikelihood());
info.weightsKeys = uKeys(stats.weights());
info.weightsValues = uValues(stats.weights());
info.localPath = stats.localPath();
// Statistics data
info.statsKeys = uKeys(stats.data());
info.statsValues = uValues(stats.data());
}
}
rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
{
return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.variance);