Function display

simulation/tools/sim_test_performance.cpp:171–327

}

void
display()
{
  float* reference =
      new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
  const float* depth_buffer = range_likelihood_->getDepthBuffer();
  // Copy one image from our last as a reference.
  for (int i = 0, n = 0; i < range_likelihood_->getRowHeight(); ++i) {
    for (int j = 0; j < range_likelihood_->getColWidth(); ++j) {
      reference[n++] = depth_buffer
          [(i + range_likelihood_->getRowHeight() * range_likelihood_->getRows() / 2) *
               range_likelihood_->getWidth() +
           j + range_likelihood_->getColWidth() * range_likelihood_->getCols() / 2];
    }
  }

  float* reference_vis = new float[range_likelihood_visualization_->getRowHeight() *
                                   range_likelihood_visualization_->getColWidth()];
  const float* depth_buffer_vis = range_likelihood_visualization_->getDepthBuffer();
  // Copy one image from our last as a reference.
  for (int i = 0, n = 0; i < range_likelihood_visualization_->getRowHeight(); ++i) {
    for (int j = 0; j < range_likelihood_visualization_->getColWidth(); ++j) {
      reference_vis[n++] =
          depth_buffer_vis[i * range_likelihood_visualization_->getWidth() + j];
    }
  }

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
  std::vector<float> scores;

  // Render a single pose for visualization
  poses.clear();
  poses.push_back(camera_->getPose());
  range_likelihood_visualization_->computeLikelihoods(reference_vis, poses, scores);

  glDrawBuffer(GL_BACK);
  glReadBuffer(GL_BACK);

  // Draw the resulting images from the range_likelihood
  glViewport(range_likelihood_visualization_->getWidth(),
             0,
             range_likelihood_visualization_->getWidth(),
             range_likelihood_visualization_->getHeight());
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  // Draw the color image
  glColorMask(true, true, true, true);
  glClearColor(0, 0, 0, 0);
  glClearDepth(1);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glDisable(GL_DEPTH_TEST);

  glRasterPos2i(-1, -1);
  glDrawPixels(range_likelihood_visualization_->getWidth(),
               range_likelihood_visualization_->getHeight(),