void PeopleDetector::addNewCloudToViewer(PointCloudT::Ptr cloud, pcl::visualization::PCLVisualizer::Ptr viewer_obj)
{
        viewer_obj->removeAllPointClouds();
        viewer_obj->removeAllShapes();
        pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
        viewer_obj->addPointCloud<PointT> (cloud, rgb, "input_cloud");
}
Esempio n. 2
0
 void
 setCam( /*  in: */ Eigen::Vector3d &pos, Eigen::Vector3d &up, Eigen::Vector3d &dir,
         ::pcl::visualization::PCLVisualizer::Ptr pViewerPtr )
 {
     vtkSmartPointer<vtkRendererCollection> rens =
             pViewerPtr->getRendererCollection();
     rens->InitTraversal ();
     vtkRenderer* renderer = NULL;
     int it = 0;
     while ((renderer = rens->GetNextItem ()) != NULL && (it < 1) )
     {
         vtkCamera& camera = *renderer->GetActiveCamera();
         camera.SetPosition  ( pos[0], pos[1], pos[2] );
         camera.SetViewUp    (  up[0],  up[1],  up[2] );
         camera.SetFocalPoint( dir[0], dir[1], dir[2] );
         ++it;
     }
     pViewerPtr->spinOnce();
 }
Esempio n. 3
0
void Visualizer::saveCloud(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,  pcl::visualization::PCLVisualizer::Ptr &visualizer)
{
  lock.lock();
  std::ostringstream oss, oss_cloud;
  oss_cloud << savePath << std::setfill('0') << std::setw(5) << saveFrameCloud << "_" << names[index] << ".pcd";
  oss << savePath << std::setfill('0') << std::setw(5) << saveFrameCloud << "_" << names[index] << ".png";

  outInfo("saving cloud: " << oss_cloud.str());
  //  pcl::io::savePCDFileASCII(oss.str(), *cloud);
  outInfo("saving screenshot: " << oss.str());
  visualizer->saveScreenshot(oss.str());
  ++saveFrameCloud;
  lock.unlock();
}
void showGT(const std::vector<ModelT> &model_set, const std::vector<poseT> &poses, pcl::visualization::PCLVisualizer::Ptr viewer)
{
    std::vector<pcl::PointCloud<myPointXYZ>::Ptr> rec;
    for( std::vector<ModelT>::const_iterator it = model_set.begin() ; it < model_set.end() ; it++ )
    {
        pcl::PointCloud<myPointXYZ>::Ptr cur_cloud(new pcl::PointCloud<myPointXYZ>()); 
        pcl::fromPCLPointCloud2(it->model_mesh->cloud, *cur_cloud);
        rec.push_back(cur_cloud);
    }
    
    int count = 0;
    Eigen::Quaternionf calibrate_rot(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f (1, 0, 0)));
    for(std::vector<poseT>::const_iterator it = poses.begin() ; it < poses.end() ; it++, count++ )
    {
        for( int i = 0 ; i < model_set.size() ; i++ )
        {
            if(model_set[i].model_label == it->model_name )
            {
                pcl::PointCloud<myPointXYZ>::Ptr cur_cloud(new pcl::PointCloud<myPointXYZ>()); 
                //pcl::transformPointCloud(*rec[i], *cur_cloud, Eigen::Vector3f (0, 0, 0), calibrate_rot);
                pcl::transformPointCloud(*rec[i], *cur_cloud, it->shift, it->rotation*calibrate_rot);

                std::stringstream ss;
                ss << count;

                viewer->addPolygonMesh<myPointXYZ>(cur_cloud, model_set[i].model_mesh->polygons, it->model_name+"_"+ss.str());
                if( it->model_name == "link" )
                    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.55, 0.05, it->model_name+"_"+ss.str());
                else if( it->model_name == "node" )
                    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.05, 0.55, 1.0, it->model_name+"_"+ss.str());
                break;
            }
        }
        
    }
}
Esempio n. 5
0
void visualize(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud, pcl::visualization::PCLVisualizer::Ptr pVisualizer)
{
    //init visualizer
	pVisualizer->setSize(640, 480);
	pVisualizer->setPosition(640, 0);
	pVisualizer->setBackgroundColor(0x00, 0x00, 0x00);
	pVisualizer->initCameraParameters();
	pVisualizer->addPointCloud(pCloud, "cloud");

	//reload visualizer content
	pVisualizer->spinOnce(1000000);
}
Esempio n. 6
0
            void fetchViewerZBuffer( /* out: */ cv::Mat & zBufMat,
                                     /*  in: */ ::pcl::visualization::PCLVisualizer::Ptr const& viewer, double zNear, double zFar )
            {
                std::cout << "saving vtkZBuffer...";

                vtkSmartPointer<vtkWindowToImageFilter> filter  = vtkSmartPointer<vtkWindowToImageFilter>::New();
                vtkSmartPointer<vtkImageShiftScale>     scale   = vtkSmartPointer<vtkImageShiftScale>::New();
                vtkSmartPointer<vtkWindow>              renWin  = viewer->getRenderWindow();

                // Create Depth Map
                filter->SetInput( renWin.GetPointer() );
                filter->SetMagnification(1);
                filter->SetInputBufferTypeToZBuffer();

                // scale
                scale->SetOutputScalarTypeToFloat();
                scale->SetInputConnection(filter->GetOutputPort());
                scale->SetShift(0.f);
                scale->SetScale(1.f);
                scale->Update();

                // fetch data
                vtkSmartPointer<vtkImageData> imageData = scale->GetOutput();
                int* dims = imageData->GetDimensions();
                if ( dims[2] > 1 )
                {
                    std::cerr << "am::util::pcl::fetchZBuffer(): ZDim != 1 !!!!" << std::endl;
                }

                // output
                zBufMat.create( dims[1], dims[0], CV_32FC1 );
                for ( int y = 0; y < dims[1]; ++y )
                {
                    for ( int x = 0; x < dims[0]; ++x )
                    {
                        float* pixel = static_cast<float*>( imageData->GetScalarPointer(x,y,0) );
                        float d = round(2.0 * zNear * zFar / (zFar + zNear - pixel[0] * (zFar - zNear)) * 1000.f);
                        zBufMat.at<float>( dims[1] - y - 1, x ) = (d > 10000.f) ? 0.f : d;

                        //data[ z * dims[1] * dims[0] + (dims[1] - y - 1) * dims[0] + x ] = pixel[0];//(pixel[0] == 10001) ? 0 : pixel[0];
                    }
                    //std::cout << std::endl;
                }
                //std::cout << std::endl;
            }
    virtual void processBufferElement(CloudConstPtr cloud) override {
        cloud_viewer_->spinOnce();

        if (!cloud_viewer_->updatePointCloud(cloud, "OpenNICloud")) {
            cloud_viewer_->setPosition(0, 0);
            cloud_viewer_->setSize(cloud->width, cloud->height);
            cloud_viewer_->addPointCloud(cloud, "OpenNICloud");
            cloud_viewer_->resetCameraViewpoint("OpenNICloud");
            cloud_viewer_->setCameraPosition(
                    0, 0, 0,        // Position
                    0, 0, 1,        // Viewpoint
                    0, -1, 0);    // Up
        }

        // TODO; Figure out how to achieve this
//        if(cloud_viewer_->wasStopped()) {
//            this->cloud_buffer_.reset(new Buffer<CloudConstPtr>());
//            this->stop();
//            cloud_viewer_->close();
//        }
    }
Esempio n. 8
0
void callback(const sensor_msgs::PointCloud2 &pc) {

  double t1, t2, avg_t;

  ROS_INFO("Received point cloud! Applying method %d",method_id);

  pcl::PointCloud<PointT>::Ptr scene_pc(new pcl::PointCloud<PointT>());

  pcl::PCLPointCloud2 pcl_pc;
  pcl_conversions::toPCL(pc, pcl_pc);

  pcl::fromPCLPointCloud2(pcl_pc, *scene_pc);

  if( scene_pc->empty() == true ) {
    ROS_ERROR("Recieved empty point cloud message!");
    return;
  }

  pcl::PointCloud<myPointXYZ>::Ptr scene_xyz(new pcl::PointCloud<myPointXYZ>());
  pcl::copyPointCloud(*scene_pc, *scene_xyz);

  if( view_flag == true )
  {
    viewer->removeAllPointClouds();
    viewer->addPointCloud(scene_xyz, "whole_scene");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.5, 0.5, 0.5, "whole_scene");
  }
  std::vector<poseT> all_poses;
  std::vector<poseT> link_poses, node_poses;

  std::cout << " ... processing:" << std::endl;
  switch(method_id)
  {
    case 0:
      t1 = get_wall_time();
      objrec->StandardRecognize(scene_xyz, all_poses);
      t2 = get_wall_time();
      break;
    case 1:
      {
        t1 = get_wall_time();
        int pose_num = 0;
        int iter = 0;
        pcl::PointCloud<myPointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<myPointXYZ>());
        filtered_cloud = scene_xyz;
        while(true)
        {
          //std::cerr<< "Recognizing Attempt --- " << iter << std::endl;
          objrec->StandardRecognize(filtered_cloud, all_poses);
          if( all_poses.size() <= pose_num )
            break;
          else
            pose_num = all_poses.size();

          pcl::PointCloud<myPointXYZ>::Ptr model_cloud = objrec->FillModelCloud(all_poses);
          filtered_cloud = FilterCloud(filtered_cloud, model_cloud);

          iter++;
        }
        t2 = get_wall_time();
        //std::cerr<< "Recognizing Done!!!" << std::endl;
        break;
      }
    case 2:
      {
        //std::vector<poseT> tmp_poses;
        t1 = get_wall_time();
        objrec->GreedyRecognize(scene_xyz, all_poses);
        t2 = get_wall_time();
        //all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses);
        break;
      }
    case 3:
      {
        pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>());
        pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>());
        splitCloud(scene_pc, link_cloud, node_cloud);

        t1 = get_wall_time();
        std::cout << " ... starting at " << t1 << std::endl;
        //std::vector<poseT> link_poses, node_poses;
        linkrec->StandardRecognize(link_cloud, link_poses);
        noderec->StandardRecognize(node_cloud, node_poses);
        t2 = get_wall_time();
        std::cout << " ... done at " << t2 << std::endl;

        all_poses.insert(all_poses.end(), link_poses.begin(), link_poses.end());
        all_poses.insert(all_poses.end(), node_poses.begin(), node_poses.end());
        break;
      }
    case 4:
      {
        pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>());
        pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>());
        splitCloud(scene_pc, link_cloud, node_cloud);

        t1 = get_wall_time();
        std::cout << " ... starting at " << t1 << std::endl;
        int pose_num = 0;
        std::vector<poseT> tmp_poses;
        int iter = 0;
        while(true)
        {
          //std::cerr<< "Recognizing Attempt --- " << iter << std::endl;
          list<AcceptedHypothesis> acc_hypotheses;

          
          std::cout << " ... generating" << std::endl;
          linkrec->genHypotheses(link_cloud, acc_hypotheses);
          noderec->genHypotheses(node_cloud, acc_hypotheses);

          std::cout << " ... merging" << std::endl;
          linkrec->mergeHypotheses(scene_xyz, acc_hypotheses, tmp_poses);

          if( tmp_poses.size() <= pose_num ) {
            break;
          } else {
            pose_num = tmp_poses.size();
            std::cout << "Number of merged hypotheses: " << tmp_poses.size() << std::endl;
          }

          pcl::PointCloud<myPointXYZ>::Ptr link_model = linkrec->FillModelCloud(tmp_poses);
          pcl::PointCloud<myPointXYZ>::Ptr node_model = noderec->FillModelCloud(tmp_poses);

          std::cout << " ... filtering" << std::endl;
          if( link_model->empty() == false ) {
            link_cloud = FilterCloud(link_cloud, link_model);
          }
          if( node_model->empty() == false) {
            node_cloud = FilterCloud(node_cloud, node_model);
          }
          iter++;
        }
        t2 = get_wall_time();
        std::cout << " ... done at " << t2 << std::endl;
        //std::cerr<< "Recognizing Done!!!" << std::endl;
        all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses);
        break;
      }
    case 5:
      {
        pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>());
        pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>());
        splitCloud(scene_pc, link_cloud, node_cloud);

        t1 = get_wall_time();
        linkrec->GreedyRecognize(link_cloud, link_poses);
        noderec->GreedyRecognize(node_cloud, node_poses);
        t2 = get_wall_time();

        std::vector<poseT> tmp_poses;
        tmp_poses.insert(tmp_poses.end(), link_poses.begin(), link_poses.end());
        tmp_poses.insert(tmp_poses.end(), node_poses.begin(), node_poses.end());

        //all_poses = tmp_poses;
        all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses);
        break;
      }
    default:
      ROS_ERROR("Code %d not recognized!",method_id);
      return;
  }

  if( view_flag == true )
  {
    switch(method_id)
    {
      case 0:
      case 1:
      case 2:
        objrec->visualize(viewer, all_poses);
        break;
      case 3:
      case 4:
      case 5:
        linkrec->visualize(viewer, all_poses);
        noderec->visualize(viewer, all_poses);
        break;
    }
    viewer->spin();
  }

  geometry_msgs::PoseArray links;
  geometry_msgs::PoseArray nodes;
  links.header.frame_id = pc.header.frame_id;
  nodes.header.frame_id = pc.header.frame_id;

  // publish poses as TF
  if (method_id < 3) {
    for (poseT &p: all_poses) {
      geometry_msgs::Pose pose;
      pose.position.x = p.shift[0];
      pose.position.y = p.shift[1];
      pose.position.z = p.shift[2];
      pose.orientation.x = p.rotation.x();
      pose.orientation.y = p.rotation.y();
      pose.orientation.z = p.rotation.z();
      pose.orientation.w = p.rotation.w();
      links.poses.push_back(pose);
    }
    pub_link.publish(links);
  } else {
    for (poseT &p: all_poses) {
      geometry_msgs::Pose pose;
      pose.position.x = p.shift[0];
      pose.position.y = p.shift[1];
      pose.position.z = p.shift[2];
      pose.orientation.x = p.rotation.x();
      pose.orientation.y = p.rotation.y();
      pose.orientation.z = p.rotation.z();
      pose.orientation.w = p.rotation.w();
      links.poses.push_back(pose);
    }
    for (poseT &p: all_poses) {
      geometry_msgs::Pose pose;
      pose.position.x = p.shift[0];
      pose.position.y = p.shift[1];
      pose.position.z = p.shift[2];
      pose.orientation.x = p.rotation.x();
      pose.orientation.y = p.rotation.y();
      pose.orientation.z = p.rotation.z();
      pose.orientation.w = p.rotation.w();
      nodes.poses.push_back(pose);
    }
    pub_link.publish(links);
    pub_link.publish(nodes);
  }

  std::cout << "Time 1 = " << t1 << std::endl;
  std::cout << "Time 2 = " << t2 << std::endl;
  ROS_INFO("... done.");

}
Esempio n. 9
0
int main(int argc, char** argv)
{

  ros::init(argc,argv,"greedy_objrec_ransac_node");
  ros::NodeHandle nh;

  pcl::console::parse_argument(argc, argv, "--m", method_id);
  pcl::console::parse_argument(argc, argv, "--p", path);
  pcl::console::parse_argument(argc, argv, "--t", trial_id);
  pcl::console::parse_argument(argc, argv, "--link", link_mesh_name);
  pcl::console::parse_argument(argc, argv, "--node", node_mesh_name);
  pcl::console::parse_argument(argc, argv, "--link-width", linkWidth);
  pcl::console::parse_argument(argc, argv, "--node-width", nodeWidth);
  pcl::console::parse_argument(argc, argv, "--voxel-size", voxelSize);

  linkrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(linkWidth, voxelSize));
  noderec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));
  objrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));

  bool view_flag = false;
  if( pcl::console::find_switch(argc, argv, "-v") == true )
    view_flag = true;

  if( view_flag == true )
  {
    viewer = pcl::visualization::PCLVisualizer::Ptr (new pcl::visualization::PCLVisualizer());
    viewer->initCameraParameters();
    viewer->addCoordinateSystem(0.1);
    viewer->setSize(640, 480);
    viewer->setCameraPosition(0, 0, 0.1, 0, 0, 1, 0, -1, 0);
  }

  std::stringstream mm;
  mm << method_id;

  if( method_id >= 3 )
  {
    ROS_INFO("Loading link mesh from \"%s.obj\"...",link_mesh_name.c_str());
    link_mesh = LoadMesh(link_mesh_name+".obj", "link");
    ROS_INFO("Loading node mesh from \"%s.obj\"...",node_mesh_name.c_str());
    node_mesh = LoadMesh(node_mesh_name+".obj", "node");
    mesh_set.push_back(link_mesh);
    mesh_set.push_back(node_mesh);
  }

  switch(method_id)
  {
    case 0:
    case 1:
    case 2:
      objrec->AddModel(link_mesh_name, "link");
      objrec->AddModel(node_mesh_name, "node");
      break;
    case 3:
    case 4:
    case 5:
      linkrec->AddModel(link_mesh_name, "link");
      noderec->AddModel(node_mesh_name, "node");
      break;
    default:return 0;
  }

  if (method_id < 3) {
    pub_link = nh.advertise<geometry_msgs::PoseArray>("objects",1000);
  } else {
    pub_link = nh.advertise<geometry_msgs::PoseArray>("links",1000);
    pub_node = nh.advertise<geometry_msgs::PoseArray>("nodes",1000);
  }
  sub = nh.subscribe("points_in",1,callback);

  // send and recieve messages
  while (ros::ok()) {
    ros::spin();
  }

  return 0;
}
Esempio n. 10
0
int
main (int argc, char** argv)
{
  srand (time (0));

  print_info ("The viewer window provides interactive commands; for help, press 'h' or 'H' from within the window.\n");

  if (argc < 2)
  {
    printHelp (argc, argv);
    return (-1);
  }

  // Command line parsing
  double bcolor[3] = {0, 0, 0};
  pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]);

  std::vector<double> fcolor_r, fcolor_b, fcolor_g;
  bool fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);

  std::vector<int> psize;
  pcl::console::parse_multiple_arguments (argc, argv, "-ps", psize);

  std::vector<double> opaque;
  pcl::console::parse_multiple_arguments (argc, argv, "-opaque", opaque);

  int mview = 0;
  pcl::console::parse_argument (argc, argv, "-multiview", mview);

  int normals = 0;
  pcl::console::parse_argument (argc, argv, "-normals", normals);
  double normals_scale = NORMALS_SCALE;
  pcl::console::parse_argument (argc, argv, "-normals_scale", normals_scale);

  int pc = 0;
  pcl::console::parse_argument (argc, argv, "-pc", pc);
  double pc_scale = PC_SCALE;
  pcl::console::parse_argument (argc, argv, "-pc_scale", pc_scale);

  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices   = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  std::vector<int> vtk_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".vtk");

  if (p_file_indices.size () == 0 && vtk_file_indices.size () == 0)
  {
    print_error ("No .PCD or .VTK file given. Nothing to visualize.\n");
    return (-1);
  }

  // Multiview enabled?
  int y_s = 0, x_s = 0;
  double x_step = 0, y_step = 0;
  if (mview)
  {
    print_highlight ("Multi-viewport rendering enabled.\n");

    if (p_file_indices.size () != 0)
    {
      y_s = static_cast<int>(floor (sqrt (static_cast<float>(p_file_indices.size ()))));
      x_s = y_s + static_cast<int>(ceil ((p_file_indices.size () / static_cast<double>(y_s)) - y_s));
      print_highlight ("Preparing to load "); print_value ("%d", p_file_indices.size ());
    }
    else if (vtk_file_indices.size () != 0)
    {
      y_s = static_cast<int>(floor (sqrt (static_cast<float>(vtk_file_indices.size ()))));
      x_s = y_s + static_cast<int>(ceil ((vtk_file_indices.size () / static_cast<double>(y_s)) - y_s));
      print_highlight ("Preparing to load "); print_value ("%d", vtk_file_indices.size ());
    }

    x_step = static_cast<double>(1.0 / static_cast<double>(x_s));
    y_step = static_cast<double>(1.0 / static_cast<double>(y_s));
    print_info (" files ("); print_value ("%d", x_s);    print_info ("x"); print_value ("%d", y_s);
    print_info (" / ");      print_value ("%f", x_step); print_info ("x"); print_value ("%f", y_step);
    print_info (")\n");
  }

  // Fix invalid multiple arguments
  if (psize.size () != p_file_indices.size () && psize.size () > 0)
    for (size_t i = psize.size (); i < p_file_indices.size (); ++i)
      psize.push_back (1);
  if (opaque.size () != p_file_indices.size () && opaque.size () > 0)
    for (size_t i = opaque.size (); i < p_file_indices.size (); ++i)
      opaque.push_back (1.0);

  // Create the PCLHistogramVisualizer object
  pcl::visualization::PCLHistogramVisualizer::Ptr ph;

  // Using min_p, max_p to set the global Y min/max range for the histogram
  float min_p = FLT_MAX; float max_p = -FLT_MAX;

  int k = 0, l = 0, viewport = 0;
  // Load the data files
  pcl::PCDReader pcd;
  pcl::console::TicToc tt;
  ColorHandlerPtr color_handler;
  GeometryHandlerPtr geometry_handler;

  // Go through VTK files
  for (size_t i = 0; i < vtk_file_indices.size (); ++i)
  {
    // Load file
    tt.tic ();
    print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[vtk_file_indices.at (i)]);
    vtkPolyDataReader* reader = vtkPolyDataReader::New ();
    reader->SetFileName (argv[vtk_file_indices.at (i)]);
    reader->Update ();
    vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput ();
    if (!polydata)
      return (-1);
    print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", polydata->GetNumberOfPoints ()); print_info (" points]\n");

    // Create the PCLVisualizer object here on the first encountered XYZ file
    if (!p)
      p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));

    // Multiview enabled?
    if (mview)
    {
      p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
      k++;
      if (k >= x_s)
      {
        k = 0;
        l++;
      }
    }

    // Add as actor
    std::stringstream cloud_name ("vtk-");
    cloud_name << argv[vtk_file_indices.at (i)] << "-" << i;
    p->addModelFromPolyData (polydata, cloud_name.str (), viewport);

    // Change the shape rendered color
    if (fcolorparam && fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name.str ());

    // Change the shape rendered point size
    if (psize.size () > 0)
      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());

    // Change the shape rendered opacity
    if (opaque.size () > 0)
      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
  }

  pcl::PCLPointCloud2::Ptr cloud;
  // Go through PCD files
  for (size_t i = 0; i < p_file_indices.size (); ++i)
  {
    cloud.reset (new pcl::PCLPointCloud2);
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;
    int version;

    print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[p_file_indices.at (i)]);

    tt.tic ();
    if (pcd.read (argv[p_file_indices.at (i)], *cloud, origin, orientation, version) < 0)
      return (-1);

    std::stringstream cloud_name;

    // ---[ Special check for 1-point multi-dimension histograms
    if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0]))
    {
      cloud_name << argv[p_file_indices.at (i)];

      if (!ph)
        ph.reset (new pcl::visualization::PCLHistogramVisualizer);
      print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n");

      pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p);
      ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ());
      continue;
    }

    cloud_name << argv[p_file_indices.at (i)] << "-" << i;

    // Create the PCLVisualizer object here on the first encountered XYZ file
    if (!p)
    {
      p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
      p->registerPointPickingCallback (&pp_callback, (void*)&cloud);
      Eigen::Matrix3f rotation;
      rotation = orientation;
      p->setCameraPosition (origin [0]                  , origin [1]                  , origin [2],
                        origin [0] + rotation (0, 2), origin [1] + rotation (1, 2), origin [2] + rotation (2, 2),
                                     rotation (0, 1),              rotation (1, 1),              rotation (2, 1));
    }

    // Multiview enabled?
    if (mview)
    {
      p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
      k++;
      if (k >= x_s)
      {
        k = 0;
        l++;
      }
    }

    if (cloud->width * cloud->height == 0)
    {
      print_error ("[error: no points found!]\n");
      return (-1);
    }
    print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", (int)cloud->width * cloud->height); print_info (" points]\n");
    print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ());

    // If no color was given, get random colors
    if (fcolorparam)
    {
      if (fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
        color_handler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i]));
      else
        color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));
    }
    else
      color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));

    // Add the dataset with a XYZ and a random handler
    geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2> (cloud));
    // Add the cloud to the renderer
    //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport);
    p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport);

    // If normal lines are enabled
    if (normals != 0)
    {
      int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
      if (normal_idx == -1)
      {
        print_error ("Normal information requested but not available.\n");
        continue;
        //return (-1);
      }
      //
      // Convert from blob to pcl::PointCloud
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
      pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
      cloud_xyz->sensor_origin_ = origin;
      cloud_xyz->sensor_orientation_ = orientation;

      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
      pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
      std::stringstream cloud_name_normals;
      cloud_name_normals << argv[p_file_indices.at (i)] << "-" << i << "-normals";
      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport);
    }

    // If principal curvature lines are enabled
    if (pc != 0)
    {
      if (normals == 0)
        normals = pc;

      int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
      if (normal_idx == -1)
      {
        print_error ("Normal information requested but not available.\n");
        continue;
        //return (-1);
      }
      int pc_idx = pcl::getFieldIndex (*cloud, "principal_curvature_x");
      if (pc_idx == -1)
      {
        print_error ("Principal Curvature information requested but not available.\n");
        continue;
        //return (-1);
      }
      //
      // Convert from blob to pcl::PointCloud
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
      pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
      cloud_xyz->sensor_origin_ = origin;
      cloud_xyz->sensor_orientation_ = orientation;
      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
      pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
      pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>);
      pcl::fromPCLPointCloud2 (*cloud, *cloud_pc);
      std::stringstream cloud_name_normals_pc;
      cloud_name_normals_pc << argv[p_file_indices.at (i)] << "-" << i << "-normals";
      int factor = (std::min)(normals, pc);
      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport);
      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ());
      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
      cloud_name_normals_pc << "-pc";
      p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport);
      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
    }

    // Add every dimension as a possible color
    if (!fcolorparam)
    {
      for (size_t f = 0; f < cloud->fields.size (); ++f)
      {
        if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba")
          color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2> (cloud));
        else
        {
          if (!isValidFieldName (cloud->fields[f].name))
            continue;
          color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (cloud, cloud->fields[f].name));
        }
        // Add the cloud to the renderer
        //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport);
        p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport);
      }
    }
    // Additionally, add normals as a handler
    geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<pcl::PCLPointCloud2> (cloud));
    if (geometry_handler->isCapable ())
      //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport);
      p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport);

    // Set immediate mode rendering ON
    p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ());

    // Change the cloud rendered point size
    if (psize.size () > 0)
      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());

    // Change the cloud rendered opacity
    if (opaque.size () > 0)
      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());

    // Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud
    //if (i == 0 && !p->cameraParamsSet ())
     // p->resetCameraViewpoint (cloud_name.str ());
  }
  
  
  ////////////////////////////////////////////////////////////////
  // Key binding for saving simulated point cloud:
  if (p)
    p->registerKeyboardCallback(simulate_callback, (void*)&p);
  
  
  
  
  int width = 640;
  int height = 480;
  window_width_ = width * 2;
  window_height_ = height * 2;

  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);

  for (int i=0; i<2048; i++)
  {
    float v = i/2048.0;
    v = powf(v, 3)* 6;
    t_gamma[i] = v*6*256;
  }    

  GLenum err = glewInit ();
  if (GLEW_OK != err)
  {
    std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
    exit (-1);
  }

  std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;

  if (glewIsSupported ("GL_VERSION_2_0"))
    std::cout << "OpenGL 2.0 supported" << std::endl;
  else
  {
    std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
    exit(1);
  }


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

  range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood(1, 1, height, width, scene_));

  // 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_->setCameraIntrinsicsParameters (640,480, 576.09757860,
            576.09757860, 321.06398107, 242.97676897);
  range_likelihood_->setComputeOnCPU (false);
  range_likelihood_->setSumOnCPU (true);
  range_likelihood_->setUseColor (true);
  initialize (argc, argv); 

  if (p)
    p->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]);
  // Read axes settings
  double axes  = 0.0;
  pcl::console::parse_argument (argc, argv, "-ax", axes);
  if (axes != 0.0 && p)
  {
    double ax_x = 0.0, ax_y = 0.0, ax_z = 0.0;
    pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false);
    // Draw XYZ axes if command-line enabled
    p->addCoordinateSystem (axes, ax_x, ax_y, ax_z, "reference");
  }

  // Clean up the memory used by the binary blob
  // Note: avoid resetting the cloud, otherwise the PointPicking callback will fail
  //cloud.reset ();

  if (ph)
  {
    print_highlight ("Setting the global Y range for all histograms to: "); print_value ("%f -> %f\n", min_p, max_p);
    ph->setGlobalYRange (min_p, max_p);
    ph->updateWindowPositions ();
    if (p)
      p->spin ();
    else
      ph->spin ();
  }
  else if (p)
    p->spin ();
}
 virtual void initializeThreadResources() override {
     cloud_viewer_.reset(new pcl::visualization::PCLVisualizer("PCL OpenNI2 Cloud"));
     cloud_viewer_->registerMouseCallback(&CloudViewer::mouseCallback, *this);
     cloud_viewer_->registerKeyboardCallback(&CloudViewer::keyboardCallback, *this);
 }