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

Fixed labels not correctly published

Browse Source
This commit is contained in:
matlabbe
2018-11-19 18:00:39 -05:00
parent 54c455f82a
commit 801bac5221
4 changed files with 19 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+12
View File
@@ -394,18 +394,21 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
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)
{
@@ -413,6 +416,13 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
}
stat.setWeights(mapIntInt);
std::map<int, std::string> mapIntStr;
for(unsigned int i=0; i<info.labelsKeys.size() && i<info.labelsValues.size(); ++i)
{
mapIntStr.insert(std::pair<int, std::string>(info.labelsKeys.at(i), info.labelsValues.at(i)));
}
stat.setLabels(mapIntStr);
stat.setLocalPath(info.localPath);
stat.setCurrentGoalId(info.currentGoalId);
@@ -443,6 +453,8 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
info.rawLikelihoodValues = uValues(stats.rawLikelihood());
info.weightsKeys = uKeys(stats.weights());
info.weightsValues = uValues(stats.weights());
info.labelsKeys = uKeys(stats.labels());
info.labelsValues = uValues(stats.labels());
info.localPath = stats.localPath();
info.currentGoalId = stats.currentGoalId();