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

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
//.........这里部分代码省略.........

void display() {
  glViewport(range_likelihood_->width(), 0, range_likelihood_->width(), range_likelihood_->height());
  
  float* reference = new float[range_likelihood_->row_height() * range_likelihood_->col_width()];
  const float* depth_buffer = range_likelihood_->depth_buffer();
  // Copy one image from our last as a reference.
  for (int i=0, n=0; i<range_likelihood_->row_height(); ++i) {
    for (int j=0; j<range_likelihood_->col_width(); ++j) {
      reference[n++] = depth_buffer[i*range_likelihood_->width() + j];
    }
  }

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  std::vector<float> scores;
  int n = range_likelihood_->rows()*range_likelihood_->cols();
  for (int i=0; i<n; ++i) { // This is used when there is 
    Camera camera(*camera_);
    camera.move(0.0,0.0,0.0);
    //camera.move(0.0,i*0.02,0.0);
    poses.push_back(camera.pose());
  }
  float* depth_field =NULL;
  bool do_depth_field =false;
  range_likelihood_->compute_likelihoods(reference, poses, scores,depth_field,do_depth_field);
//  range_likelihood_->compute_likelihoods(reference, poses, scores);
  delete [] reference;
  delete [] depth_field;

  std::cout << "score: ";
  for (size_t i=0; i<scores.size(); ++i) {
    std::cout << " " << scores[i];
  }
  std::cout << std::endl;

  // Draw the depth image
  //  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  //  glColorMask(true, true, true, true);
  glDisable(GL_DEPTH_TEST);
  glViewport(0, 0, range_likelihood_->width(), range_likelihood_->height());
  //glViewport(0, 0, range_likelihood_->width(), range_likelihood_->height());

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  //glRasterPos2i(-1,-1);
  //glDrawPixels(range_likelihood_->width(), range_likelihood_->height(), GL_LUMINANCE, GL_FLOAT, range_likelihood_->depth_buffer());
  display_depth_image(range_likelihood_->depth_buffer());
  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
    range_likelihood_->getPointCloud(pc_out,false,camera_->pose());
    // TODO: what to do when there are more than one simulated view?
    
    pcl::PCDWriter writer;
    writer.write ("simulated_range_image.pcd", *pc_out,	false);  
    cout << "finished writing file\n";
    
    pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
    viewer.showCloud (pc_out);

    // Problem: vtk and opengl dont seem to play very well together
    // vtk seems to misbehavew after a little while and wont keep the window on the screen

    // method1: kill with [x] - but eventually it crashes:
    //while (!viewer.wasStopped ()){
    //}    
    
    // method2: eventually starts ignoring cin and pops up on screen and closes almost 
    // immediately
    //  cout << "enter 1 to cont\n";
    //  cin >> pause;
    //  viewer.wasStopped ();
    
    // method 3: if you interact with the window with keys, the window is not closed properly
    // TODO: use pcl methods as this time stuff is probably not cross playform
    struct timespec t;
    t.tv_sec = 100;
    //t.tv_nsec = (time_t)(20000000); // short sleep
    t.tv_nsec = (time_t)(0);  // long sleep - normal speed
    nanosleep(&t, NULL);
    write_file_ =0;
  }
}