Method RangeLikelihood

simulation/src/range_likelihood.cpp:275–468

}

pcl::simulation::RangeLikelihood::RangeLikelihood(
    int rows, int cols, int row_height, int col_width, Scene::Ptr scene)
: scene_(scene)
, rows_(rows)
, cols_(cols)
, row_height_(row_height)
, col_width_(col_width)
, sum_reduce_(cols * col_width, rows * row_height, max_level(col_width, row_height))
{
  height_ = rows_ * row_height;
  width_ = cols_ * col_width;

  depth_buffer_ = new float[width_ * height_];
  color_buffer_ = new std::uint8_t[width_ * height_ * 3];

  // Set Default Camera Intrinstic Parameters. techquad
  // Correspond closely to those stated here:
  // http://www.ros.org/wiki/kinect_calibration/technical
  camera_width_ = 640;
  camera_height_ = 480;
  camera_fx_ = 576.09757860f;
  camera_fy_ = 576.09757860f;
  camera_cx_ = 321.06398107f;
  camera_cy_ = 242.97676897f;

  z_near_ = 0.7f;
  z_far_ = 20.0f;

  which_cost_function_ = 2; // default to commonly used meter based function

  // default lhood parameters - these should always be set by the user
  // so might want to add to constructor eventually:
  sigma_ = 0.1;
  floor_proportion_ = 0.9;

  int height = rows * row_height;
  int width = cols * col_width;

  // For now we only support a limited size texture
  assert(height > 0 && height <= 8192 && width > 0 && width <= 8192);
  // throw std::runtime_error "

  // Allocate framebuffer
  glGenRenderbuffers(1, &depth_render_buffer_);
  glBindRenderbuffer(GL_RENDERBUFFER, depth_render_buffer_);
  glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT32, width, height);
  glBindRenderbuffer(GL_RENDERBUFFER, 0);

  glGenRenderbuffers(1, &color_render_buffer_);
  glBindRenderbuffer(GL_RENDERBUFFER, color_render_buffer_);
  glRenderbufferStorage(GL_RENDERBUFFER, GL_RGB8UI, width, height);
  glBindRenderbuffer(GL_RENDERBUFFER, 0);

  // Setup texture to store depth image
  glGenTextures(1, &depth_texture_);
  glBindTexture(GL_TEXTURE_2D, depth_texture_);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);