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

rtabmap-pkg: Updated rtabmap interface

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@804 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-02-03 22:12:44 +00:00
parent 61ae26327f
commit c362dc628c
2 changed files with 102 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/src/CoreWrapper.cpp
+99 -105
View File
@@ -50,8 +50,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
image_transport::ImageTransport it(nh); image_transport::ImageTransport it(nh);
imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this); imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
UEventsManager::addHandler(this);
} }
CoreWrapper::~CoreWrapper() CoreWrapper::~CoreWrapper()
@@ -112,6 +110,8 @@ void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
//ROS_INFO("Received image."); //ROS_INFO("Received image.");
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
rtabmap_->process(ptr->image.clone(), ptr->header.seq); rtabmap_->process(ptr->image.clone(), ptr->header.seq);
const Statistics & stats = rtabmap_->getStatistics();
this->publishStats(stats);
} }
} }
@@ -152,123 +152,117 @@ void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
} }
} }
ROS_INFO("Updating parameters"); ROS_INFO("Updating parameters");
rtabmap_->updateParameters(parameters); rtabmap_->parseParameters(parameters);
} }
void CoreWrapper::handleEvent(UEvent * anEvent) void CoreWrapper::publishStats(const Statistics & stats)
{ {
if(anEvent->getClassName().compare("RtabmapEvent") == 0) if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers())
{ {
if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers()) if(infoPub_.getNumSubscribers())
{ {
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent; //ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
const Statistics & stat = rtabmapEvent->getStats(); rtabmap::InfoPtr msg(new rtabmap::Info);
msg->refId = stats.refImageId();
msg->loopClosureId = stats.loopClosureId();
infoPub_.publish(msg);
}
if(infoPub_.getNumSubscribers()) if(infoPubEx_.getNumSubscribers())
{
//ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId());
rtabmap::InfoExPtr msg(new rtabmap::InfoEx);
msg->refId = stats.refImageId();
msg->loopClosureId = stats.loopClosureId();
// Detailed info
if(stats.extended())
{ {
//ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId()); if(!stats.refImage().empty())
rtabmap::InfoPtr msg(new rtabmap::Info);
msg->refId = stat.refImageId();
msg->loopClosureId = stat.loopClosureId();
infoPub_.publish(msg);
}
if(infoPubEx_.getNumSubscribers())
{
//ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId());
rtabmap::InfoExPtr msg(new rtabmap::InfoEx);
msg->refId = stat.refImageId();
msg->loopClosureId = stat.loopClosureId();
// Detailed info
if(stat.extended())
{ {
if(!stat.refImage().empty()) cv_bridge::CvImage img;
if(stats.refImage().channels() == 1)
{ {
cv_bridge::CvImage img; img.encoding = sensor_msgs::image_encodings::MONO8;
if(stat.refImage().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
else
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = stat.refImage();
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header.frame_id = "camera";
rosMsg->header.stamp = ros::Time::now();
msg->refImage = *rosMsg;
} }
if(!stat.loopImage().empty()) else
{ {
cv_bridge::CvImage img; img.encoding = sensor_msgs::image_encodings::BGR8;
if(stat.loopImage().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
else
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = stat.loopImage();
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header.frame_id = "camera";
rosMsg->header.stamp = ros::Time::now();
msg->loopImage = *rosMsg;
} }
img.image = stats.refImage();
//Posterior, likelihood, childCount sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
msg->posteriorKeys = uKeys(stat.posterior()); rosMsg->header.frame_id = "camera";
msg->posteriorValues = uValues(stat.posterior()); rosMsg->header.stamp = ros::Time::now();
msg->likelihoodKeys = uKeys(stat.likelihood()); msg->refImage = *rosMsg;
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());
//Features stuff...
msg->refWordsKeys = uKeys(stat.refWords());
msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
int index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
i!=stat.refWords().end();
++i)
{
msg->refWordsValues.at(index).angle = i->second.angle;
msg->refWordsValues.at(index).response = i->second.response;
msg->refWordsValues.at(index).ptx = i->second.pt.x;
msg->refWordsValues.at(index).pty = i->second.pt.y;
msg->refWordsValues.at(index).size = i->second.size;
msg->refWordsValues.at(index).octave = i->second.octave;
msg->refWordsValues.at(index).class_id = i->second.class_id;
++index;
}
msg->loopWordsKeys = uKeys(stat.loopWords());
msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
i!=stat.loopWords().end();
++i)
{
msg->loopWordsValues.at(index).angle = i->second.angle;
msg->loopWordsValues.at(index).response = i->second.response;
msg->loopWordsValues.at(index).ptx = i->second.pt.x;
msg->loopWordsValues.at(index).pty = i->second.pt.y;
msg->loopWordsValues.at(index).size = i->second.size;
msg->loopWordsValues.at(index).octave = i->second.octave;
msg->loopWordsValues.at(index).class_id = i->second.class_id;
++index;
}
// Statistics data
msg->statsKeys = uKeys(stat.data());
msg->statsValues = uValues(stat.data());
} }
infoPubEx_.publish(msg); if(!stats.loopImage().empty())
{
cv_bridge::CvImage img;
if(stats.loopImage().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
else
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = stats.loopImage();
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header.frame_id = "camera";
rosMsg->header.stamp = ros::Time::now();
msg->loopImage = *rosMsg;
}
//Posterior, likelihood, childCount
msg->posteriorKeys = uKeys(stats.posterior());
msg->posteriorValues = uValues(stats.posterior());
msg->likelihoodKeys = uKeys(stats.likelihood());
msg->likelihoodValues = uValues(stats.likelihood());
msg->rawLikelihoodKeys = uKeys(stats.rawLikelihood());
msg->rawLikelihoodValues = uValues(stats.rawLikelihood());
msg->weightsKeys = uKeys(stats.weights());
msg->weightsValues = uValues(stats.weights());
//Features stuff...
msg->refWordsKeys = uKeys(stats.refWords());
msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stats.refWords().size());
int index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stats.refWords().begin();
i!=stats.refWords().end();
++i)
{
msg->refWordsValues.at(index).angle = i->second.angle;
msg->refWordsValues.at(index).response = i->second.response;
msg->refWordsValues.at(index).ptx = i->second.pt.x;
msg->refWordsValues.at(index).pty = i->second.pt.y;
msg->refWordsValues.at(index).size = i->second.size;
msg->refWordsValues.at(index).octave = i->second.octave;
msg->refWordsValues.at(index).class_id = i->second.class_id;
++index;
}
msg->loopWordsKeys = uKeys(stats.loopWords());
msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stats.loopWords().size());
index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stats.loopWords().begin();
i!=stats.loopWords().end();
++i)
{
msg->loopWordsValues.at(index).angle = i->second.angle;
msg->loopWordsValues.at(index).response = i->second.response;
msg->loopWordsValues.at(index).ptx = i->second.pt.x;
msg->loopWordsValues.at(index).pty = i->second.pt.y;
msg->loopWordsValues.at(index).size = i->second.size;
msg->loopWordsValues.at(index).octave = i->second.octave;
msg->loopWordsValues.at(index).class_id = i->second.class_id;
++index;
}
// Statistics data
msg->statsKeys = uKeys(stats.data());
msg->statsValues = uValues(stats.data());
} }
infoPubEx_.publish(msg);
} }
} }
} }
rtabmap/src/CoreWrapper.h
+3 -4
View File
@@ -13,8 +13,7 @@
#include <std_srvs/Empty.h> #include <std_srvs/Empty.h>
#include <std_msgs/Empty.h> #include <std_msgs/Empty.h>
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
#include "utilite/UEventsHandler.h" #include <rtabmap/core/Statistics.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <image_transport/image_transport.h> #include <image_transport/image_transport.h>
@@ -24,7 +23,7 @@ namespace rtabmap
class Rtabmap; class Rtabmap;
} }
class CoreWrapper : public UEventsHandler class CoreWrapper
{ {
public: public:
CoreWrapper(bool deleteDbOnStart = false); CoreWrapper(bool deleteDbOnStart = false);
@@ -43,7 +42,7 @@ private:
void loadNodeParameters(const std::string & configFile); void loadNodeParameters(const std::string & configFile);
void saveNodeParameters(const std::string & configFile); void saveNodeParameters(const std::string & configFile);
virtual void handleEvent(UEvent * anEvent); void publishStats(const rtabmap::Statistics & stats);
private: private:
rtabmap::Rtabmap * rtabmap_; rtabmap::Rtabmap * rtabmap_;