register.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #include <ros/ros.h>
35 #include <nodelet/nodelet.h>
41 #include <tf2_ros/buffer.h>
45 #include <Eigen/Geometry>
48 
49 namespace depth_image_proc {
50 
51 using namespace message_filters::sync_policies;
53 
55 {
58 
59  // Subscriptions
67 
68  // Publications
69  boost::mutex connect_mutex_;
71 
73 
74  virtual void onInit();
75 
76  void connectCb();
77 
78  void imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
79  const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
80  const sensor_msgs::CameraInfoConstPtr& rgb_info_msg);
81 
82  template<typename T>
83  void convert(const sensor_msgs::ImageConstPtr& depth_msg,
84  const sensor_msgs::ImagePtr& registered_msg,
85  const Eigen::Affine3d& depth_to_rgb);
86 };
87 
89 {
90  ros::NodeHandle& nh = getNodeHandle();
91  ros::NodeHandle& private_nh = getPrivateNodeHandle();
92  nh_depth_.reset( new ros::NodeHandle(nh, "depth") );
93  nh_rgb_.reset( new ros::NodeHandle(nh, "rgb") );
94  it_depth_.reset( new image_transport::ImageTransport(*nh_depth_) );
95  tf_buffer_.reset( new tf2_ros::Buffer );
96  tf_.reset( new tf2_ros::TransformListener(*tf_buffer_) );
97 
98  // Read parameters
99  int queue_size;
100  private_nh.param("queue_size", queue_size, 5);
101 
102  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
103  sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image_, sub_depth_info_, sub_rgb_info_) );
104  sync_->registerCallback(boost::bind(&RegisterNodelet::imageCb, this, _1, _2, _3));
105 
106  // Monitor whether anyone is subscribed to the output
107  image_transport::ImageTransport it_depth_reg(ros::NodeHandle(nh, "depth_registered"));
108  image_transport::SubscriberStatusCallback image_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
109  ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
110  // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_
111  boost::lock_guard<boost::mutex> lock(connect_mutex_);
112  pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1,
113  image_connect_cb, image_connect_cb,
114  info_connect_cb, info_connect_cb);
115 }
116 
117 // Handles (un)subscribing when clients (un)subscribe
119 {
120  boost::lock_guard<boost::mutex> lock(connect_mutex_);
121  if (pub_registered_.getNumSubscribers() == 0)
122  {
123  sub_depth_image_.unsubscribe();
124  sub_depth_info_ .unsubscribe();
125  sub_rgb_info_ .unsubscribe();
126  }
127  else if (!sub_depth_image_.getSubscriber())
128  {
129  image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
130  sub_depth_image_.subscribe(*it_depth_, "image_rect", 1, hints);
131  sub_depth_info_ .subscribe(*nh_depth_, "camera_info", 1);
132  sub_rgb_info_ .subscribe(*nh_rgb_, "camera_info", 1);
133  }
134 }
135 
136 void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
137  const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
138  const sensor_msgs::CameraInfoConstPtr& rgb_info_msg)
139 {
140  // Update camera models - these take binning & ROI into account
141  depth_model_.fromCameraInfo(depth_info_msg);
142  rgb_model_ .fromCameraInfo(rgb_info_msg);
143 
144  // Query tf2 for transform from (X,Y,Z) in depth camera frame to RGB camera frame
145  Eigen::Affine3d depth_to_rgb;
146  try
147  {
148  geometry_msgs::TransformStamped transform = tf_buffer_->lookupTransform (
149  rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id,
150  depth_info_msg->header.stamp);
151 
152  tf::transformMsgToEigen(transform.transform, depth_to_rgb);
153  }
154  catch (tf2::TransformException& ex)
155  {
156  NODELET_WARN_THROTTLE(2, "TF2 exception:\n%s", ex.what());
157  return;
160  }
161 
162  // Allocate registered depth image
163  sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image );
164  registered_msg->header.stamp = depth_image_msg->header.stamp;
165  registered_msg->header.frame_id = rgb_info_msg->header.frame_id;
166  registered_msg->encoding = depth_image_msg->encoding;
167 
168  cv::Size resolution = rgb_model_.reducedResolution();
169  registered_msg->height = resolution.height;
170  registered_msg->width = resolution.width;
171  // step and data set in convert(), depend on depth data type
172 
173  if (depth_image_msg->encoding == enc::TYPE_16UC1)
174  {
175  convert<uint16_t>(depth_image_msg, registered_msg, depth_to_rgb);
176  }
177  else if (depth_image_msg->encoding == enc::TYPE_32FC1)
178  {
179  convert<float>(depth_image_msg, registered_msg, depth_to_rgb);
180  }
181  else
182  {
183  NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image_msg->encoding.c_str());
184  return;
185  }
186 
187  // Registered camera info is the same as the RGB info, but uses the depth timestamp
188  sensor_msgs::CameraInfoPtr registered_info_msg( new sensor_msgs::CameraInfo(*rgb_info_msg) );
189  registered_info_msg->header.stamp = registered_msg->header.stamp;
190 
191  pub_registered_.publish(registered_msg, registered_info_msg);
192 }
193 
194 template<typename T>
195 void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
196  const sensor_msgs::ImagePtr& registered_msg,
197  const Eigen::Affine3d& depth_to_rgb)
198 {
199  // Allocate memory for registered depth image
200  registered_msg->step = registered_msg->width * sizeof(T);
201  registered_msg->data.resize( registered_msg->height * registered_msg->step );
202  // data is already zero-filled in the uint16 case, but for floats we want to initialize everything to NaN.
203  DepthTraits<T>::initializeBuffer(registered_msg->data);
204 
205  // Extract all the parameters we need
206  double inv_depth_fx = 1.0 / depth_model_.fx();
207  double inv_depth_fy = 1.0 / depth_model_.fy();
208  double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy();
209  double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty();
210  double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
211  double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
212  double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();
213 
214  // Transform the depth values into the RGB frame
216  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
217  int row_step = depth_msg->step / sizeof(T);
218  T* registered_data = reinterpret_cast<T*>(&registered_msg->data[0]);
219  int raw_index = 0;
220  for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step)
221  {
222  for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index)
223  {
224  T raw_depth = depth_row[u];
225  if (!DepthTraits<T>::valid(raw_depth))
226  continue;
227 
228  double depth = DepthTraits<T>::toMeters(raw_depth);
229 
231  // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame
232  Eigen::Vector4d xyz_depth;
233  xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx,
234  ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy,
235  depth,
236  1;
237 
238  // Transform to RGB camera frame
239  Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth;
240 
241  // Project to (u,v) in RGB image
242  double inv_Z = 1.0 / xyz_rgb.z();
243  int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5;
244  int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5;
245 
246  if (u_rgb < 0 || u_rgb >= (int)registered_msg->width ||
247  v_rgb < 0 || v_rgb >= (int)registered_msg->height)
248  continue;
249 
250  T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb];
251  T new_depth = DepthTraits<T>::fromMeters(xyz_rgb.z());
252  // Validity and Z-buffer checks
253  if (!DepthTraits<T>::valid(reg_depth) || reg_depth > new_depth)
254  reg_depth = new_depth;
255  }
256  }
257 }
258 
259 } // namespace depth_image_proc
260 
261 // Register as nodelet
void convert(const sensor_msgs::ImageConstPtr &depth_msg, PointCloud::Ptr &cloud_msg, const image_geometry::PinholeCameraModel &model, double range_max=0.0)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
boost::shared_ptr< Synchronizer > sync_
Definition: register.cpp:66
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_rgb_info_
Definition: register.cpp:61
ros::NodeHandlePtr nh_rgb_
Definition: register.cpp:56
#define NODELET_ERROR_THROTTLE(rate,...)
ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicy
boost::shared_ptr< image_transport::ImageTransport > it_depth_
Definition: register.cpp:57
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
boost::shared_ptr< tf2_ros::TransformListener > tf_
Definition: register.cpp:63
image_geometry::PinholeCameraModel rgb_model_
Definition: register.cpp:72
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
#define NODELET_WARN_THROTTLE(rate,...)
image_transport::SubscriberFilter sub_depth_image_
Definition: register.cpp:60
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void imageCb(const sensor_msgs::ImageConstPtr &depth_image_msg, const sensor_msgs::CameraInfoConstPtr &depth_info_msg, const sensor_msgs::CameraInfoConstPtr &rgb_info_msg)
Definition: register.cpp:136
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet, nodelet::Nodelet)
void convert(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImagePtr &registered_msg, const Eigen::Affine3d &depth_to_rgb)
Definition: register.cpp:195
image_transport::CameraPublisher pub_registered_
Definition: register.cpp:70
boost::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: register.cpp:62
ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > SyncPolicy
Definition: register.cpp:64
message_filters::Synchronizer< SyncPolicy > Synchronizer
Definition: register.cpp:65


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Thu Nov 7 2019 03:44:56