Skip to content

Commit

Permalink
rtabmap: supporting RGB+camera_info only with in RGBD/Enabled=true an…
Browse files Browse the repository at this point in the history 
…d Mem/IncrementalMemory=false (used to localize using only RGB against a prebuilt map)
  • Loading branch information
@matlabbe
matlabbe committed Jul 10, 2017
1 parent 1a1919e commit e944786
Show file tree
Hide file tree
Showing 4 changed files with 179 additions and 55 deletions.
1 change: 1 addition & 0 deletions include/rtabmap_ros/CommonDataSubscriber.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ class CommonDataSubscriber {
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD();}
int getQueueSize() const {return queueSize_;}
bool isApproxSync() const {return approxSync_;}

protected:
void setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name);
Expand Down
7 changes: 7 additions & 0 deletions include/rtabmap_ros/CoreWrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,13 @@ class CoreWrapper : public CommonDataSubscriber, public nodelet::Nodelet
// for loop closure detection only
image_transport::Subscriber defaultSub_;

// for rgb/localization
image_transport::SubscriberFilter rgbSub_;
message_filters::Subscriber<nav_msgs::Odometry> rgbOdomSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> rgbCameraInfoSub_;
DATA_SYNCS2(rgb, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS3(rgbOdom, sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry);

ros::Subscriber userDataAsyncSub_;
cv::Mat userData_;

Expand Down
125 changes: 110 additions & 15 deletions src/CoreWrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,8 @@ CoreWrapper::CoreWrapper() :
mapToOdom_(rtabmap::Transform::getIdentity()),
transformThread_(0),
tfThreadRunning_(false),
SYNC_INIT(rgb),
SYNC_INIT(rgbOdom),
stereoToDepth_(false),
odomSensorSync_(false),
rate_(Parameters::defaultRtabmapDetectionRate()),
Expand Down Expand Up @@ -478,21 +480,84 @@ void CoreWrapper::onInit()
if(!this->isDataSubscribed())
{
bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
if(isRGBD)
{
NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
"parameter \"%s\" is true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
"to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure "
"detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str());
bool incremental = uStr2Bool(parameters_.at(Parameters::kMemIncrementalMemory()).c_str());
if(isRGBD && !incremental)
{
NODELET_INFO("\"%s\" is true and \"%s\" is false, subscribing to RGB + camera info...",
Parameters::kRGBDEnabled().c_str(),
Parameters::kMemIncrementalMemory().c_str());
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle rgb_pnh(pnh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
rgbCameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);

std::string odomFrameId;
pnh.getParam("odom_frame_id", odomFrameId);
if(!odomFrameId.empty())
{
// odom from TF
if(isApproxSync())
{
rgbApproximateSync_ = new message_filters::Synchronizer<rgbApproximateSyncPolicy>(
rgbApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
rgbApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
}
else
{
rgbExactSync_ = new message_filters::Synchronizer<rgbExactSyncPolicy>(
rgbExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
rgbExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
}
NODELET_INFO("\n%s subscribed to:\n %s\n %s",
getName().c_str(),
rgbSub_.getTopic().c_str(),
rgbCameraInfoSub_.getTopic().c_str());
}
else
{
rgbOdomSub_.subscribe(nh, "odom", 1);
if(isApproxSync())
{
rgbOdomApproximateSync_ = new message_filters::Synchronizer<rgbOdomApproximateSyncPolicy>(
rgbOdomApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
rgbOdomApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
}
else
{
rgbOdomExactSync_ = new message_filters::Synchronizer<rgbOdomExactSyncPolicy>(
rgbOdomExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
rgbOdomExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
}
NODELET_INFO("\n%s subscribed to:\n %s\n %s\n %s",
getName().c_str(),
rgbSub_.getTopic().c_str(),
rgbCameraInfoSub_.getTopic().c_str(),
rgbOdomSub_.getTopic().c_str());
}
}
else
{
if(isRGBD)
{
NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
"parameter \"%s\" and \"%s\" are true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
"to true to use rtabmap node for RGB-D SLAM, set \"%s\" to false for loop closure "
"detection on images-only or set \"%s\" to false to localize a single RGB camera against pre-built 3D map.",
Parameters::kRGBDEnabled().c_str(),
Parameters::kMemIncrementalMemory().c_str(),
Parameters::kRGBDEnabled().c_str(),
Parameters::kMemIncrementalMemory().c_str());
}
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle rgb_pnh(pnh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);

ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle rgb_pnh(pnh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);

NODELET_INFO("\n%s subscribed to:\n %s", getName().c_str(), defaultSub_.getTopic().c_str());
NODELET_INFO("\n%s subscribed to:\n %s", getName().c_str(), defaultSub_.getTopic().c_str());
}
}

userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
Expand All @@ -508,6 +573,9 @@ CoreWrapper::~CoreWrapper()
delete transformThread_;
}

SYNC_DEL(rgb);
SYNC_DEL(rgbOdom);

this->saveParameters(configPath_);

ros::NodeHandle nh;
Expand Down Expand Up @@ -636,6 +704,33 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
}
}


void CoreWrapper::rgbCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}

void CoreWrapper::rgbOdomCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}

bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
{
if(!paused_)
Expand Down Expand Up @@ -847,7 +942,7 @@ void CoreWrapper::commonDepthCallbackImpl(
}

UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format()));
if(depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
if(!depth.empty() && depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
{
depth = rtabmap::util2d::cvtDepthFromFloat(depth);
static bool shown = false;
Expand All @@ -865,7 +960,7 @@ void CoreWrapper::commonDepthCallbackImpl(
Transform scanLocalTransform = Transform::getIdentity();
pcl::PointCloud<pcl::PointXYZ> scanCloud2d;
bool genMaxScanPts = 0;
if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && genScan_)
if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && !depth.empty() && genScan_)
{
scanCloud2d = util3d::laserScanFromDepthImages(
depth,
Expand Down
101 changes: 61 additions & 40 deletions src/MsgConversion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1106,18 +1106,21 @@ bool convertRGBDMsgs(
double waitForTransform)
{
UASSERT(imageMsgs.size()>0 &&
imageMsgs.size() == depthMsgs.size() &&
(imageMsgs.size() == depthMsgs.size() || depthMsgs.empty()) &&
imageMsgs.size() == cameraInfoMsgs.size());

int imageWidth = imageMsgs[0]->image.cols;
int imageHeight = imageMsgs[0]->image.rows;
int depthWidth = depthMsgs[0]->image.cols;
int depthHeight = depthMsgs[0]->image.rows;
int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0;
int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0;

UASSERT_MSG(
imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
if(depthMsgs.size())
{
UASSERT_MSG(
imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
}

int cameraCount = imageMsgs.size();
for(unsigned int i=0; i<imageMsgs.size(); ++i)
Expand All @@ -1129,14 +1132,19 @@ bool convertRGBDMsgs(
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
imageMsgs[i]->encoding.c_str(),

ROS_ERROR("Input rgb type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb=%s",
imageMsgs[i]->encoding.c_str());
return false;
}
if(depthMsgs.size() &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
ROS_ERROR("Input depth type must be image_depth=32FC1,16UC1,mono16. Current depth=%s",
depthMsgs[i]->encoding.c_str());
return false;
}
Expand All @@ -1147,38 +1155,47 @@ bool convertRGBDMsgs(
imageMsgs[i]->image.cols,
imageHeight,
imageMsgs[i]->image.rows).c_str());
UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
depthWidth,
depthMsgs[i]->image.cols,
depthHeight,
depthMsgs[i]->image.rows).c_str());
ros::Time stamp;
if(depthMsgs.size())
{
UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
depthWidth,
depthMsgs[i]->image.cols,
depthHeight,
depthMsgs[i]->image.rows).c_str());
stamp = depthMsgs[i]->header.stamp;
}
else
{
stamp = imageMsgs[i]->header.stamp;
}

// use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb)
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, imageMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform);
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, imageMsgs[i]->header.frame_id, stamp, listener, waitForTransform);
if(localTransform.isNull())
{
ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, depthMsgs[i]->header.stamp.toSec());
ROS_ERROR("TF of received image %d at time %fs is not set!", i, stamp.toSec());
return false;
}
// sync with odometry stamp
if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp)
if(!odomFrameId.empty() && odomStamp != stamp)
{
rtabmap::Transform sensorT = getTransform(
frameId,
odomFrameId,
odomStamp,
depthMsgs[i]->header.stamp,
stamp,
listener,
waitForTransform);
if(sensorT.isNull())
{
ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
"stamp is %fs. The depth image pose will not be synchronized with odometry.", depthMsgs[i]->header.stamp.toSec(), odomStamp.toSec());
"stamp is %fs. The depth image pose will not be synchronized with odometry.", stamp.toSec(), odomStamp.toSec());
}
else
{
//ROS_WARN("RGBD correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(depthMsgs[i]->header.stamp.toSec()-odomStamp.toSec()));
//ROS_WARN("RGBD correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(stamp.toSec()-odomStamp.toSec()));
localTransform = sensorT * localTransform;
}
}
Expand All @@ -1198,19 +1215,12 @@ bool convertRGBDMsgs(
{
ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;

// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
}

if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
Expand All @@ -1221,14 +1231,25 @@ bool convertRGBDMsgs(
return false;
}

if(subDepth.type() == depth.type())
if(depthMsgs.size())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
ROS_ERROR("Some Depth images are not the same type!");
return false;
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;

if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
}

if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
ROS_ERROR("Some Depth images are not the same type!");
return false;
}
}

cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
Expand Down

0 comments on commit e944786

Please sign in to comment.