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

Updated OdomInfo msg to include inliers/outliers data, updated default launch files

Browse Source
This commit is contained in:
Mathieu Labbe
2015-01-23 15:18:06 -05:00
parent 92c30adad8
commit 4ae2cedc00
11 changed files with 251 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GuiWrapper.cpp
+92 -5
View File
@@ -61,7 +61,10 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
mainWindow_(0),
frameId_("base_link"),
waitForTransform_(false),
cameraNodeName_("")
cameraNodeName_(""),
depthScanSync_(0),
depthSync_(0),
depthOdomInfoSync_(0)
{
ros::NodeHandle nh;
app_ = new QApplication(argc, argv);
@@ -97,14 +100,16 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
// To receive odometry events
bool subscribeLaserScan = false;
bool subscribeDepth = false;
bool subscribeOdomInfo = false;
int queueSize = 10;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
this->setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
this->setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeOdomInfo, queueSize);
UEventsManager::addHandler(this);
UEventsManager::addHandler(mainWindow_);
@@ -117,6 +122,19 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
GuiWrapper::~GuiWrapper()
{
if(depthSync_)
{
delete depthSync_;
}
if(depthScanSync_)
{
delete depthScanSync_;
}
if(depthOdomInfoSync_)
{
delete depthOdomInfoSync_;
}
delete infoMapSync_;
delete mainWindow_;
delete app_;
}
@@ -364,7 +382,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
{
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
rtabmap::SensorData data;
rtabmap::SensorData data(cv::Mat(), odomMsg->header.seq);
data.setPose(odom, odomMsg->pose.covariance[0]);
this->post(new OdometryEvent(data));
}
@@ -417,10 +435,66 @@ void GuiWrapper::depthCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]);
odomMsg->pose.covariance[0],
odomMsg->header.seq);
this->post(new OdometryEvent(image));
}
void GuiWrapper::depthOdomInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
// TF ready?
Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
rtabmap::SensorData image(
ptrImage->image.clone(),
ptrDepth->image.clone(),
fx,
fy,
cx,
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0],
odomMsg->header.seq);
OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
this->post(new OdometryEvent(image, info));
}
void GuiWrapper::depthScanCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -480,13 +554,15 @@ void GuiWrapper::depthScanCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]);
odomMsg->pose.covariance[0],
odomMsg->header.seq);
this->post(new OdometryEvent(image));
}
void GuiWrapper::setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeOdomInfo,
int queueSize)
{
ros::NodeHandle nh; // public
@@ -511,6 +587,17 @@ void GuiWrapper::setupCallbacks(
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
}
else if(subscribeDepth && !subscribeLaserScan && subscribeOdomInfo)
{
ROS_INFO("Registering Depth callback...");
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
odomSub_.subscribe(nh, "odom", 1);
odomInfoSub_.subscribe(nh, "odom_info", 1);
depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), imageSub_, odomSub_, odomInfoSub_, imageDepthSub_, cameraInfoSub_);
depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
}
else if(subscribeDepth && !subscribeLaserScan)
{
ROS_INFO("Registering Depth callback...");
src/GuiWrapper.h
+17 -1
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include "rtabmap_ros/Info.h"
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/OdomInfo.h"
#include "rtabmap/utilite/UEventsHandler.h"
#include <tf/transform_listener.h>
@@ -71,12 +72,18 @@ protected:
private:
void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, int queueSize);
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom
void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void depthOdomInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
@@ -103,6 +110,7 @@ private:
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
typedef message_filters::sync_policies::ExactTime<
@@ -124,6 +132,14 @@ private:
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
rtabmap_ros::OdomInfo,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
};
#endif /* GUIWRAPPER_H_ */
src/MsgConversion.cpp
+48
View File
@@ -154,6 +154,25 @@ void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg)
msg.size = kpt.size;
}
std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg)
{
std::vector<cv::KeyPoint> v(msg.size());
for(unsigned int i=0; i<msg.size(); ++i)
{
v[i] = keypointFromROS(msg[i]);
}
return v;
}
void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg)
{
msg.resize(kpts.size());
for(unsigned int i=0; i<msg.size(); ++i)
{
keypointToROS(kpts[i], msg[i]);
}
}
void mapGraphFromROS(
const rtabmap_ros::Graph & msg,
std::map<int, rtabmap::Transform> & poses,
@@ -314,6 +333,22 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.localMapSize = msg.localMapSize;
info.time = msg.time;
info.variance = msg.variance;
info.type = msg.type;
UASSERT(msg.wordsKeys.size() == msg.wordsValues.size());
for(unsigned int i=0; i<msg.wordsKeys.size(); ++i)
{
info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
}
info.wordMatches = msg.wordMatches;
info.wordInliers = msg.wordInliers;
info.refCorners = keypointsFromROS(msg.refCorners);
info.newCorners = keypointsFromROS(msg.newCorners);
info.cornerInliers = msg.cornerInliers;
return info;
}
@@ -326,6 +361,19 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localMapSize = info.localMapSize;
msg.time = info.time;
msg.variance = info.variance;
msg.type = info.type;
msg.wordsKeys = uKeys(info.words);
keypointsToROS(uValues(info.words), msg.wordsValues);
msg.wordMatches = info.wordMatches;
msg.wordInliers = info.wordInliers;
keypointsToROS(info.refCorners, msg.refCorners);
keypointsToROS(info.newCorners, msg.newCorners);
msg.cornerInliers = info.cornerInliers;
}
}
src/PreferencesDialogROS.cpp
-1
View File
@@ -46,7 +46,6 @@ PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
PreferencesDialogROS::~PreferencesDialogROS()
{
ROS_INFO("rtabmapviz: GUI settings are saved to \"%s\"", configFile_.toStdString().c_str());
}
QString PreferencesDialogROS::getIniFilePath() const