depth_image_octomap_updater.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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 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 
35 /* Author: Ioan Sucan, Suat Gedikli */
36 
41 #include <XmlRpcException.h>
42 #include <stdint.h>
43 
44 #include <memory>
45 
46 namespace occupancy_map_monitor
47 {
49  : OccupancyMapUpdater("DepthImageUpdater")
50  , nh_("~")
51  , input_depth_transport_(nh_)
52  , model_depth_transport_(nh_)
53  , filtered_depth_transport_(nh_)
54  , filtered_label_transport_(nh_)
55  , image_topic_("depth")
56  , queue_size_(5)
57  , near_clipping_plane_distance_(0.3)
58  , far_clipping_plane_distance_(5.0)
59  , shadow_threshold_(0.04)
60  , padding_scale_(0.0)
61  , padding_offset_(0.02)
62  , max_update_rate_(0)
63  , skip_vertical_pixels_(4)
64  , skip_horizontal_pixels_(6)
65  , image_callback_count_(0)
66  , average_callback_dt_(0.0)
67  , good_tf_(5)
68  , // start optimistically, so we do not output warnings right from the beginning
69  failed_tf_(0)
70  , K0_(0.0)
71  , K2_(0.0)
72  , K4_(0.0)
73  , K5_(0.0)
74 {
75 }
76 
78 {
79  stopHelper();
80 }
81 
83 {
84  try
85  {
86  sensor_type_ = (std::string)params["sensor_type"];
87  if (params.hasMember("image_topic"))
88  image_topic_ = (std::string)params["image_topic"];
89  if (params.hasMember("queue_size"))
90  queue_size_ = (int)params["queue_size"];
91 
92  readXmlParam(params, "near_clipping_plane_distance", &near_clipping_plane_distance_);
93  readXmlParam(params, "far_clipping_plane_distance", &far_clipping_plane_distance_);
94  readXmlParam(params, "shadow_threshold", &shadow_threshold_);
95  readXmlParam(params, "padding_scale", &padding_scale_);
96  readXmlParam(params, "padding_offset", &padding_offset_);
97  if (params.hasMember("max_update_rate"))
98  readXmlParam(params, "max_update_rate", &max_update_rate_);
99  readXmlParam(params, "skip_vertical_pixels", &skip_vertical_pixels_);
100  readXmlParam(params, "skip_horizontal_pixels", &skip_horizontal_pixels_);
101  if (params.hasMember("filtered_cloud_topic"))
102  filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
103  }
104  catch (XmlRpc::XmlRpcException& ex)
105  {
106  ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
107  return false;
108  }
109 
110  return true;
111 }
112 
114 {
115  tf_ = monitor_->getTFClient();
117 
118  // create our mesh filter
122  mesh_filter_->setShadowThreshold(shadow_threshold_);
123  mesh_filter_->setPaddingOffset(padding_offset_);
124  mesh_filter_->setPaddingScale(padding_scale_);
125  mesh_filter_->setTransformCallback(boost::bind(&DepthImageOctomapUpdater::getShapeTransform, this, _1, _2));
126 
127  return true;
128 }
129 
131 {
134 
135  if (!filtered_cloud_topic_.empty())
137  else
139 
141 
144 }
145 
147 {
148  stopHelper();
149 }
150 
152 {
154 }
155 
157 {
159  if (mesh_filter_)
160  {
161  if (shape->type == shapes::MESH)
162  h = mesh_filter_->addMesh(static_cast<const shapes::Mesh&>(*shape));
163  else
164  {
165  std::unique_ptr<shapes::Mesh> m(shapes::createMeshFromShape(shape.get()));
166  if (m)
167  h = mesh_filter_->addMesh(*m);
168  }
169  }
170  else
171  ROS_ERROR("Mesh filter not yet initialized!");
172  return h;
173 }
174 
176 {
177  if (mesh_filter_)
178  mesh_filter_->removeMesh(handle);
179 }
180 
182 {
183  ShapeTransformCache::const_iterator it = transform_cache_.find(h);
184  if (it == transform_cache_.end())
185  {
186  ROS_ERROR("Internal error. Mesh filter handle %u not found", h);
187  return false;
188  }
189  transform = it->second;
190  return true;
191 }
192 
193 namespace
194 {
195 bool host_is_big_endian(void)
196 {
197  union
198  {
199  uint32_t i;
200  char c[sizeof(uint32_t)];
201  } bint = { 0x01020304 };
202  return bint.c[0] == 1;
203 }
204 }
205 
206 static const bool HOST_IS_BIG_ENDIAN = host_is_big_endian();
207 
208 void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstPtr& depth_msg,
209  const sensor_msgs::CameraInfoConstPtr& info_msg)
210 {
211  ROS_DEBUG("Received a new depth image message (frame = '%s', encoding='%s')", depth_msg->header.frame_id.c_str(),
212  depth_msg->encoding.c_str());
214 
215  if (max_update_rate_ > 0)
216  {
217  // ensure we are not updating the octomap representation too often
219  return;
221  }
222 
223  // measure the frequency at which we receive updates
224  if (image_callback_count_ < 1000)
225  {
226  if (image_callback_count_ > 0)
227  {
228  const double dt_start = (start - last_depth_callback_start_).toSec();
229  if (image_callback_count_ < 2)
230  average_callback_dt_ = dt_start;
231  else
234  }
235  }
236  else
237  // every 1000 updates we reset the counter almost to the beginning (use 2 so we don't have so much of a ripple in
238  // the measured average)
242 
243  if (monitor_->getMapFrame().empty())
244  monitor_->setMapFrame(depth_msg->header.frame_id);
245 
246  /* get transform for cloud into map frame */
247  tf::StampedTransform map_H_sensor;
248  if (monitor_->getMapFrame() == depth_msg->header.frame_id)
249  map_H_sensor.setIdentity();
250  else
251  {
252  if (tf_)
253  {
254  // wait at most 50ms
255  static const double TEST_DT = 0.005;
256  const int nt = (int)(0.5 + average_callback_dt_ / TEST_DT) * std::max(1, ((int)queue_size_ / 2));
257  bool found = false;
258  std::string err;
259  for (int t = 0; t < nt; ++t)
260  try
261  {
262  tf_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id, depth_msg->header.stamp,
263  map_H_sensor);
264  found = true;
265  break;
266  }
267  catch (tf::TransformException& ex)
268  {
269  static const ros::Duration d(TEST_DT);
270  err = ex.what();
271  d.sleep();
272  }
273  static const unsigned int MAX_TF_COUNTER = 1000; // so we avoid int overflow
274  if (found)
275  {
276  good_tf_++;
277  if (good_tf_ > MAX_TF_COUNTER)
278  {
279  const unsigned int div = MAX_TF_COUNTER / 10;
280  good_tf_ /= div;
281  failed_tf_ /= div;
282  }
283  }
284  else
285  {
286  failed_tf_++;
287  if (failed_tf_ > good_tf_)
288  ROS_WARN_THROTTLE(1, "More than half of the image messages discared due to TF being unavailable (%u%%). "
289  "Transform error of sensor data: %s; quitting callback.",
290  (100 * failed_tf_) / (good_tf_ + failed_tf_), err.c_str());
291  else
292  ROS_DEBUG_THROTTLE(1, "Transform error of sensor data: %s; quitting callback", err.c_str());
293  if (failed_tf_ > MAX_TF_COUNTER)
294  {
295  const unsigned int div = MAX_TF_COUNTER / 10;
296  good_tf_ /= div;
297  failed_tf_ /= div;
298  }
299  return;
300  }
301  }
302  else
303  return;
304  }
305 
306  if (!updateTransformCache(depth_msg->header.frame_id, depth_msg->header.stamp))
307  {
308  ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail.");
309  return;
310  }
311 
312  if (depth_msg->is_bigendian && !HOST_IS_BIG_ENDIAN)
313  ROS_ERROR_THROTTLE(1, "endian problem: received image data does not match host");
314 
315  const int w = depth_msg->width;
316  const int h = depth_msg->height;
317 
318  // call the mesh filter
320  params.setCameraParameters(info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]);
321  params.setImageSize(w, h);
322 
323  const bool is_u_short = depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1;
324  if (is_u_short)
325  mesh_filter_->filter(&depth_msg->data[0], GL_UNSIGNED_SHORT);
326  else
327  {
328  if (depth_msg->encoding != sensor_msgs::image_encodings::TYPE_32FC1)
329  {
330  ROS_ERROR_THROTTLE(1, "Unexpected encoding type: '%s'. Ignoring input.", depth_msg->encoding.c_str());
331  return;
332  }
333  mesh_filter_->filter(&depth_msg->data[0], GL_FLOAT);
334  }
335 
336  // the mesh filter runs in background; compute extra things in the meantime
337 
338  // Use correct principal point from calibration
339  const double px = info_msg->K[2];
340  const double py = info_msg->K[5];
341 
342  // if the camera parameters have changed at all, recompute the cache we had
343  if (w >= static_cast<int>(x_cache_.size()) || h >= static_cast<int>(y_cache_.size()) || K2_ != px || K5_ != py ||
344  K0_ != info_msg->K[0] || K4_ != info_msg->K[4])
345  {
346  K2_ = px;
347  K5_ = py;
348  K0_ = info_msg->K[0];
349  K4_ = info_msg->K[4];
350 
351  inv_fx_ = 1.0 / K0_;
352  inv_fy_ = 1.0 / K4_;
353 
354  // if there are any NaNs, discard data
355  if (!(px == px && py == py && inv_fx_ == inv_fx_ && inv_fy_ == inv_fy_))
356  return;
357 
358  // Pre-compute some constants
359  if (static_cast<int>(x_cache_.size()) < w)
360  x_cache_.resize(w);
361  if (static_cast<int>(y_cache_.size()) < h)
362  y_cache_.resize(h);
363 
364  for (int x = 0; x < w; ++x)
365  x_cache_[x] = (x - px) * inv_fx_;
366 
367  for (int y = 0; y < h; ++y)
368  y_cache_[y] = (y - py) * inv_fy_;
369  }
370 
371  const octomap::point3d sensor_origin(map_H_sensor.getOrigin().getX(), map_H_sensor.getOrigin().getY(),
372  map_H_sensor.getOrigin().getZ());
373 
374  octomap::KeySet* occupied_cells_ptr = new octomap::KeySet();
375  octomap::KeySet* model_cells_ptr = new octomap::KeySet();
376  octomap::KeySet& occupied_cells = *occupied_cells_ptr;
377  octomap::KeySet& model_cells = *model_cells_ptr;
378 
379  // allocate memory if needed
380  std::size_t img_size = h * w;
381  if (filtered_labels_.size() < img_size)
382  filtered_labels_.resize(img_size);
383 
384  // get the labels of the filtered data
385  const unsigned int* labels_row = &filtered_labels_[0];
386  mesh_filter_->getFilteredLabels(&filtered_labels_[0]);
387 
388  // publish debug information if needed
389  if (debug_info_)
390  {
391  sensor_msgs::Image debug_msg;
392  debug_msg.header = depth_msg->header;
393  debug_msg.height = depth_msg->height;
394  debug_msg.width = depth_msg->width;
395  debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
396  debug_msg.is_bigendian = depth_msg->is_bigendian;
397  debug_msg.step = depth_msg->step;
398  debug_msg.data.resize(img_size * sizeof(float));
399  mesh_filter_->getModelDepth(reinterpret_cast<float*>(&debug_msg.data[0]));
400  pub_model_depth_image_.publish(debug_msg, *info_msg);
401 
402  sensor_msgs::Image filtered_depth_msg;
403  filtered_depth_msg.header = depth_msg->header;
404  filtered_depth_msg.height = depth_msg->height;
405  filtered_depth_msg.width = depth_msg->width;
406  filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
407  filtered_depth_msg.is_bigendian = depth_msg->is_bigendian;
408  filtered_depth_msg.step = depth_msg->step;
409  filtered_depth_msg.data.resize(img_size * sizeof(float));
410 
411  mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_depth_msg.data[0]));
412  pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg);
413 
414  sensor_msgs::Image label_msg;
415  label_msg.header = depth_msg->header;
416  label_msg.height = depth_msg->height;
417  label_msg.width = depth_msg->width;
418  label_msg.encoding = sensor_msgs::image_encodings::RGBA8;
419  label_msg.is_bigendian = depth_msg->is_bigendian;
420  label_msg.step = w * sizeof(unsigned int);
421  label_msg.data.resize(img_size * sizeof(unsigned int));
422  mesh_filter_->getFilteredLabels(reinterpret_cast<unsigned int*>(&label_msg.data[0]));
423 
424  pub_filtered_label_image_.publish(label_msg, *info_msg);
425  }
426 
427  if (!filtered_cloud_topic_.empty())
428  {
429  static std::vector<float> filtered_data;
430  sensor_msgs::Image filtered_msg;
431  filtered_msg.header = depth_msg->header;
432  filtered_msg.height = depth_msg->height;
433  filtered_msg.width = depth_msg->width;
434  filtered_msg.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
435  filtered_msg.is_bigendian = depth_msg->is_bigendian;
436  filtered_msg.step = depth_msg->step;
437  filtered_msg.data.resize(img_size * sizeof(unsigned short));
438  if (filtered_data.size() < img_size)
439  filtered_data.resize(img_size);
440  mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_data[0]));
441  unsigned short* tmp_ptr = (unsigned short*)&filtered_msg.data[0];
442  for (std::size_t i = 0; i < img_size; ++i)
443  {
444  tmp_ptr[i] = (unsigned short)(filtered_data[i] * 1000 + 0.5);
445  }
446  pub_filtered_depth_image_.publish(filtered_msg, *info_msg);
447  }
448 
449  // figure out occupied cells and model cells
450  tree_->lockRead();
451 
452  try
453  {
454  const int h_bound = h - skip_vertical_pixels_;
455  const int w_bound = w - skip_horizontal_pixels_;
456 
457  if (is_u_short)
458  {
459  const uint16_t* input_row = reinterpret_cast<const uint16_t*>(&depth_msg->data[0]);
460 
461  for (int y = skip_vertical_pixels_; y < h_bound; ++y, labels_row += w, input_row += w)
462  for (int x = skip_horizontal_pixels_; x < w_bound; ++x)
463  {
464  // not filtered
465  if (labels_row[x] == mesh_filter::MeshFilterBase::Background)
466  {
467  float zz = (float)input_row[x] * 1e-3; // scale from mm to m
468  float yy = y_cache_[y] * zz;
469  float xx = x_cache_[x] * zz;
470  /* transform to map frame */
471  tf::Vector3 point_tf = map_H_sensor * tf::Vector3(xx, yy, zz);
472  occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
473  }
474  // on far plane or a model point -> remove
475  else if (labels_row[x] >= mesh_filter::MeshFilterBase::FarClip)
476  {
477  float zz = input_row[x] * 1e-3;
478  float yy = y_cache_[y] * zz;
479  float xx = x_cache_[x] * zz;
480  /* transform to map frame */
481  tf::Vector3 point_tf = map_H_sensor * tf::Vector3(xx, yy, zz);
482  // add to the list of model cells
483  model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
484  }
485  }
486  }
487  else
488  {
489  const float* input_row = reinterpret_cast<const float*>(&depth_msg->data[0]);
490 
491  for (int y = skip_vertical_pixels_; y < h_bound; ++y, labels_row += w, input_row += w)
492  for (int x = skip_horizontal_pixels_; x < w_bound; ++x)
493  {
494  if (labels_row[x] == mesh_filter::MeshFilterBase::Background)
495  {
496  float zz = input_row[x];
497  float yy = y_cache_[y] * zz;
498  float xx = x_cache_[x] * zz;
499  /* transform to map frame */
500  tf::Vector3 point_tf = map_H_sensor * tf::Vector3(xx, yy, zz);
501  occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
502  }
503  else if (labels_row[x] >= mesh_filter::MeshFilterBase::FarClip)
504  {
505  float zz = input_row[x];
506  float yy = y_cache_[y] * zz;
507  float xx = x_cache_[x] * zz;
508  /* transform to map frame */
509  tf::Vector3 point_tf = map_H_sensor * tf::Vector3(xx, yy, zz);
510  // add to the list of model cells
511  model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
512  }
513  }
514  }
515  }
516  catch (...)
517  {
518  tree_->unlockRead();
519  ROS_ERROR("Internal error while parsing depth data");
520  return;
521  }
522  tree_->unlockRead();
523 
524  /* cells that overlap with the model are not occupied */
525  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
526  occupied_cells.erase(*it);
527 
528  // mark occupied cells
529  tree_->lockWrite();
530  try
531  {
532  /* now mark all occupied cells */
533  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
534  tree_->updateNode(*it, true);
535  }
536  catch (...)
537  {
538  ROS_ERROR("Internal error while updating octree");
539  }
540  tree_->unlockWrite();
541  tree_->triggerUpdateCallback();
542 
543  // at this point we still have not freed the space
544  free_space_updater_->pushLazyUpdate(occupied_cells_ptr, model_cells_ptr, sensor_origin);
545 
546  ROS_DEBUG("Processed depth image in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
547 }
548 }
d
virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape)
const std::string & getMessage() const
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
#define ROS_WARN_THROTTLE(rate,...)
virtual bool initialize()
Do any necessary setup (subscribe to ros topics, etc.). This call assumes setMonitor() and setParams(...
#define ROS_DEBUG_THROTTLE(rate,...)
CameraSubscriber subscribeCamera(const std::string &base_topic, uint32_t queue_size, const CameraSubscriber::Callback &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
Base class for classes which update the occupancy map.
bool sleep() const
TFSIMD_FORCE_INLINE const tfScalar & getY() const
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
double y
void setIdentity()
#define ROS_ERROR_THROTTLE(rate,...)
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
static const StereoCameraModel::Parameters & RegisteredPSDKParams
predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion) ...
const boost::shared_ptr< tf::Transformer > & getTFClient() const
Parameters for Stereo-like devices.
const std::string TYPE_32FC1
const std::string TYPE_16UC1
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void setCameraParameters(float fx, float fy, float cx, float cy)
sets the camera parameters of the pinhole camera where the disparities were obtained. Usually the left camera
TFSIMD_FORCE_INLINE const tfScalar & w() const
bool hasMember(const std::string &name) const
bool getShapeTransform(mesh_filter::MeshHandle h, Eigen::Affine3d &transform) const
std::unique_ptr< LazyFreeSpaceUpdater > free_space_updater_
static WallTime now()
static void readXmlParam(XmlRpc::XmlRpcValue &params, const std::string &param_name, double *value)
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
virtual bool setParams(XmlRpc::XmlRpcValue &params)
Set updater params using struct that comes from parsing a yaml string. This must be called after setM...
bool updateTransformCache(const std::string &target_frame, const ros::Time &target_time)
void depthImageCallback(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
Mesh * createMeshFromShape(const Shape *shape)
void setImageSize(unsigned width, unsigned height)
sets the image size
double x
#define ROS_ERROR(...)
std::unique_ptr< mesh_filter::MeshFilter< mesh_filter::StereoCameraModel > > mesh_filter_
boost::function< bool(MeshHandle, Eigen::Affine3d &)> TransformCallback
unsigned int MeshHandle
#define ROS_DEBUG(...)
std::shared_ptr< const Shape > ShapeConstPtr


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23