From 5569d0403b020bc9d37a5e4ca689b50c97111309 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timo=20R=C3=B6hling?= Date: Fri, 14 Sep 2018 10:04:55 +0200 Subject: [PATCH] Add option to use the RGB image timestamp for the registered depth image For some applications, it is beneficial that the registered depth image adopts the time stamp from the RGB image, e.g. if one wants to pair them with ExactTime policy afterwards. It also makes sense from a theoretical point of view, since the registration aligns the frames both spatially and temporally. The new behavior can be enabled on a opt-in basis by setting the parameter "use_rgb_timestamp". --- depth_image_proc/src/nodelets/register.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/depth_image_proc/src/nodelets/register.cpp b/depth_image_proc/src/nodelets/register.cpp index 2c3753384..412feb34b 100644 --- a/depth_image_proc/src/nodelets/register.cpp +++ b/depth_image_proc/src/nodelets/register.cpp @@ -73,6 +73,7 @@ class RegisterNodelet : public nodelet::Nodelet // Parameters bool fill_upsampling_holes_; // fills holes which occur due to upsampling by scaling each pixel to the target image scale (only takes effect on upsampling) + bool use_rgb_timestamp_; // use source time stamp from RGB camera virtual void onInit(); @@ -102,6 +103,7 @@ void RegisterNodelet::onInit() int queue_size; private_nh.param("queue_size", queue_size, 5); private_nh.param("fill_upsampling_holes", fill_upsampling_holes_, false); + private_nh.param("use_rgb_timestamp", use_rgb_timestamp_, false); // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image_, sub_depth_info_, sub_rgb_info_) ); @@ -165,7 +167,7 @@ void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg, // Allocate registered depth image sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image ); - registered_msg->header.stamp = depth_image_msg->header.stamp; + registered_msg->header.stamp = use_rgb_timestamp_ ? rgb_info_msg->header.stamp : depth_image_msg->header.stamp; registered_msg->header.frame_id = rgb_info_msg->header.frame_id; registered_msg->encoding = depth_image_msg->encoding;