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 RGBDImage msg with depth camera info, also made changes so that RGBDImage can be used also to sync stereo images.

Browse Source
This commit is contained in:
matlabbe
2018-02-13 21:35:15 -05:00
parent 6b14693a95
commit 438fad643c
24 changed files with 384 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/rgbd_sync.cpp
+28 -16
View File
@@ -101,13 +101,13 @@ private:
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4));
}
ros::NodeHandle rgb_nh(nh, "rgb");
@@ -122,13 +122,15 @@ private:
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);
cameraDepthInfoSub_.subscribe(depth_nh, "camera_info", 1);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
getName().c_str(),
approxSync?"approx":"exact",
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
cameraInfoSub_.getTopic().c_str(),
cameraDepthInfoSub_.getTopic().c_str());
warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
@@ -156,7 +158,8 @@ private:
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
const sensor_msgs::CameraInfoConstPtr& cameraInfo,
const sensor_msgs::CameraInfoConstPtr& cameraDepthInfo)
{
callbackCalled_ = true;
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
@@ -164,7 +167,8 @@ private:
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfo->header.frame_id;
msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
msg.cameraInfo = *cameraInfo;
msg.rgbCameraInfo = *cameraInfo;
msg.depthCameraInfo = *cameraDepthInfo;
if(rgbdImageCompressedPub_.getNumSubscribers())
{
@@ -174,17 +178,24 @@ private:
imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
ROS_ASSERT(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1);
msgCompressed.depthCompressed.header = imageDepthPtr->header;
if(depthScale_ != 1.0)
if(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1)
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
msgCompressed.depthCompressed.header = imageDepthPtr->header;
if(depthScale_ != 1.0)
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
}
else
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
}
msgCompressed.depthCompressed.format = "png";
}
else
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
// Assume right stereo image
imageDepthPtr->toCompressedImageMsg(msgCompressed.depthCompressed, cv_bridge::JPG);
}
msgCompressed.depthCompressed.format = "png";
rgbdImageCompressedPub_.publish(msgCompressed);
}
@@ -218,11 +229,12 @@ private:
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraDepthInfoSub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
};