From fcd02da1a41124a1a3f9c9ca16768799f3a00f62 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 24 Jun 2026 17:43:02 +0700 Subject: [PATCH] first commit --- src/convert_metric.cpp | 140 ------------------ src/crop_foremost.cpp | 142 ------------------- src/disparity.cpp | 189 ------------------------- src/register.cpp | 313 ----------------------------------------- 4 files changed, 784 deletions(-) delete mode 100644 src/convert_metric.cpp delete mode 100755 src/crop_foremost.cpp delete mode 100644 src/disparity.cpp delete mode 100644 src/register.cpp diff --git a/src/convert_metric.cpp b/src/convert_metric.cpp deleted file mode 100644 index 22552ae..0000000 --- a/src/convert_metric.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ -#include -#include -#include -#include -#include - -namespace depth_image_proc { - -namespace enc = sensor_msgs::image_encodings; - -class ConvertMetricNodelet : public nodelet::Nodelet -{ - // Subscriptions - boost::shared_ptr it_; - image_transport::Subscriber sub_raw_; - - // Publications - boost::mutex connect_mutex_; - image_transport::Publisher pub_depth_; - - virtual void onInit(); - - void connectCb(); - - void depthCb(const sensor_msgs::ImageConstPtr& raw_msg); -}; - -void ConvertMetricNodelet::onInit() -{ - ros::NodeHandle& nh = getNodeHandle(); - it_.reset(new image_transport::ImageTransport(nh)); - - // Monitor whether anyone is subscribed to the output - image_transport::SubscriberStatusCallback connect_cb = boost::bind(&ConvertMetricNodelet::connectCb, this); - // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ - boost::lock_guard lock(connect_mutex_); - pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb); -} - -// Handles (un)subscribing when clients (un)subscribe -void ConvertMetricNodelet::connectCb() -{ - boost::lock_guard lock(connect_mutex_); - if (pub_depth_.getNumSubscribers() == 0) - { - sub_raw_.shutdown(); - } - else if (!sub_raw_) - { - image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_raw_ = it_->subscribe("image_raw", 1, &ConvertMetricNodelet::depthCb, this, hints); - } -} - -void ConvertMetricNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg) -{ - // Allocate new Image message - sensor_msgs::ImagePtr depth_msg( new sensor_msgs::Image ); - depth_msg->header = raw_msg->header; - depth_msg->height = raw_msg->height; - depth_msg->width = raw_msg->width; - - // Set data, encoding and step after converting the metric. - if (raw_msg->encoding == enc::TYPE_16UC1) - { - depth_msg->encoding = enc::TYPE_32FC1; - depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8); - depth_msg->data.resize(depth_msg->height * depth_msg->step); - // Fill in the depth image data, converting mm to m - float bad_point = std::numeric_limits::quiet_NaN (); - const uint16_t* raw_data = reinterpret_cast(&raw_msg->data[0]); - float* depth_data = reinterpret_cast(&depth_msg->data[0]); - for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index) - { - uint16_t raw = raw_data[index]; - depth_data[index] = (raw == 0) ? bad_point : (float)raw * 0.001f; - } - } - else if (raw_msg->encoding == enc::TYPE_32FC1) - { - depth_msg->encoding = enc::TYPE_16UC1; - depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8); - depth_msg->data.resize(depth_msg->height * depth_msg->step); - // Fill in the depth image data, converting m to mm - uint16_t bad_point = 0; - const float* raw_data = reinterpret_cast(&raw_msg->data[0]); - uint16_t* depth_data = reinterpret_cast(&depth_msg->data[0]); - for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index) - { - float raw = raw_data[index]; - depth_data[index] = std::isnan(raw) ? bad_point : (uint16_t)(raw * 1000); - } - } - else - { - ROS_ERROR("Unsupported image conversion from %s.", raw_msg->encoding.c_str()); - return; - } - - pub_depth_.publish(depth_msg); -} - -} // namespace depth_image_proc - -// Register as nodelet -#include -PLUGINLIB_EXPORT_CLASS(depth_image_proc::ConvertMetricNodelet,nodelet::Nodelet); diff --git a/src/crop_foremost.cpp b/src/crop_foremost.cpp deleted file mode 100755 index f4cbac8..0000000 --- a/src/crop_foremost.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ -//#include -#include -#include -#include -#include -#include - -namespace depth_image_proc { - -namespace enc = sensor_msgs::image_encodings; - -class CropForemostNodelet : public nodelet::Nodelet -{ - // Subscriptions - boost::shared_ptr it_; - image_transport::Subscriber sub_raw_; - - // Publications - boost::mutex connect_mutex_; - image_transport::Publisher pub_depth_; - - virtual void onInit(); - - void connectCb(); - - void depthCb(const sensor_msgs::ImageConstPtr& raw_msg); - - double distance_; -}; - -void CropForemostNodelet::onInit() -{ - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& private_nh = getPrivateNodeHandle(); - private_nh.getParam("distance", distance_); - it_.reset(new image_transport::ImageTransport(nh)); - - // Monitor whether anyone is subscribed to the output - image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropForemostNodelet::connectCb, this); - // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ - boost::lock_guard lock(connect_mutex_); - pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb); -} - -// Handles (un)subscribing when clients (un)subscribe -void CropForemostNodelet::connectCb() -{ - boost::lock_guard lock(connect_mutex_); - if (pub_depth_.getNumSubscribers() == 0) - { - sub_raw_.shutdown(); - } - else if (!sub_raw_) - { - image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_raw_ = it_->subscribe("image_raw", 1, &CropForemostNodelet::depthCb, this, hints); - } -} - -void CropForemostNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg) -{ - cv_bridge::CvImagePtr cv_ptr; - try - { - cv_ptr = cv_bridge::toCvCopy(raw_msg); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - // Check the number of channels - if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){ - NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str()); - return; - } - - // search the min value without invalid value "0" - double minVal; - cv::minMaxIdx(cv_ptr->image, &minVal, 0, 0, 0, cv_ptr->image != 0); - - int imtype = cv_bridge::getCvType(raw_msg->encoding); - switch (imtype){ - case CV_8UC1: - case CV_8SC1: - case CV_32F: - cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 0, CV_THRESH_TOZERO_INV); - break; - case CV_16UC1: - case CV_16SC1: - case CV_32SC1: - case CV_64F: - // 8 bit or 32 bit floating array is required to use cv::threshold - cv_ptr->image.convertTo(cv_ptr->image, CV_32F); - cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 1, CV_THRESH_TOZERO_INV); - - cv_ptr->image.convertTo(cv_ptr->image, imtype); - break; - } - - pub_depth_.publish(cv_ptr->toImageMsg()); -} - -} // namespace depth_image_proc - -// Register as nodelet -#include -PLUGINLIB_EXPORT_CLASS(depth_image_proc::CropForemostNodelet,nodelet::Nodelet); diff --git a/src/disparity.cpp b/src/disparity.cpp deleted file mode 100644 index 83b0cf4..0000000 --- a/src/disparity.cpp +++ /dev/null @@ -1,189 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ -#include -#if ((BOOST_VERSION / 100) % 1000) >= 53 -#include -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace depth_image_proc { - -namespace enc = sensor_msgs::image_encodings; - -class DisparityNodelet : public nodelet::Nodelet -{ - boost::shared_ptr left_it_; - ros::NodeHandlePtr right_nh_; - image_transport::SubscriberFilter sub_depth_image_; - message_filters::Subscriber sub_info_; - typedef message_filters::TimeSynchronizer Sync; - boost::shared_ptr sync_; - - boost::mutex connect_mutex_; - ros::Publisher pub_disparity_; - double min_range_; - double max_range_; - double delta_d_; - - virtual void onInit(); - - void connectCb(); - - void depthCb(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg); - - template - void convert(const sensor_msgs::ImageConstPtr& depth_msg, - stereo_msgs::DisparityImagePtr& disp_msg); -}; - -void DisparityNodelet::onInit() -{ - ros::NodeHandle &nh = getNodeHandle(); - ros::NodeHandle &private_nh = getPrivateNodeHandle(); - ros::NodeHandle left_nh(nh, "left"); - left_it_.reset(new image_transport::ImageTransport(left_nh)); - right_nh_.reset( new ros::NodeHandle(nh, "right") ); - - // Read parameters - int queue_size; - private_nh.param("queue_size", queue_size, 5); - private_nh.param("min_range", min_range_, 0.0); - private_nh.param("max_range", max_range_, std::numeric_limits::infinity()); - private_nh.param("delta_d", delta_d_, 0.125); - - // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. - sync_.reset( new Sync(sub_depth_image_, sub_info_, queue_size) ); - sync_->registerCallback(boost::bind(&DisparityNodelet::depthCb, this, boost::placeholders::_1, boost::placeholders::_2)); - - // Monitor whether anyone is subscribed to the output - ros::SubscriberStatusCallback connect_cb = boost::bind(&DisparityNodelet::connectCb, this); - // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_ - boost::lock_guard lock(connect_mutex_); - pub_disparity_ = left_nh.advertise("disparity", 1, connect_cb, connect_cb); -} - -// Handles (un)subscribing when clients (un)subscribe -void DisparityNodelet::connectCb() -{ - boost::lock_guard lock(connect_mutex_); - if (pub_disparity_.getNumSubscribers() == 0) - { - sub_depth_image_.unsubscribe(); - sub_info_ .unsubscribe(); - } - else if (!sub_depth_image_.getSubscriber()) - { - image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_depth_image_.subscribe(*left_it_, "image_rect", 1, hints); - sub_info_.subscribe(*right_nh_, "camera_info", 1); - } -} - -void DisparityNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg) -{ - // Allocate new DisparityImage message - stereo_msgs::DisparityImagePtr disp_msg( new stereo_msgs::DisparityImage ); - disp_msg->header = depth_msg->header; - disp_msg->image.header = disp_msg->header; - disp_msg->image.encoding = enc::TYPE_32FC1; - disp_msg->image.height = depth_msg->height; - disp_msg->image.width = depth_msg->width; - disp_msg->image.step = disp_msg->image.width * sizeof (float); - disp_msg->image.data.resize( disp_msg->image.height * disp_msg->image.step, 0.0f ); - double fx = info_msg->P[0]; - disp_msg->T = -info_msg->P[3] / fx; - disp_msg->f = fx; - // Remaining fields depend on device characteristics, so rely on user input - disp_msg->min_disparity = disp_msg->f * disp_msg->T / max_range_; - disp_msg->max_disparity = disp_msg->f * disp_msg->T / min_range_; - disp_msg->delta_d = delta_d_; - - if (depth_msg->encoding == enc::TYPE_16UC1) - { - convert(depth_msg, disp_msg); - } - else if (depth_msg->encoding == enc::TYPE_32FC1) - { - convert(depth_msg, disp_msg); - } - else - { - NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); - return; - } - - pub_disparity_.publish(disp_msg); -} - -template -void DisparityNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, - stereo_msgs::DisparityImagePtr& disp_msg) -{ - // For each depth Z, disparity d = fT / Z - float unit_scaling = DepthTraits::toMeters( T(1) ); - float constant = disp_msg->f * disp_msg->T / unit_scaling; - - const T* depth_row = reinterpret_cast(&depth_msg->data[0]); - int row_step = depth_msg->step / sizeof(T); - float* disp_data = reinterpret_cast(&disp_msg->image.data[0]); - for (int v = 0; v < (int)depth_msg->height; ++v) - { - for (int u = 0; u < (int)depth_msg->width; ++u) - { - T depth = depth_row[u]; - if (DepthTraits::valid(depth)) - *disp_data = constant / depth; - ++disp_data; - } - - depth_row += row_step; - } -} - -} // namespace depth_image_proc - -// Register as nodelet -#include -PLUGINLIB_EXPORT_CLASS(depth_image_proc::DisparityNodelet,nodelet::Nodelet); diff --git a/src/register.cpp b/src/register.cpp deleted file mode 100644 index 412feb3..0000000 --- a/src/register.cpp +++ /dev/null @@ -1,313 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace depth_image_proc { - -using namespace message_filters::sync_policies; -namespace enc = sensor_msgs::image_encodings; - -class RegisterNodelet : public nodelet::Nodelet -{ - ros::NodeHandlePtr nh_depth_, nh_rgb_; - boost::shared_ptr it_depth_; - - // Subscriptions - image_transport::SubscriberFilter sub_depth_image_; - message_filters::Subscriber sub_depth_info_, sub_rgb_info_; - boost::shared_ptr tf_buffer_; - boost::shared_ptr tf_; - typedef ApproximateTime SyncPolicy; - typedef message_filters::Synchronizer Synchronizer; - boost::shared_ptr sync_; - - // Publications - boost::mutex connect_mutex_; - image_transport::CameraPublisher pub_registered_; - - image_geometry::PinholeCameraModel depth_model_, rgb_model_; - - // 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(); - - void connectCb(); - - void imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg, - const sensor_msgs::CameraInfoConstPtr& depth_info_msg, - const sensor_msgs::CameraInfoConstPtr& rgb_info_msg); - - template - void convert(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::ImagePtr& registered_msg, - const Eigen::Affine3d& depth_to_rgb); -}; - -void RegisterNodelet::onInit() -{ - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& private_nh = getPrivateNodeHandle(); - nh_depth_.reset( new ros::NodeHandle(nh, "depth") ); - nh_rgb_.reset( new ros::NodeHandle(nh, "rgb") ); - it_depth_.reset( new image_transport::ImageTransport(*nh_depth_) ); - tf_buffer_.reset( new tf2_ros::Buffer ); - tf_.reset( new tf2_ros::TransformListener(*tf_buffer_) ); - - // Read parameters - 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_) ); - sync_->registerCallback(boost::bind(&RegisterNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - - // Monitor whether anyone is subscribed to the output - image_transport::ImageTransport it_depth_reg(ros::NodeHandle(nh, "depth_registered")); - image_transport::SubscriberStatusCallback image_connect_cb = boost::bind(&RegisterNodelet::connectCb, this); - ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNodelet::connectCb, this); - // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_ - boost::lock_guard lock(connect_mutex_); - pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1, - image_connect_cb, image_connect_cb, - info_connect_cb, info_connect_cb); -} - -// Handles (un)subscribing when clients (un)subscribe -void RegisterNodelet::connectCb() -{ - boost::lock_guard lock(connect_mutex_); - if (pub_registered_.getNumSubscribers() == 0) - { - sub_depth_image_.unsubscribe(); - sub_depth_info_ .unsubscribe(); - sub_rgb_info_ .unsubscribe(); - } - else if (!sub_depth_image_.getSubscriber()) - { - image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_depth_image_.subscribe(*it_depth_, "image_rect", 1, hints); - sub_depth_info_ .subscribe(*nh_depth_, "camera_info", 1); - sub_rgb_info_ .subscribe(*nh_rgb_, "camera_info", 1); - } -} - -void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg, - const sensor_msgs::CameraInfoConstPtr& depth_info_msg, - const sensor_msgs::CameraInfoConstPtr& rgb_info_msg) -{ - // Update camera models - these take binning & ROI into account - depth_model_.fromCameraInfo(depth_info_msg); - rgb_model_ .fromCameraInfo(rgb_info_msg); - - // Query tf2 for transform from (X,Y,Z) in depth camera frame to RGB camera frame - Eigen::Affine3d depth_to_rgb; - try - { - geometry_msgs::TransformStamped transform = tf_buffer_->lookupTransform ( - rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id, - depth_info_msg->header.stamp); - - tf::transformMsgToEigen(transform.transform, depth_to_rgb); - } - catch (tf2::TransformException& ex) - { - NODELET_WARN_THROTTLE(2, "TF2 exception:\n%s", ex.what()); - return; - /// @todo Can take on order of a minute to register a disconnect callback when we - /// don't call publish() in this cb. What's going on roscpp? - } - - // Allocate registered depth image - sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image ); - 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; - - cv::Size resolution = rgb_model_.reducedResolution(); - registered_msg->height = resolution.height; - registered_msg->width = resolution.width; - // step and data set in convert(), depend on depth data type - - if (depth_image_msg->encoding == enc::TYPE_16UC1) - { - convert(depth_image_msg, registered_msg, depth_to_rgb); - } - else if (depth_image_msg->encoding == enc::TYPE_32FC1) - { - convert(depth_image_msg, registered_msg, depth_to_rgb); - } - else - { - NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image_msg->encoding.c_str()); - return; - } - - // Registered camera info is the same as the RGB info, but uses the depth timestamp - sensor_msgs::CameraInfoPtr registered_info_msg( new sensor_msgs::CameraInfo(*rgb_info_msg) ); - registered_info_msg->header.stamp = registered_msg->header.stamp; - - pub_registered_.publish(registered_msg, registered_info_msg); -} - -template -void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::ImagePtr& registered_msg, - const Eigen::Affine3d& depth_to_rgb) -{ - // Allocate memory for registered depth image - registered_msg->step = registered_msg->width * sizeof(T); - registered_msg->data.resize( registered_msg->height * registered_msg->step ); - // data is already zero-filled in the uint16 case, but for floats we want to initialize everything to NaN. - DepthTraits::initializeBuffer(registered_msg->data); - - // Extract all the parameters we need - double inv_depth_fx = 1.0 / depth_model_.fx(); - double inv_depth_fy = 1.0 / depth_model_.fy(); - double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy(); - double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty(); - double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy(); - double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy(); - double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty(); - - // Transform the depth values into the RGB frame - /// @todo When RGB is higher res, interpolate by rasterizing depth triangles onto the registered image - const T* depth_row = reinterpret_cast(&depth_msg->data[0]); - int row_step = depth_msg->step / sizeof(T); - T* registered_data = reinterpret_cast(®istered_msg->data[0]); - int raw_index = 0; - for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step) - { - for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index) - { - T raw_depth = depth_row[u]; - if (!DepthTraits::valid(raw_depth)) - continue; - - double depth = DepthTraits::toMeters(raw_depth); - - if (fill_upsampling_holes_ == false) - { - /// @todo Combine all operations into one matrix multiply on (u,v,d) - // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame - Eigen::Vector4d xyz_depth; - xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx, - ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy, - depth, - 1; - - // Transform to RGB camera frame - Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth; - - // Project to (u,v) in RGB image - double inv_Z = 1.0 / xyz_rgb.z(); - int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5; - int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5; - - if (u_rgb < 0 || u_rgb >= (int)registered_msg->width || - v_rgb < 0 || v_rgb >= (int)registered_msg->height) - continue; - - T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb]; - T new_depth = DepthTraits::fromMeters(xyz_rgb.z()); - // Validity and Z-buffer checks - if (!DepthTraits::valid(reg_depth) || reg_depth > new_depth) - reg_depth = new_depth; - } - else - { - // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame - Eigen::Vector4d xyz_depth_1, xyz_depth_2; - xyz_depth_1 << ((u-0.5f - depth_cx)*depth - depth_Tx) * inv_depth_fx, - ((v-0.5f - depth_cy)*depth - depth_Ty) * inv_depth_fy, - depth, - 1; - xyz_depth_2 << ((u+0.5f - depth_cx)*depth - depth_Tx) * inv_depth_fx, - ((v+0.5f - depth_cy)*depth - depth_Ty) * inv_depth_fy, - depth, - 1; - - // Transform to RGB camera frame - Eigen::Vector4d xyz_rgb_1 = depth_to_rgb * xyz_depth_1; - Eigen::Vector4d xyz_rgb_2 = depth_to_rgb * xyz_depth_2; - - // Project to (u,v) in RGB image - double inv_Z = 1.0 / xyz_rgb_1.z(); - int u_rgb_1 = (rgb_fx*xyz_rgb_1.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5; - int v_rgb_1 = (rgb_fy*xyz_rgb_1.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5; - inv_Z = 1.0 / xyz_rgb_2.z(); - int u_rgb_2 = (rgb_fx*xyz_rgb_2.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5; - int v_rgb_2 = (rgb_fy*xyz_rgb_2.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5; - - if (u_rgb_1 < 0 || u_rgb_2 >= (int)registered_msg->width || - v_rgb_1 < 0 || v_rgb_2 >= (int)registered_msg->height) - continue; - - for (int nv=v_rgb_1; nv<=v_rgb_2; ++nv) - { - for (int nu=u_rgb_1; nu<=u_rgb_2; ++nu) - { - T& reg_depth = registered_data[nv*registered_msg->width + nu]; - T new_depth = DepthTraits::fromMeters(0.5*(xyz_rgb_1.z()+xyz_rgb_2.z())); - // Validity and Z-buffer checks - if (!DepthTraits::valid(reg_depth) || reg_depth > new_depth) - reg_depth = new_depth; - } - } - } - } - } -} - -} // namespace depth_image_proc - -// Register as nodelet -#include -PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet,nodelet::Nodelet);