i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-14 15:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap_pkg: added rawLikelihood to infoEx.msg. Updated statistics interface. Warning about parameters not read from the ROS param server is not shown anymore on start, only when the parameters dialog is opened.

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@783 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-01-29 16:04:54 +00:00
parent 0af3b84b8b
commit e64f5a291f
4 changed files with 31 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/msg/InfoEx.msg
+4
View File
@@ -16,6 +16,10 @@ float32[] posteriorValues
int32[] likelihoodKeys
float32[] likelihoodValues
# std::map<int, float> rawLikelihood;
int32[] rawLikelihoodKeys
float32[] rawLikelihoodValues
# std::map<int, int> weights;
int32[] weightsKeys
int32[] weightsValues
rtabmap/src/CoreWrapper.cpp
+2
View File
@@ -223,6 +223,8 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
msg->posteriorValues = uValues(stat.posterior());
msg->likelihoodKeys = uKeys(stat.likelihood());
msg->likelihoodValues = uValues(stat.likelihood());
msg->rawLikelihoodKeys = uKeys(stat.rawLikelihood());
msg->rawLikelihoodValues = uValues(stat.rawLikelihood());
msg->weightsKeys = uKeys(stat.weights());
msg->weightsValues = uValues(stat.weights());
rtabmap/src/GuiWrapper.cpp
+19 -13
View File
@@ -63,20 +63,20 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
//ROS_INFO("RTAB-Map info ex received!");
// Map from ROS struct to rtabmap struct
rtabmap::Statistics * stat = new rtabmap::Statistics();
rtabmap::Statistics stat;
stat->setExtended(true); // Extended
stat.setExtended(true); // Extended
stat->setRefImageId(msg->refId);
stat->setLoopClosureId(msg->loopClosureId);
stat.setRefImageId(msg->refId);
stat.setLoopClosureId(msg->loopClosureId);
if(msg->refImage.data.size())
{
stat->setRefImage(cv_bridge::toCvShare(msg->refImage, msg)->image.clone());
stat.setRefImage(cv_bridge::toCvShare(msg->refImage, msg)->image.clone());
}
if(msg->loopImage.data.size())
{
stat->setLoopImage(cv_bridge::toCvShare(msg->loopImage, msg)->image.clone());
stat.setLoopImage(cv_bridge::toCvShare(msg->loopImage, msg)->image.clone());
}
//Posterior, likelihood, childCount
@@ -85,19 +85,25 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
{
mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
}
stat->setPosterior(mapIntFloat);
stat.setPosterior(mapIntFloat);
mapIntFloat.clear();
for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
}
stat->setLikelihood(mapIntFloat);
stat.setLikelihood(mapIntFloat);
mapIntFloat.clear();
for(unsigned int i=0; i<msg->rawLikelihoodKeys.size() && i<msg->rawLikelihoodValues.size(); ++i)
{
mapIntFloat.insert(std::pair<int, float>(msg->rawLikelihoodKeys.at(i), msg->rawLikelihoodValues.at(i)));
}
stat.setRawLikelihood(mapIntFloat);
std::map<int, int> mapIntInt;
for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
{
mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
}
stat->setWeights(mapIntInt);
stat.setWeights(mapIntInt);
//SURF stuff...
std::multimap<int, cv::KeyPoint> mapIntKeypoint;
@@ -111,7 +117,7 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
pt.size = msg->refWordsValues.at(i).size;
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
}
stat->setRefWords(mapIntKeypoint);
stat.setRefWords(mapIntKeypoint);
mapIntKeypoint.clear();
for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); ++i)
{
@@ -123,16 +129,16 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
pt.size = msg->loopWordsValues.at(i).size;
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
}
stat->setLoopWords(mapIntKeypoint);
stat.setLoopWords(mapIntKeypoint);
// Statistics data
for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
{
stat->addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
stat.addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
}
//ROS_INFO("Publishing statistics...");
UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
UEventsManager::post(new rtabmap::RtabmapEvent(stat));
}
void GuiWrapper::handleEvent(UEvent * anEvent)
rtabmap/src/PreferencesDialogROS.cpp
+6 -3
View File
@@ -66,9 +66,12 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
else
{
validParameters = false;
QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
ROS_ERROR("%s", warning.toStdString().c_str());
QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
if(this->isVisible())
{
QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
ROS_WARN("%s", warning.toStdString().c_str());
QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
}
return false;
}
return true;