void
pcl::io::OpenNI2Grabber::depthCallback (DepthImage::Ptr depth_image, void*)
{
  if (num_slots<sig_cb_openni_point_cloud_rgb>   () > 0 ||
    num_slots<sig_cb_openni_point_cloud_rgba>  () > 0 ||
    num_slots<sig_cb_openni_image_depth_image> () > 0)
    rgb_sync_.add1 (depth_image, depth_image->getTimestamp ());

  if (num_slots<sig_cb_openni_point_cloud_i>  () > 0 ||
    num_slots<sig_cb_openni_ir_depth_image> () > 0)
    ir_sync_.add1 (depth_image, depth_image->getTimestamp ());

  if (depth_image_signal_->num_slots () > 0)
    depth_image_signal_->operator ()(depth_image);

  if (point_cloud_signal_->num_slots () > 0)
    point_cloud_signal_->operator ()(convertToXYZPointCloud (depth_image));
}

pcl::PointCloud<pcl::PointXYZI>::Ptr
pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, const DepthImage::Ptr &depth_image)
{
  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > cloud (new pcl::PointCloud<pcl::PointXYZI > ());

  cloud->header.seq = depth_image->getFrameID ();
  cloud->header.stamp = depth_image->getTimestamp ();
  cloud->header.frame_id = rgb_frame_id_;
  cloud->height = depth_height_;
  cloud->width = depth_width_;
  cloud->is_dense = false;

  cloud->points.resize (cloud->height * cloud->width);


  float fx = device_->getDepthFocalLength (); // Horizontal focal length
  float fy = device_->getDepthFocalLength (); // Vertcal focal length
  float cx = ((float)cloud->width - 1.f) / 2.f;  // Center x
  float cy = ((float)cloud->height - 1.f) / 2.f; // Center y

  // Load pre-calibrated camera parameters if they exist
  if (pcl_isfinite (depth_parameters_.focal_length_x))
    fx =  static_cast<float> (depth_parameters_.focal_length_x);

  if (pcl_isfinite (depth_parameters_.focal_length_y))
    fy =  static_cast<float> (depth_parameters_.focal_length_y);

  if (pcl_isfinite (depth_parameters_.principal_point_x))
    cx =  static_cast<float> (depth_parameters_.principal_point_x);

  if (pcl_isfinite (depth_parameters_.principal_point_y))
    cy =  static_cast<float> (depth_parameters_.principal_point_y);

  float fx_inv = 1.0f / fx;
  float fy_inv = 1.0f / fy;


  const uint16_t* depth_map = (const uint16_t*) depth_image->getData ();
  if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_)
  {
    // Resize the image if nessacery
    depth_resize_buffer_.resize(depth_width_ * depth_height_);
    depth_map = depth_resize_buffer_.data();
    depth_image->fillDepthImageRaw (depth_width_, depth_height_, (unsigned short*) depth_map );
  }

  const uint16_t* ir_map = (const uint16_t*) ir_image->getData ();
  if (ir_image->getWidth () != depth_width_ || ir_image->getHeight () != depth_height_)
  {
    // Resize the image if nessacery
    ir_resize_buffer_.resize(depth_width_ * depth_height_);
    ir_map = ir_resize_buffer_.data();
    ir_image->fillRaw (depth_width_, depth_height_, (unsigned short*) ir_map);
  }


  int depth_idx = 0;
  float bad_point = std::numeric_limits<float>::quiet_NaN ();

  for (int v = 0; v < depth_height_; ++v)
  {
    for (int u = 0; u < depth_width_; ++u, ++depth_idx)
    {
      pcl::PointXYZI& pt = cloud->points[depth_idx];
      /// @todo Different values for these cases
      // Check for invalid measurements
      if (depth_map[depth_idx] == 0 ||
        depth_map[depth_idx] == depth_image->getNoSampleValue () ||
        depth_map[depth_idx] == depth_image->getShadowValue ())
      {
        pt.x = pt.y = pt.z = bad_point;
      }
      else
      {
        pt.z = depth_map[depth_idx] * 0.001f; // millimeters to meters
        pt.x = (static_cast<float> (u) - cx) * pt.z * fx_inv;
        pt.y = (static_cast<float> (v) - cy) * pt.z * fy_inv;
      }

      pt.data_c[0] = pt.data_c[1] = pt.data_c[2] = pt.data_c[3] = 0;
      pt.intensity = static_cast<float> (ir_map[depth_idx]);
    }
  }
  cloud->sensor_origin_.setZero ();
  cloud->sensor_orientation_.setIdentity ();
  return (cloud);
}

bool LcmTranslator::
toLcm(const DepthImageView& iView, drc::map_image_t& oMessage,
      const float iQuantMax, const bool iCompress) {

  oMessage.view_id = iView.getId();

  // copy depth array
  DepthImage::Type imageType = DepthImage::TypeDepth;
  DepthImage::Ptr depthImage = iView.getDepthImage();
  float invalidValue = std::numeric_limits<float>::infinity();
  int numDepths = depthImage->getWidth() * depthImage->getHeight();
  std::vector<float> inDepths = depthImage->getData(imageType);
  std::vector<uint8_t> data((uint8_t*)(&inDepths[0]),
                            (uint8_t*)(&inDepths[0]) + numDepths*sizeof(float));
  float* outDepths = (float*)(&data[0]);

  // find extremal values and transform
  float zMin(std::numeric_limits<float>::infinity());
  float zMax(-std::numeric_limits<float>::infinity());
  for (int i = 0; i < numDepths; ++i) {
    float val = outDepths[i];
    if (val == invalidValue) continue;
    zMin = std::min(zMin, val);
    zMax = std::max(zMax, val);
  }
  if (zMin >= zMax) {
    zMin = 0;
    zMax = 1;
  }
  float zOffset(zMin), zScale(zMax-zMin);
  int bits = 11;
  if (iQuantMax == 0) {
    bits = 32;
    zScale = 1;
    zOffset = 0;
  }
  else {
    if (iQuantMax > 0) {
      bits = ceil(std::log2(zScale/iQuantMax));
      bits = std::min(bits, 16);
      bits = std::max(bits, 0);
    }
    zScale /= ((1 << bits) - 1);
  }
  for (int i = 0; i < numDepths; ++i) {
    float val = outDepths[i];
    if (val == invalidValue) continue;
    outDepths[i] = (outDepths[i]-zOffset)/zScale;
    if (bits < 32) outDepths[i] += 0.5f;  // for rounding
  }

  // store to blob
  DataBlob::Spec spec;
  spec.mDimensions.push_back(depthImage->getWidth());
  spec.mDimensions.push_back(depthImage->getHeight());
  spec.mStrideBytes.push_back(sizeof(float));
  spec.mStrideBytes.push_back(depthImage->getWidth()*sizeof(float));
  spec.mCompressionType = DataBlob::CompressionTypeNone;
  spec.mDataType = DataBlob::DataTypeFloat32;
  DataBlob blob;
  blob.setData(data, spec);

  // compress and convert
  DataBlob::CompressionType compressionType =
    iCompress ? DataBlob::CompressionTypeZlib : DataBlob::CompressionTypeNone;
  DataBlob::DataType dataType;
  if (bits <= 8) dataType = DataBlob::DataTypeUint8;
  else if (bits <= 16) dataType = DataBlob::DataTypeUint16;
  else dataType = DataBlob::DataTypeFloat32;
  blob.convertTo(compressionType, dataType);
  if (!toLcm(blob, oMessage.blob)) return false;

  // transform from reference to image
  Eigen::Projective3f xform = iView.getTransform();
  oMessage.data_scale = zScale;
  oMessage.data_shift = zOffset;
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      oMessage.transform[i][j] = xform(i,j);
    }
  }

  return true;
}