51 static const std::string
LOGNAME =
"depth_image_octomap_updater";
54 : OccupancyMapUpdater(
"DepthImageUpdater")
56 , input_depth_transport_(nh_)
57 , model_depth_transport_(nh_)
58 , filtered_depth_transport_(nh_)
59 , filtered_label_transport_(nh_)
60 , image_topic_(
"depth")
62 , near_clipping_plane_distance_(0.3)
63 , far_clipping_plane_distance_(5.0)
64 , shadow_threshold_(0.04)
66 , padding_offset_(0.02)
68 , skip_vertical_pixels_(4)
69 , skip_horizontal_pixels_(6)
70 , image_callback_count_(0)
71 , average_callback_dt_(0.0)
82 DepthImageOctomapUpdater::~DepthImageOctomapUpdater()
91 sensor_type_ = (std::string)params[
"sensor_type"];
93 image_topic_ = (std::string)params[
"image_topic"];
95 queue_size_ = (
int)params[
"queue_size"];
97 readXmlParam(params,
"near_clipping_plane_distance", &near_clipping_plane_distance_);
98 readXmlParam(params,
"far_clipping_plane_distance", &far_clipping_plane_distance_);
99 readXmlParam(params,
"shadow_threshold", &shadow_threshold_);
100 readXmlParam(params,
"padding_scale", &padding_scale_);
101 readXmlParam(params,
"padding_offset", &padding_offset_);
103 readXmlParam(params,
"max_update_rate", &max_update_rate_);
104 readXmlParam(params,
"skip_vertical_pixels", &skip_vertical_pixels_);
105 readXmlParam(params,
"skip_horizontal_pixels", &skip_horizontal_pixels_);
106 if (params.
hasMember(
"filtered_cloud_topic"))
107 filtered_cloud_topic_ =
static_cast<const std::string&
>(params[
"filtered_cloud_topic"]);
109 ns_ =
static_cast<const std::string&
>(params[
"ns"]);
120 bool DepthImageOctomapUpdater::initialize()
122 tf_buffer_ = monitor_->getTFClient();
123 free_space_updater_ = std::make_unique<LazyFreeSpaceUpdater>(tree_);
126 mesh_filter_ = std::make_unique<mesh_filter::MeshFilter<mesh_filter::StereoCameraModel>>(
128 mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
129 mesh_filter_->setShadowThreshold(shadow_threshold_);
130 mesh_filter_->setPaddingOffset(padding_offset_);
131 mesh_filter_->setPaddingScale(padding_scale_);
132 mesh_filter_->setTransformCallback(
138 void DepthImageOctomapUpdater::start()
142 std::string prefix =
"";
146 pub_model_depth_image_ = model_depth_transport_.advertiseCamera(prefix +
"model_depth", 1);
147 if (!filtered_cloud_topic_.empty())
148 pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(prefix + filtered_cloud_topic_, 1);
150 pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(prefix +
"filtered_depth", 1);
152 pub_filtered_label_image_ = filtered_label_transport_.advertiseCamera(prefix +
"filtered_label", 1);
154 sub_depth_image_ = input_depth_transport_.subscribeCamera(image_topic_, queue_size_,
155 &DepthImageOctomapUpdater::depthImageCallback,
this, hints);
158 void DepthImageOctomapUpdater::stop()
163 void DepthImageOctomapUpdater::stopHelper()
165 sub_depth_image_.shutdown();
174 h = mesh_filter_->addMesh(
static_cast<const shapes::Mesh&
>(*shape));
179 h = mesh_filter_->addMesh(*m);
190 mesh_filter_->removeMesh(handle);
193 bool DepthImageOctomapUpdater::getShapeTransform(
mesh_filter::MeshHandle h, Eigen::Isometry3d& transform)
const
195 ShapeTransformCache::const_iterator it = transform_cache_.find(h);
196 if (it == transform_cache_.end())
207 bool host_is_big_endian()
212 char c[
sizeof(uint32_t)];
213 } bint = { 0x01020304 };
214 return bint.c[0] == 1;
220 void DepthImageOctomapUpdater::depthImageCallback(
const sensor_msgs::ImageConstPtr& depth_msg,
221 const sensor_msgs::CameraInfoConstPtr& info_msg)
224 depth_msg->header.frame_id.c_str(), depth_msg->encoding.c_str());
227 if (max_update_rate_ > 0)
236 if (image_callback_count_ < 1000)
238 if (image_callback_count_ > 0)
240 const double dt_start = (
start - last_depth_callback_start_).toSec();
241 if (image_callback_count_ < 2)
242 average_callback_dt_ = dt_start;
244 average_callback_dt_ =
245 ((image_callback_count_ - 1) * average_callback_dt_ + dt_start) / (double)image_callback_count_;
251 image_callback_count_ = 2;
253 ++image_callback_count_;
255 if (monitor_->getMapFrame().empty())
256 monitor_->setMapFrame(depth_msg->header.frame_id);
260 if (monitor_->getMapFrame() == depth_msg->header.frame_id)
261 map_h_sensor.setIdentity();
267 static const double TEST_DT = 0.005;
268 const int nt = (int)(0.5 + average_callback_dt_ / TEST_DT) * std::max(1, ((
int)queue_size_ / 2));
271 for (
int t = 0;
t < nt; ++
t)
274 tf2::fromMsg(tf_buffer_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id,
275 depth_msg->header.stamp),
286 static const unsigned int MAX_TF_COUNTER = 1000;
290 if (good_tf_ > MAX_TF_COUNTER)
292 const unsigned int div = MAX_TF_COUNTER / 10;
300 if (failed_tf_ > good_tf_)
302 "More than half of the image messages discared due to TF being unavailable (%u%%). "
303 "Transform error of sensor data: %s; quitting callback.",
304 (100 * failed_tf_) / (good_tf_ + failed_tf_), err.c_str());
307 if (failed_tf_ > MAX_TF_COUNTER)
309 const unsigned int div = MAX_TF_COUNTER / 10;
320 if (!updateTransformCache(depth_msg->header.frame_id, depth_msg->header.stamp))
326 const int w = depth_msg->width;
327 const int h = depth_msg->height;
331 params.
setCameraParameters(info_msg->K[0], info_msg->K[4], info_msg->K[2], info_msg->K[5]);
336 mesh_filter_->filter(&depth_msg->data[0], GL_UNSIGNED_SHORT);
342 depth_msg->encoding.c_str());
345 mesh_filter_->filter(&depth_msg->data[0], GL_FLOAT);
351 const double px = info_msg->K[2];
352 const double py = info_msg->K[5];
355 if (w >=
static_cast<int>(x_cache_.size()) || h >=
static_cast<int>(y_cache_.size()) || K2_ != px || K5_ != py ||
356 K0_ != info_msg->K[0] || K4_ != info_msg->K[4])
360 K0_ = info_msg->K[0];
361 K4_ = info_msg->K[4];
367 if (!(px == px && py == py && inv_fx_ == inv_fx_ && inv_fy_ == inv_fy_))
371 if (
static_cast<int>(x_cache_.size()) < w)
373 if (
static_cast<int>(y_cache_.size()) < h)
376 for (
int x = 0;
x < w; ++
x)
377 x_cache_[x] = (x - px) * inv_fx_;
379 for (
int y = 0;
y < h; ++
y)
380 y_cache_[y] = (y - py) * inv_fy_;
383 const octomap::point3d sensor_origin(map_h_sensor.getOrigin().getX(), map_h_sensor.getOrigin().getY(),
384 map_h_sensor.getOrigin().getZ());
392 std::size_t img_size = h * w;
393 if (filtered_labels_.size() < img_size)
394 filtered_labels_.resize(img_size);
397 const unsigned int* labels_row = &filtered_labels_[0];
398 mesh_filter_->getFilteredLabels(&filtered_labels_[0]);
403 sensor_msgs::Image debug_msg;
404 debug_msg.header = depth_msg->header;
405 debug_msg.height = h;
409 debug_msg.step = w *
sizeof(float);
410 debug_msg.data.resize(img_size *
sizeof(
float));
411 mesh_filter_->getModelDepth(
reinterpret_cast<float*
>(&debug_msg.data[0]));
412 pub_model_depth_image_.publish(debug_msg, *info_msg);
414 sensor_msgs::Image filtered_depth_msg;
415 filtered_depth_msg.header = depth_msg->header;
416 filtered_depth_msg.height = h;
417 filtered_depth_msg.width = w;
420 filtered_depth_msg.step = w *
sizeof(float);
421 filtered_depth_msg.data.resize(img_size *
sizeof(
float));
422 mesh_filter_->getFilteredDepth(
reinterpret_cast<float*
>(&filtered_depth_msg.data[0]));
423 pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg);
425 sensor_msgs::Image label_msg;
426 label_msg.header = depth_msg->header;
427 label_msg.height = h;
431 label_msg.step = w *
sizeof(
unsigned int);
432 label_msg.data.resize(img_size *
sizeof(
unsigned int));
433 mesh_filter_->getFilteredLabels(
reinterpret_cast<unsigned int*
>(&label_msg.data[0]));
435 pub_filtered_label_image_.publish(label_msg, *info_msg);
438 if (!filtered_cloud_topic_.empty())
440 sensor_msgs::Image filtered_msg;
441 filtered_msg.header = depth_msg->header;
442 filtered_msg.height = h;
443 filtered_msg.width = w;
446 filtered_msg.step = w *
sizeof(
unsigned short);
447 filtered_msg.data.resize(img_size *
sizeof(
unsigned short));
450 static std::vector<float> filtered_data;
451 if (filtered_data.size() < img_size)
452 filtered_data.resize(img_size);
454 mesh_filter_->getFilteredDepth(
reinterpret_cast<float*
>(&filtered_data[0]));
455 unsigned short* msg_data =
reinterpret_cast<unsigned short*
>(&filtered_msg.data[0]);
456 for (std::size_t i = 0; i < img_size; ++i)
459 msg_data[i] =
static_cast<unsigned short>(filtered_data[i] * 1000 + 0.5);
461 pub_filtered_depth_image_.publish(filtered_msg, *info_msg);
469 const int h_bound = h - skip_vertical_pixels_;
470 const int w_bound = w - skip_horizontal_pixels_;
474 const uint16_t* input_row =
reinterpret_cast<const uint16_t*
>(&depth_msg->data[0]);
476 for (
int y = skip_vertical_pixels_;
y < h_bound; ++
y, labels_row += w, input_row += w)
477 for (
int x = skip_horizontal_pixels_;
x < w_bound; ++
x)
482 float zz = (float)input_row[x] * 1e-3;
483 float yy = y_cache_[
y] * zz;
484 float xx = x_cache_[
x] * zz;
486 tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz);
487 occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
492 float zz = input_row[
x] * 1e-3;
493 float yy = y_cache_[
y] * zz;
494 float xx = x_cache_[
x] * zz;
496 tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz);
498 model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
504 const float* input_row =
reinterpret_cast<const float*
>(&depth_msg->data[0]);
506 for (
int y = skip_vertical_pixels_;
y < h_bound; ++
y, labels_row += w, input_row += w)
507 for (
int x = skip_horizontal_pixels_;
x < w_bound; ++
x)
511 float zz = input_row[
x];
512 float yy = y_cache_[
y] * zz;
513 float xx = x_cache_[
x] * zz;
515 tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz);
516 occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
520 float zz = input_row[
x];
521 float yy = y_cache_[
y] * zz;
522 float xx = x_cache_[
x] * zz;
524 tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz);
526 model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
541 occupied_cells.erase(model_cell);
549 tree_->updateNode(occupied_cell,
true);
555 tree_->unlockWrite();
556 tree_->triggerUpdateCallback();
559 free_space_updater_->pushLazyUpdate(occupied_cells_ptr, model_cells_ptr, sensor_origin);