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 <tf/transform_listener.h>
00042 #include <sensor_msgs/image_encodings.h>
00043 #include <image_geometry/pinhole_camera_model.h>
00044 #include <Eigen/Core>
00045 #include <depth_image_proc/depth_traits.h>
00046 
00047 namespace depth_image_proc {
00048 
00049 using namespace message_filters::sync_policies;
00050 namespace enc = sensor_msgs::image_encodings;
00051 
00052 class RegisterNodelet : public nodelet::Nodelet
00053 {
00054   ros::NodeHandlePtr nh_depth_, nh_rgb_;
00055   boost::shared_ptr<image_transport::ImageTransport> it_depth_;
00056   
00057   // Subscriptions
00058   image_transport::SubscriberFilter sub_depth_image_;
00059   message_filters::Subscriber<sensor_msgs::CameraInfo> sub_depth_info_, sub_rgb_info_;
00060   boost::shared_ptr<tf::TransformListener> tf_;
00061   typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> SyncPolicy;
00062   typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
00063   boost::shared_ptr<Synchronizer> sync_;
00064 
00065   // Publications
00066   boost::mutex connect_mutex_;
00067   image_transport::CameraPublisher pub_registered_;
00068 
00069   image_geometry::PinholeCameraModel depth_model_, rgb_model_;
00070 
00071   virtual void onInit();
00072 
00073   void connectCb();
00074 
00075   void imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
00076                const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
00077                const sensor_msgs::CameraInfoConstPtr& rgb_info_msg);
00078 
00079   template<typename T>
00080   void convert(const sensor_msgs::ImageConstPtr& depth_msg,
00081                const sensor_msgs::ImagePtr& registered_msg,
00082                const Eigen::Matrix4d& depth_to_rgb);
00083 };
00084 
00085 void RegisterNodelet::onInit()
00086 {
00087   ros::NodeHandle& nh         = getNodeHandle();
00088   ros::NodeHandle& private_nh = getPrivateNodeHandle();
00089   nh_depth_.reset( new ros::NodeHandle(nh, "depth") );
00090   nh_rgb_.reset( new ros::NodeHandle(nh, "rgb") );
00091   it_depth_.reset( new image_transport::ImageTransport(*nh_depth_) );
00092   tf_.reset( new tf::TransformListener );
00093 
00094   // Read parameters
00095   int queue_size;
00096   private_nh.param("queue_size", queue_size, 5);
00097 
00098   // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
00099   sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image_, sub_depth_info_, sub_rgb_info_) );
00100   sync_->registerCallback(boost::bind(&RegisterNodelet::imageCb, this, _1, _2, _3));
00101 
00102   // Monitor whether anyone is subscribed to the output
00103   image_transport::ImageTransport it_depth_reg(ros::NodeHandle(nh, "depth_registered"));
00104   image_transport::SubscriberStatusCallback image_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
00105   ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
00106   // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_
00107   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00108   pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1,
00109                                                  image_connect_cb, image_connect_cb,
00110                                                  info_connect_cb, info_connect_cb);
00111 }
00112 
00113 // Handles (un)subscribing when clients (un)subscribe
00114 void RegisterNodelet::connectCb()
00115 {
00116   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00117   if (pub_registered_.getNumSubscribers() == 0)
00118   {
00119     sub_depth_image_.unsubscribe();
00120     sub_depth_info_ .unsubscribe();
00121     sub_rgb_info_   .unsubscribe();
00122   }
00123   else if (!sub_depth_image_.getSubscriber())
00124   {
00125     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00126     sub_depth_image_.subscribe(*it_depth_, "image_rect",  1, hints);
00127     sub_depth_info_ .subscribe(*nh_depth_, "camera_info", 1);
00128     sub_rgb_info_   .subscribe(*nh_rgb_,   "camera_info", 1);
00129   }
00130 }
00131 
00132 void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
00133                               const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
00134                               const sensor_msgs::CameraInfoConstPtr& rgb_info_msg)
00135 {
00136   // Update camera models - these take binning & ROI into account
00137   depth_model_.fromCameraInfo(depth_info_msg);
00138   rgb_model_  .fromCameraInfo(rgb_info_msg);
00139 
00140   // Query tf for transform from (X,Y,Z) in depth camera frame to RGB camera frame
00141   Eigen::Matrix4d depth_to_rgb;
00142   try
00143   {
00144     ros::Duration timeout(1.0 / 30); 
00145     tf_->waitForTransform(rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id,
00146                           depth_info_msg->header.stamp, timeout);
00147     tf::StampedTransform transform;
00148     tf_->lookupTransform (rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id,
00149                           depth_info_msg->header.stamp, transform);
00150 
00151     tf::Matrix3x3& R = transform.getBasis();
00152     tf::Vector3&   T = transform.getOrigin();
00153     depth_to_rgb << R[0][0], R[0][1], R[0][2], T[0],
00154                     R[1][0], R[1][1], R[1][2], T[1],
00155                     R[2][0], R[2][1], R[2][2], T[2],
00156                           0,       0,       0,    1;
00157   }
00158   catch (tf::TransformException& ex)
00159   {
00160     NODELET_WARN_THROTTLE(2, "TF exception:\n%s", ex.what());
00161     return;
00164   }
00165 
00166   // Allocate registered depth image
00167   sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image );
00168   registered_msg->header.stamp    = depth_image_msg->header.stamp;
00169   registered_msg->header.frame_id = rgb_info_msg->header.frame_id;
00170   registered_msg->encoding        = depth_image_msg->encoding;
00171   
00172   cv::Size resolution = rgb_model_.reducedResolution();
00173   registered_msg->height = resolution.height;
00174   registered_msg->width  = resolution.width;
00175   // step and data set in convert(), depend on depth data type
00176 
00177   if (depth_image_msg->encoding == enc::TYPE_16UC1)
00178   {
00179     convert<uint16_t>(depth_image_msg, registered_msg, depth_to_rgb);
00180   }
00181   else if (depth_image_msg->encoding == enc::TYPE_32FC1)
00182   {
00183     convert<float>(depth_image_msg, registered_msg, depth_to_rgb);
00184   }
00185   else
00186   {
00187     NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image_msg->encoding.c_str());
00188     return;
00189   }
00190 
00191   // Registered camera info is the same as the RGB info, but uses the depth timestamp
00192   sensor_msgs::CameraInfoPtr registered_info_msg( new sensor_msgs::CameraInfo(*rgb_info_msg) );
00193   registered_info_msg->header.stamp = registered_msg->header.stamp;
00194 
00195   pub_registered_.publish(registered_msg, registered_info_msg);
00196 }
00197 
00198 template<typename T>
00199 void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
00200                               const sensor_msgs::ImagePtr& registered_msg,
00201                               const Eigen::Matrix4d& depth_to_rgb)
00202 {
00203   // Allocate memory for registered depth image
00204   registered_msg->step = registered_msg->width * sizeof(T);
00205   registered_msg->data.resize( registered_msg->height * registered_msg->step );
00206   // data is already zero-filled in the uint16 case, but for floats we want to initialize everything to NaN.
00207   DepthTraits<T>::initializeBuffer(registered_msg->data);
00208 
00209   // Extract all the parameters we need
00210   double inv_depth_fx = 1.0 / depth_model_.fx();
00211   double inv_depth_fy = 1.0 / depth_model_.fy();
00212   double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy();
00213   double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty();
00214   double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
00215   double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
00216   double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();
00217   
00218   // Transform the depth values into the RGB frame
00220   const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00221   int row_step = depth_msg->step / sizeof(T);
00222   T* registered_data = reinterpret_cast<T*>(&registered_msg->data[0]);
00223   int raw_index = 0;
00224   for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step)
00225   {
00226     for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index)
00227     {
00228       T raw_depth = depth_row[u];
00229       if (!DepthTraits<T>::valid(raw_depth))
00230         continue;
00231       
00232       double depth = DepthTraits<T>::toMeters(raw_depth);
00233 
00235       // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame
00236       Eigen::Vector4d xyz_depth;
00237       xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx,
00238                    ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy,
00239                    depth,
00240                    1;
00241 
00242       // Transform to RGB camera frame
00243       Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth;
00244 
00245       // Project to (u,v) in RGB image
00246       double inv_Z = 1.0 / xyz_rgb.z();
00247       int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5;
00248       int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5;
00249       
00250       if (u_rgb < 0 || u_rgb >= (int)registered_msg->width ||
00251           v_rgb < 0 || v_rgb >= (int)registered_msg->height)
00252         continue;
00253       
00254       T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb];
00255       T  new_depth = DepthTraits<T>::fromMeters(xyz_rgb.z());
00256       // Validity and Z-buffer checks
00257       if (!DepthTraits<T>::valid(reg_depth) || reg_depth > new_depth)
00258         reg_depth = new_depth;
00259     }
00260   }
00261 }
00262 
00263 } // namespace depth_image_proc
00264 
00265 // Register as nodelet
00266 #include <pluginlib/class_list_macros.h>
00267 PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet,nodelet::Nodelet);


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed Aug 26 2015 11:57:40