register.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <ros/ros.h>
00035 #include <nodelet/nodelet.h>
00036 #include <image_transport/image_transport.h>
00037 #include <image_transport/subscriber_filter.h>
00038 #include <message_filters/subscriber.h>
00039 #include <message_filters/synchronizer.h>
00040 #include <message_filters/sync_policies/approximate_time.h>
00041 #include <tf2_ros/buffer.h>
00042 #include <tf2_ros/transform_listener.h>
00043 #include <sensor_msgs/image_encodings.h>
00044 #include <image_geometry/pinhole_camera_model.h>
00045 #include <Eigen/Geometry>
00046 #include <eigen_conversions/eigen_msg.h>
00047 #include <depth_image_proc/depth_traits.h>
00048 
00049 namespace depth_image_proc {
00050 
00051 using namespace message_filters::sync_policies;
00052 namespace enc = sensor_msgs::image_encodings;
00053 
00054 class RegisterNodelet : public nodelet::Nodelet
00055 {
00056   ros::NodeHandlePtr nh_depth_, nh_rgb_;
00057   boost::shared_ptr<image_transport::ImageTransport> it_depth_;
00058   
00059   // Subscriptions
00060   image_transport::SubscriberFilter sub_depth_image_;
00061   message_filters::Subscriber<sensor_msgs::CameraInfo> sub_depth_info_, sub_rgb_info_;
00062   boost::shared_ptr<tf2_ros::Buffer> tf_buffer_;
00063   boost::shared_ptr<tf2_ros::TransformListener> tf_;
00064   typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> SyncPolicy;
00065   typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
00066   boost::shared_ptr<Synchronizer> sync_;
00067 
00068   // Publications
00069   boost::mutex connect_mutex_;
00070   image_transport::CameraPublisher pub_registered_;
00071 
00072   image_geometry::PinholeCameraModel depth_model_, rgb_model_;
00073 
00074   virtual void onInit();
00075 
00076   void connectCb();
00077 
00078   void imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
00079                const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
00080                const sensor_msgs::CameraInfoConstPtr& rgb_info_msg);
00081 
00082   template<typename T>
00083   void convert(const sensor_msgs::ImageConstPtr& depth_msg,
00084                const sensor_msgs::ImagePtr& registered_msg,
00085                const Eigen::Affine3d& depth_to_rgb);
00086 };
00087 
00088 void RegisterNodelet::onInit()
00089 {
00090   ros::NodeHandle& nh         = getNodeHandle();
00091   ros::NodeHandle& private_nh = getPrivateNodeHandle();
00092   nh_depth_.reset( new ros::NodeHandle(nh, "depth") );
00093   nh_rgb_.reset( new ros::NodeHandle(nh, "rgb") );
00094   it_depth_.reset( new image_transport::ImageTransport(*nh_depth_) );
00095   tf_buffer_.reset( new tf2_ros::Buffer );
00096   tf_.reset( new tf2_ros::TransformListener(*tf_buffer_) );
00097 
00098   // Read parameters
00099   int queue_size;
00100   private_nh.param("queue_size", queue_size, 5);
00101 
00102   // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
00103   sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image_, sub_depth_info_, sub_rgb_info_) );
00104   sync_->registerCallback(boost::bind(&RegisterNodelet::imageCb, this, _1, _2, _3));
00105 
00106   // Monitor whether anyone is subscribed to the output
00107   image_transport::ImageTransport it_depth_reg(ros::NodeHandle(nh, "depth_registered"));
00108   image_transport::SubscriberStatusCallback image_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
00109   ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
00110   // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_
00111   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00112   pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1,
00113                                                  image_connect_cb, image_connect_cb,
00114                                                  info_connect_cb, info_connect_cb);
00115 }
00116 
00117 // Handles (un)subscribing when clients (un)subscribe
00118 void RegisterNodelet::connectCb()
00119 {
00120   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00121   if (pub_registered_.getNumSubscribers() == 0)
00122   {
00123     sub_depth_image_.unsubscribe();
00124     sub_depth_info_ .unsubscribe();
00125     sub_rgb_info_   .unsubscribe();
00126   }
00127   else if (!sub_depth_image_.getSubscriber())
00128   {
00129     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00130     sub_depth_image_.subscribe(*it_depth_, "image_rect",  1, hints);
00131     sub_depth_info_ .subscribe(*nh_depth_, "camera_info", 1);
00132     sub_rgb_info_   .subscribe(*nh_rgb_,   "camera_info", 1);
00133   }
00134 }
00135 
00136 void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
00137                               const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
00138                               const sensor_msgs::CameraInfoConstPtr& rgb_info_msg)
00139 {
00140   // Update camera models - these take binning & ROI into account
00141   depth_model_.fromCameraInfo(depth_info_msg);
00142   rgb_model_  .fromCameraInfo(rgb_info_msg);
00143 
00144   // Query tf2 for transform from (X,Y,Z) in depth camera frame to RGB camera frame
00145   Eigen::Affine3d depth_to_rgb;
00146   try
00147   {
00148     geometry_msgs::TransformStamped transform = tf_buffer_->lookupTransform (
00149                           rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id,
00150                           depth_info_msg->header.stamp);
00151 
00152     tf::transformMsgToEigen(transform.transform, depth_to_rgb);
00153   }
00154   catch (tf2::TransformException& ex)
00155   {
00156     NODELET_WARN_THROTTLE(2, "TF2 exception:\n%s", ex.what());
00157     return;
00160   }
00161 
00162   // Allocate registered depth image
00163   sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image );
00164   registered_msg->header.stamp    = depth_image_msg->header.stamp;
00165   registered_msg->header.frame_id = rgb_info_msg->header.frame_id;
00166   registered_msg->encoding        = depth_image_msg->encoding;
00167   
00168   cv::Size resolution = rgb_model_.reducedResolution();
00169   registered_msg->height = resolution.height;
00170   registered_msg->width  = resolution.width;
00171   // step and data set in convert(), depend on depth data type
00172 
00173   if (depth_image_msg->encoding == enc::TYPE_16UC1)
00174   {
00175     convert<uint16_t>(depth_image_msg, registered_msg, depth_to_rgb);
00176   }
00177   else if (depth_image_msg->encoding == enc::TYPE_32FC1)
00178   {
00179     convert<float>(depth_image_msg, registered_msg, depth_to_rgb);
00180   }
00181   else
00182   {
00183     NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image_msg->encoding.c_str());
00184     return;
00185   }
00186 
00187   // Registered camera info is the same as the RGB info, but uses the depth timestamp
00188   sensor_msgs::CameraInfoPtr registered_info_msg( new sensor_msgs::CameraInfo(*rgb_info_msg) );
00189   registered_info_msg->header.stamp = registered_msg->header.stamp;
00190 
00191   pub_registered_.publish(registered_msg, registered_info_msg);
00192 }
00193 
00194 template<typename T>
00195 void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
00196                               const sensor_msgs::ImagePtr& registered_msg,
00197                               const Eigen::Affine3d& depth_to_rgb)
00198 {
00199   // Allocate memory for registered depth image
00200   registered_msg->step = registered_msg->width * sizeof(T);
00201   registered_msg->data.resize( registered_msg->height * registered_msg->step );
00202   // data is already zero-filled in the uint16 case, but for floats we want to initialize everything to NaN.
00203   DepthTraits<T>::initializeBuffer(registered_msg->data);
00204 
00205   // Extract all the parameters we need
00206   double inv_depth_fx = 1.0 / depth_model_.fx();
00207   double inv_depth_fy = 1.0 / depth_model_.fy();
00208   double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy();
00209   double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty();
00210   double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
00211   double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
00212   double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();
00213   
00214   // Transform the depth values into the RGB frame
00216   const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00217   int row_step = depth_msg->step / sizeof(T);
00218   T* registered_data = reinterpret_cast<T*>(&registered_msg->data[0]);
00219   int raw_index = 0;
00220   for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step)
00221   {
00222     for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index)
00223     {
00224       T raw_depth = depth_row[u];
00225       if (!DepthTraits<T>::valid(raw_depth))
00226         continue;
00227       
00228       double depth = DepthTraits<T>::toMeters(raw_depth);
00229 
00231       // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame
00232       Eigen::Vector4d xyz_depth;
00233       xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx,
00234                    ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy,
00235                    depth,
00236                    1;
00237 
00238       // Transform to RGB camera frame
00239       Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth;
00240 
00241       // Project to (u,v) in RGB image
00242       double inv_Z = 1.0 / xyz_rgb.z();
00243       int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5;
00244       int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5;
00245       
00246       if (u_rgb < 0 || u_rgb >= (int)registered_msg->width ||
00247           v_rgb < 0 || v_rgb >= (int)registered_msg->height)
00248         continue;
00249       
00250       T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb];
00251       T  new_depth = DepthTraits<T>::fromMeters(xyz_rgb.z());
00252       // Validity and Z-buffer checks
00253       if (!DepthTraits<T>::valid(reg_depth) || reg_depth > new_depth)
00254         reg_depth = new_depth;
00255     }
00256   }
00257 }
00258 
00259 } // namespace depth_image_proc
00260 
00261 // Register as nodelet
00262 #include <pluginlib/class_list_macros.h>
00263 PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet,nodelet::Nodelet);


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed May 1 2019 02:51:42