Method fillDisparityImage

io/src/image_depth.cpp:256–298

}

void
pcl::io::DepthImage::fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step) const
{
  if (width > wrapper_->getWidth () || height > wrapper_->getHeight ())
    THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height);

  if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0)
    THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height);

  if (line_step == 0)
    line_step = width * static_cast<unsigned> (sizeof (float));

  unsigned xStep = wrapper_->getWidth () / width;
  unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth ();

  unsigned bufferSkip = line_step - width * static_cast<unsigned> (sizeof (float));

  // Fill in the depth image data
  // iterate over all elements and fill disparity matrix: disp[x,y] = f * b / z_distance[x,y];
  // focal length is for the native image resolution -> focal_length = focal_length_ / xStep;
  float constant = focal_length_ * baseline_ * 1000.0f / static_cast<float> (xStep);

  const auto* inputBuffer = static_cast<const unsigned short*> (wrapper_->getData ());

  for (unsigned yIdx = 0, depthIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
  {
    for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++disparity_buffer)
    {
      unsigned short pixel = inputBuffer[depthIdx];
      if (pixel == 0 || pixel == no_sample_value_ || pixel == shadow_value_)
        *disparity_buffer = 0.0;
      else
        *disparity_buffer = constant / static_cast<float> (pixel);
    }

    // if we have padding
    if (bufferSkip > 0)
    {
      char* cBuffer = reinterpret_cast<char*> (disparity_buffer);
      disparity_buffer = reinterpret_cast<float*> (cBuffer + bufferSkip);
    }
  }
}