Exemplo n.º 1
0
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) {
        Camera camera(*camera_);
        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();
}
Exemplo n.º 2
0
int main(int argc, char** argv)
{
    camera_ = Camera::Ptr(new Camera());
    scene_ = Scene::Ptr(new Scene());

    range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_, 640));
//  range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
    //range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));

    window_width_ = range_likelihood_->width() * 2;
    window_height_ = range_likelihood_->height();

    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
    glutInitWindowPosition(10,10);
    glutInitWindowSize(window_width_, window_height_);
    glutCreateWindow("OpenGL range likelihood");

    initialize(argc, argv);

    glutReshapeFunc(on_reshape);
    glutDisplayFunc(display);
    glutIdleFunc(display);
    glutKeyboardFunc(on_keyboard);
    glutMouseFunc(on_mouse);
    glutMotionFunc(on_motion);
    glutPassiveMotionFunc(on_passive_motion);
    glutEntryFunc(on_entry);
    glutMainLoop();
}
Exemplo n.º 3
0
int main(int argc, char** argv)
{
  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);

  if (argc < 2){
    printHelp (argc, argv);
    return (-1);
  }  
  
  int i;
  for (i=0; i<2048; i++) {
    float v = i/2048.0;
    v = powf(v, 3)* 6;
    t_gamma[i] = v*6*256;
  }  

  camera_ = Camera::Ptr(new Camera());
  scene_ = Scene::Ptr(new Scene());

  range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_, 640));
//  range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
  //range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));

  // Actually corresponds to default parameters:
  range_likelihood_->set_CameraIntrinsicsParameters(640,480,576.09757860,
	    576.09757860, 321.06398107,  242.97676897);

  window_width_ = range_likelihood_->width() * 2;
  window_height_ = range_likelihood_->height();

  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
  glutInitWindowPosition(10,10);
  glutInitWindowSize(window_width_, window_height_);
  glutCreateWindow("OpenGL range likelihood");

  initialize(argc, argv);

  glutReshapeFunc(on_reshape);
  glutDisplayFunc(display);
  glutIdleFunc(display);
  glutKeyboardFunc(on_keyboard);
  glutMouseFunc(on_mouse);
  glutMotionFunc(on_motion);
  glutPassiveMotionFunc(on_passive_motion);
  glutEntryFunc(on_entry);
  glutMainLoop(); 
}
Exemplo n.º 4
0
void display_depth_image(const float* depth_buffer)
{
    int npixels = range_likelihood_->width() * range_likelihood_->height();
    uint8_t* depth_img = new uint8_t[npixels * 3];

    for (int i=0; i<npixels; i++) {
        float zn = 0.7;
        float zf = 20.0;
        float d = depth_buffer[i];
        float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
        float b = 0.075;
        float f = 580.0;
        uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
        if (kd < 0) kd = 0;
        else if (kd>2047) kd = 2047;

        int pval = t_gamma[kd];
        int lb = pval & 0xff;
        switch (pval>>8) {
        case 0:
            depth_img[3*i+0] = 255;
            depth_img[3*i+1] = 255-lb;
            depth_img[3*i+2] = 255-lb;
            break;
        case 1:
            depth_img[3*i+0] = 255;
            depth_img[3*i+1] = lb;
            depth_img[3*i+2] = 0;
            break;
        case 2:
            depth_img[3*i+0] = 255-lb;
            depth_img[3*i+1] = 255;
            depth_img[3*i+2] = 0;
            break;
        case 3:
            depth_img[3*i+0] = 0;
            depth_img[3*i+1] = 255;
            depth_img[3*i+2] = lb;
            break;
        case 4:
            depth_img[3*i+0] = 0;
            depth_img[3*i+1] = 255-lb;
            depth_img[3*i+2] = 255;
            break;
        case 5:
            depth_img[3*i+0] = 0;
            depth_img[3*i+1] = 0;
            depth_img[3*i+2] = 255-lb;
            break;
        default:
            depth_img[3*i+0] = 0;
            depth_img[3*i+1] = 0;
            depth_img[3*i+2] = 0;
            break;

        }

    }

    //  glRasterPos2i(-1,-1);
    //  glDrawPixels(range_likelihood_->width(), range_likelihood_->height(), GL_LUMINANCE, GL_FLOAT, range_likelihood_->depth_buffer());

    glRasterPos2i(-1,-1);
    glDrawPixels(range_likelihood_->width(), range_likelihood_->height(), GL_RGB, GL_UNSIGNED_BYTE, depth_img);

    delete [] depth_img;
}
Exemplo n.º 5
0
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;
  }
}