C++ Ptr::getPose方法代码示例

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 (),
                GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_visualization_->getColorBuffer ());

  // Draw the depth image
  glViewport (0, 0, range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();
  display_depth_image (range_likelihood_visualization_->getDepthBuffer (),
                       range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());


  poses.clear ();
  for (int i = 0; i < range_likelihood_->getRows (); ++i)
  {
    for (int j = 0; j < range_likelihood_->getCols (); ++j)
    {
      Camera camera (*camera_);
      camera.move ((j - range_likelihood_->getCols () / 2.0) * 0.1,
                   (i - range_likelihood_->getRows () / 2.0) * 0.1,
                   0.0);
      poses.push_back (camera.getPose ());
    }
  }
  std::cout << std::endl;

  TicToc tt;
  tt.tic();
  range_likelihood_->computeLikelihoods (reference, poses, scores);
  tt.toc();
  tt.toc_print();

  if (gllib::getGLError () != GL_NO_ERROR)
  {
    std::cerr << "GL Error: RangeLikelihood::computeLikelihoods: finished" << std::endl;
  }

#if 0
  std::cout << "score: ";
  for (size_t i = 0; i < scores.size (); ++i)
  {
    std::cout << " " << scores[i];
  }
  std::cout << std::endl;
#endif
//.........这里部分代码省略.........

void capture (Eigen::Isometry3d pose_in, string point_cloud_fname)
{
  // No reference image - but this is kept for compatability 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 (size_t i = 0; i<scores.size (); ++i)
  {
    std::cout << " " << scores[i];
  }
  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: simuation only
  //  1.28Hz: simuation, addNoise?    , getPointCloud, writeASCII
  // 33.33Hz: simuation, getPointCloud
  // 23.81Hz: simuation, getPointCloud, writeBinary
  // 14.28Hz: simuation, addNoise, getPointCloud, writeBinary
  // MODULE        TIME      FRACTION
  // simuation     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>);
  bool write_cloud = true;
  
  if (write_cloud)
  {
    // Read Color Buffer from the GPU before creating PointCloud:
    // By default the buffers are not read back from the GPU
    range_likelihood_->getColorBuffer ();
    range_likelihood_->getDepthBuffer ();  
    
    // Add noise directly to the CPU depth buffer 
    range_likelihood_->addNoise ();

    // Optional argument to save point cloud in global frame:
    // Save camera relative:
    //range_likelihood_->getPointCloud(pc_out);
    // Save in global frame - applying the camera frame:
    //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
    // Save in local frame
    range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
    // TODO: what to do when there are more than one simulated view?
    std::cout << pc_out->points.size() << " points written to file\n";
   
    pcl::PCDWriter writer;
    //writer.write (point_cloud_fname, *pc_out, false);  /// ASCII
    writer.writeBinary (point_cloud_fname, *pc_out);
    //cout << "finished writing file\n";
  }
  // Disabled all OpenCV stuff for now: dont want the dependency
  /*
  bool demo_other_stuff = false;
  if (demo_other_stuff && write_cloud)
  {
    write_score_image (range_likelihood_->getScoreBuffer ());  
    write_rgb_image (range_likelihood_->getColorBuffer ());  
    write_depth_image (range_likelihood_->getDepthBuffer ());  
    
    // Demo interacton with RangeImage:
    pcl::RangeImagePlanar rangeImage;
    range_likelihood_->getRangeImagePlanar (rangeImage);
 
    // display viewer: (currently seqfaults on exit of viewer)
    if (1==0){
      boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
      viewer = simpleVis(pc_out);
    
      while (!viewer->wasStopped ()){
//.........这里部分代码省略.........

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_->getWidth() + j];
    }
  }

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  std::vector<float> scores;
  int n = range_likelihood_->getRows ()*range_likelihood_->getCols ();
  for (int i = 0; i < n; ++i)
  {
    Camera camera(*camera_);
    camera.move(0.0,i*0.02,0.0);
    //camera.move(0.0,i*0.02,0.0);
    poses.push_back (camera.getPose ());
  }
  
  range_likelihood_->computeLikelihoods (reference, poses, scores);
  
  range_likelihood_->computeLikelihoods (reference, poses, scores);
  std::cout << "score: ";
  for (size_t i = 0; i<scores.size (); ++i)
  {
    std::cout << " " << scores[i];
  }
  std::cout << std::endl;

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

  delete [] reference;

  glDrawBuffer (GL_BACK);
  glReadBuffer (GL_BACK);

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

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

  glRasterPos2i (-1,-1);
  glDrawPixels (range_likelihood_->getWidth (), range_likelihood_->getHeight (),
                GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_->getColorBuffer ());

  // Draw the depth image
  glViewport (0, 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();
//  display_depth_image (range_likelihood_->getDepthBuffer ());
  display_depth_image (range_likelihood_->getDepthBuffer (),
                       range_likelihood_->getWidth (), range_likelihood_->getHeight ());
  
  
  // Draw the score image
  glViewport (0, range_likelihood_->getHeight (),
              range_likelihood_->getWidth (), range_likelihood_->getHeight ());
  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();
  display_score_image (range_likelihood_->getScoreBuffer ());

  glutSwapBuffers ();
  
  if (write_file_)
  {
    range_likelihood_->addNoise ();
    pcl::RangeImagePlanar rangeImage;
    range_likelihood_->getRangeImagePlanar (rangeImage);

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

    // Optional argument to save point cloud in global frame:
    // Save camera relative:
    //range_likelihood_->getPointCloud(pc_out);
    // Save in global frame - applying the camera frame:
    //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
    // Save in local frame
//.........这里部分代码省略.........