Function capture

simulation/tools/sim_viewer.cpp:236–284

}

void
capture(Eigen::Isometry3d pose_in)
{
  // No reference image - but this is kept for compatibility with range_test_v2:
  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_->getWidth() + j];
    }
  }

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
  std::vector<float> scores;
  poses.push_back(pose_in);

  range_likelihood_->computeLikelihoods(reference, poses, scores);
  std::cout << "score: ";
  for (const float& score : scores) {
    std::cout << " " << score;
  }
  std::cout << std::endl;

  std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
            << camera_->getZ() << " " << camera_->getRoll() << " "
            << camera_->getPitch() << " " << camera_->getYaw() << std::endl;

  delete[] reference;

  // Benchmark Values for
  // 27840 triangle faces
  // 13670 vertices

  // 45.00Hz: simulation only
  //  1.28Hz: simulation, addNoise?    , getPointCloud, writeASCII
  // 33.33Hz: simulation, getPointCloud
  // 23.81Hz: simulation, getPointCloud, writeBinary
  // 14.28Hz: simulation, addNoise, getPointCloud, writeBinary
  // MODULE        TIME      FRACTION
  // simulation     0.02222   31%
  // addNoise      0.03	     41%
  // getPointCloud 0.008     11%
  // writeBinary   0.012     16%
  // total	   0.07222

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZRGB>);
}

void
print_Quaterniond(Eigen::Quaterniond r, std::stringstream& ss)