void
run (float pair_width, float voxel_size, float max_coplanarity_angle, int num_hypotheses_to_show)
{
  PointCloud<PointXYZ>::Ptr scene_points (new PointCloud<PointXYZ> ()), model_points (new PointCloud<PointXYZ> ());
  PointCloud<Normal>::Ptr scene_normals (new PointCloud<Normal> ()), model_normals (new PointCloud<Normal> ());

  // Get the points and normals from the scene
  if ( !vtk_to_pointcloud ("../../test/tum_table_scene.vtk", *scene_points, *scene_normals) )
    return;

  vtkPolyData *vtk_model = vtkPolyData::New ();
  // Get the points and normals from the scene
  if ( !vtk_to_pointcloud ("../../test/tum_amicelli_box.vtk", *model_points, *model_normals, vtk_model) )
    return;

  // The recognition object
  ObjRecRANSAC objrec (pair_width, voxel_size);
  objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle);
  objrec.addModel (*model_points, *model_normals, "amicelli", vtk_model);
  // Switch to the test mode in which only oriented point pairs from the scene are sampled
  objrec.enterTestModeTestHypotheses ();

  // The visualizer
  PCLVisualizer viz;

  CallbackParameters params(objrec, viz, *scene_points, *scene_normals, num_hypotheses_to_show);
  viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (&params));

  // Run the recognition and update the viewer
  update (&params);

#ifdef _SHOW_SCENE_POINTS_
  viz.addPointCloud (scene_points, "cloud in");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
#endif

#ifdef _SHOW_OCTREE_POINTS_
  PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ());
  objrec.getSceneOctree ().getFullLeavesPoints (*octree_points);
  viz.addPointCloud (octree_points, "octree points");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points");
#endif

#if defined _SHOW_OCTREE_NORMALS_ && defined _SHOW_OCTREE_POINTS_
  PointCloud<Normal>::Ptr octree_normals (new PointCloud<Normal> ());
  objrec.getSceneOctree ().getNormalsOfFullLeaves (*octree_normals);
  viz.addPointCloudNormals<PointXYZ,Normal> (octree_points, octree_normals, 1, 6.0f, "normals out");
#endif

  // Enter the main loop
  while (!viz.wasStopped ())
  {
    //main loop of the visualizer
    viz.spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  vtk_model->Delete ();
}
TEST_F(test_map, map_image_corners) {
    SKIP_IF_FAST

    vector<Point> corners;
    // select points close to corners, avoid NaNs
    int margin = 160;
    corners.push_back(Point(margin, margin));
    corners.push_back(Point(img.cols - margin, margin));
    corners.push_back(Point(img.cols - margin, img.rows - margin));
    corners.push_back(Point(margin, img.rows - margin));
    PointCloudT corners3d;
    mapToCloud(corners3d, corners, img, cloud); 
    EXPECT_EQ(corners.size(), corners3d.size());
    EXPECT_EQ(4, corners.size());
    EXPECT_EQ(4, corners3d.size());
    for (PointCloudT::iterator it = corners3d.begin(),
         end = corners3d.end(); it != end; it++) {
        cout << boost::format("%d: %8f,%8f,%8f") % (it - corners3d.begin()) % (*it).x % (*it).y % (*it).z << endl;
    }
    // show the whole stuff
    PCLVisualizer vis;
    addPose(vis, PoseRT());
    addMarkerPolygon3d(vis, corners3d);
    vis.addPointCloud(cloud);
    vis.spin();
}
void run (const char* file_name, float voxel_size)
{
  PointCloud<PointXYZ>::Ptr points_in (new PointCloud<PointXYZ> ());
  PointCloud<PointXYZ>::Ptr points_out (new PointCloud<PointXYZ> ());

  // Get the points and normals from the input vtk file
  if ( !vtk_to_pointcloud (file_name, *points_in) )
    return;

  // Build the octree with the desired resolution
  ORROctree octree;
  octree.build (*points_in, voxel_size);

  // Now build the octree z-projection
  ORROctreeZProjection zproj;
  zproj.build (octree, 0.15f*voxel_size, 0.15f*voxel_size);

  // The visualizer
  PCLVisualizer viz;

  show_octree(&octree, viz);
  show_octree_zproj(&zproj, viz);

#ifdef _SHOW_POINTS_
  // Get the point of every full octree leaf
  octree.getFullLeafPoints (*points_out);

  // Add the point clouds
  viz.addPointCloud (points_in, "cloud in");
  viz.addPointCloud (points_out, "cloud out");

  // Change the appearance
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud out");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud out");
#endif

  // Enter the main loop
  while (!viz.wasStopped ())
  {
    //main loop of the visualizer
    viz.spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }
}
TEST_F(test_map, map_markers) {
    SKIP_IF_FAST

    vector<Point> red2d;
    vector<Point> green2d;
    vector<Point> yellow2d;
    vector<Point> blue2d;

    red2d.push_back(Point(727, 269));
    red2d.push_back(Point(716, 448));
    red2d.push_back(Point(806, 449));
    red2d.push_back(Point(834, 268));

    green2d.push_back(Point(336, 195));
    green2d.push_back(Point(1157, 198));

    yellow2d.push_back(Point(338, 183));
    yellow2d.push_back(Point(1154, 189));

    blue2d.push_back(Point(697, 86)); 
    blue2d.push_back(Point(371, 92)); 
    
    PointCloudT red3d;
    PointCloudT green3d;
    PointCloudT yellow3d;
    PointCloudT blue3d;

    mapToCloud(red3d, red2d, markerImg, cloud); 
    mapToCloud(green3d, green2d, markerImg, cloud); 
    mapToCloud(yellow3d, yellow2d, markerImg, cloud); 
    mapToCloud(blue3d, blue2d, markerImg, cloud); 
   
    /* 
    PointCloudT red3d_hscaled;
    PointCloudT green3d_hscaled;
    PointCloudT yellow3d_hscaled;
    PointCloudT blue3d_hscaled;

    mapToCloud(red3d_hscaled, red2d, markerImg, cloud, false); 
    mapToCloud(green3d_hscaled, green2d, markerImg, cloud, false); 
    mapToCloud(yellow3d_hscaled, yellow2d, markerImg, cloud, false); 
    mapToCloud(blue3d_hscaled, blue2d, markerImg, cloud, false); 

    EXPECT_EQ(red2d.size(), red3d_hscaled.size());
    EXPECT_EQ(green2d.size(), green3d_hscaled.size());
    EXPECT_EQ(yellow2d.size(), yellow3d_hscaled.size());
    EXPECT_EQ(blue2d.size(), blue3d_hscaled.size());
    */

    EXPECT_EQ(640, cloud.width);
    EXPECT_EQ(480, cloud.height);

    imshow("MapMarkers", markerImg);
    waitKey(-1);

    // TODO: PCLVisualizer segfaults for no reason...
    #ifdef ENABLE_PCLVIS
        // show the whole stuff
        PCLVisualizer vis;
        addPose(vis, PoseRT());

        addMarkerPolygon3d(vis, red3d, 255, 0, 0, "red");
        addMarkerPolygon3d(vis, green3d, 0, 216, 0, "green");
        addMarkerPolygon3d(vis, yellow3d, 255, 216, 0, "yellow");
        addMarkerPolygon3d(vis, blue3d, 0, 66, 255, "blue");
        /*
        addMarkerPolygon3d(vis, red3d_hscaled, 204, 153, 153, "red_hscaled");
        addMarkerPolygon3d(vis, green3d_hscaled, 153, 204, 153, "green_hscaled");
        addMarkerPolygon3d(vis, yellow3d_hscaled, 204, 204, 153, "yellow_hscaled");
        addMarkerPolygon3d(vis, blue3d_hscaled, 153, 153, 204, "blue_hscaled");*/

        vis.addPointCloud(cloud);
        vis.spin();
    #endif 
}
int
main (int argc, char** argv)
{
  // Read command line arguments.
  for (char c; (c = getopt (argc, argv, "s:hc:r:")) != -1; ) 
  {
    switch (c) 
    {
      case 'c':
        coordinate_frame = RangeImage::CoordinateFrame (strtol (optarg, NULL, 0));
        break;
      case 'r':
      {
        angular_resolution = strtod (optarg, NULL);
        cout << PVARN(angular_resolution);
        break;
      }
      case 's':
      {
        source = strtol (optarg, NULL, 0);
        if (source < 0  ||  source > 2)
        {
          cout << "Source "<<source<<" is unknown.\n";
          exit (0);
        }
        cout << "Receiving "<<(source==0 ? "point clouds" : (source==1 ? "depth images" : "disparity images"))<<".\n";
        break;
      }
      case 'h':
      default:
        printUsage (argv[0]);
        exit (0);
    }
  }
  angular_resolution = deg2rad (angular_resolution);
  
  ros::init (argc, argv, "tutorial_range_image_live_viewer");
  ros::NodeHandle node_handle;
  ros::Subscriber subscriber, subscriber2;
  
  if (node_handle.resolveName("input")=="/input")
    std::cerr << "Did you forget input:=<your topic>?\n";
  
  if (source == 0)
    subscriber = node_handle.subscribe ("input", 1, cloud_msg_cb);
  else if (source == 1)
  {
    if (node_handle.resolveName("input2")=="/input2")
      std::cerr << "Did you forget input2:=<your camera_info_topic>?\n";
    subscriber = node_handle.subscribe ("input", 1, depth_image_msg_cb);
    subscriber2 = node_handle.subscribe ("input2", 1, camera_info_msg_cb);
  }
  else if (source == 2)
    subscriber  = node_handle.subscribe ("input",  1, disparity_image_msg_cb);
  
  PCLVisualizer viewer (argc, argv, "Live viewer - point cloud");
  RangeImageVisualizer range_image_widget("Live viewer - range image");
  
  RangeImagePlanar* range_image_planar;
  RangeImage* range_image;
  if (source==0)
    range_image = new RangeImage;
  else
  {
    range_image_planar = new RangeImagePlanar;
    range_image = range_image_planar;
  }
  
  while (node_handle.ok ())
  {
    usleep (10000);
    viewer.spinOnce (10);
    RangeImageVisualizer::spinOnce ();
    ros::spinOnce ();
    
    if (source==0)
    {
      // If no new message received: continue
      if (!cloud_msg || cloud_msg == old_cloud_msg)
        continue;
      old_cloud_msg = cloud_msg;
      
      Eigen::Affine3f sensor_pose(Eigen::Affine3f::Identity());
      PointCloud<PointWithViewpoint> far_ranges;
      RangeImage::extractFarRanges(*cloud_msg, far_ranges);
      if (pcl::getFieldIndex(*cloud_msg, "vp_x")>=0)
      {
        PointCloud<PointWithViewpoint> tmp_pc;
        fromROSMsg(*cloud_msg, tmp_pc);
        Eigen::Vector3f average_viewpoint = RangeImage::getAverageViewPoint(tmp_pc);
        sensor_pose = Eigen::Translation3f(average_viewpoint) * sensor_pose;
      }
      PointCloud<PointType> point_cloud;
      fromROSMsg(*cloud_msg, point_cloud);
      range_image->createFromPointCloud(point_cloud, angular_resolution, deg2rad(360.0f), deg2rad(180.0f),
                                        sensor_pose, coordinate_frame, noise_level, min_range, border_size);
    }
    else if (source==1)
    {
      // If no new message received: continue
      if (!depth_image_msg || depth_image_msg == old_depth_image_msg || !camera_info_msg)
        continue;
      old_depth_image_msg = depth_image_msg;
      range_image_planar->setDepthImage(reinterpret_cast<const float*> (&depth_image_msg->data[0]),
                                        depth_image_msg->width, depth_image_msg->height,
                                        camera_info_msg->P[2],  camera_info_msg->P[6],
                                        camera_info_msg->P[0],  camera_info_msg->P[5], angular_resolution);
    }
    else if (source==2)
    {
      // If no new message received: continue
      if (!disparity_image_msg || disparity_image_msg == old_disparity_image_msg)
        continue;
      old_disparity_image_msg = disparity_image_msg;
      range_image_planar->setDisparityImage(reinterpret_cast<const float*> (&disparity_image_msg->image.data[0]),
                                            disparity_image_msg->image.width, disparity_image_msg->image.height,
                                            disparity_image_msg->f, disparity_image_msg->T, angular_resolution);
    }

    range_image_widget.setRangeImage (*range_image);
    viewer.removePointCloud ("range image cloud");
    pcl_visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud(*range_image,
                                                                                             200, 200, 200);
    viewer.addPointCloud (*range_image, color_handler_cloud, "range image cloud");
  }
}
void
run (float pair_width, float voxel_size, float max_coplanarity_angle)
{
  // The object recognizer
  ObjRecRANSAC objrec (pair_width, voxel_size);
  objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle);

  // The models to be loaded
  list<string> model_names;
  model_names.emplace_back("tum_amicelli_box");
  model_names.emplace_back("tum_rusk_box");
  model_names.emplace_back("tum_soda_bottle");

  list<PointCloud<PointXYZ>::Ptr> model_points_list;
  list<PointCloud<Normal>::Ptr> model_normals_list;
  list<vtkSmartPointer<vtkPolyData> > vtk_models_list;

  // Load the models and add them to the recognizer
  for (const auto &model_name : model_names)
  {
    PointCloud<PointXYZ>::Ptr model_points (new PointCloud<PointXYZ> ());
    model_points_list.push_back (model_points);

    PointCloud<Normal>::Ptr model_normals (new PointCloud<Normal> ());
    model_normals_list.push_back (model_normals);

    vtkSmartPointer<vtkPolyData> vtk_model = vtkSmartPointer<vtkPolyData>::New ();
    vtk_models_list.push_back (vtk_model);

    // Compose the file
    string file_name = string("../../test/") + model_name + string (".vtk");

    // Get the points and normals from the input model
    if ( !vtk2PointCloud (file_name.c_str (), *model_points, *model_normals, vtk_model) )
      continue;

    // Add the model
    objrec.addModel (*model_points, *model_normals, model_name, vtk_model);
  }

  // The scene in which the models are supposed to be recognized
  PointCloud<PointXYZ>::Ptr non_plane_points (new PointCloud<PointXYZ> ()), plane_points (new PointCloud<PointXYZ> ());
  PointCloud<Normal>::Ptr non_plane_normals (new PointCloud<Normal> ());

  // Detect the largest plane in the dataset
  if ( !loadScene ("../../test/tum_table_scene.vtk", *non_plane_points, *non_plane_normals, *plane_points) )
    return;

  // The parameters for the callback function and the visualizer
  PCLVisualizer viz;
  CallbackParameters params(objrec, viz, *non_plane_points, *non_plane_normals);
  viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (&params));

  // Run the recognition and update the viewer. Have a look at this method, to see how to start the recognition and use the result!
  update (&params);

  // From this line on: visualization stuff only!
#ifdef _SHOW_OCTREE_
  show_octree(objrec.getSceneOctree (), viz);
#endif

#ifdef _SHOW_SCENE_POINTS_
  viz.addPointCloud (scene_points, "scene points");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "scene points");
#endif

#ifdef _SHOW_OCTREE_POINTS_
  PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ());
  objrec.getSceneOctree ().getFullLeavesPoints (*octree_points);
  viz.addPointCloud (octree_points, "octree points");
//  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "octree points");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points");

  viz.addPointCloud (plane_points, "plane points");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.9, 0.9, 0.9, "plane points");
#endif

#if defined _SHOW_OCTREE_NORMALS_ && defined _SHOW_OCTREE_POINTS_
  PointCloud<Normal>::Ptr normals_octree (new PointCloud<Normal> ());
  objrec.getSceneOctree ().getNormalsOfFullLeaves (*normals_octree);
  viz.addPointCloudNormals<PointXYZ,Normal> (points_octree, normals_octree, 1, 6.0f, "normals out");
#endif

  // Enter the main loop
  while (!viz.wasStopped ())
  {
    //main loop of the visualizer
    viz.spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }
}
Exemple #7
0
void run (const char* file_name, float voxel_size)
{
    PointCloud<PointXYZ>::Ptr points_in (new PointCloud<PointXYZ> ());
    PointCloud<PointXYZ>::Ptr points_out (new PointCloud<PointXYZ> ());
    PointCloud<Normal>::Ptr normals_in (new PointCloud<Normal> ());
    PointCloud<Normal>::Ptr normals_out (new PointCloud<Normal> ());

    // Get the points and normals from the input vtk file
#ifdef _SHOW_OCTREE_NORMALS_
    if ( !vtk_to_pointcloud (file_name, *points_in, &(*normals_in)) )
        return;
#else
    if ( !vtk_to_pointcloud (file_name, *points_in, NULL) )
        return;
#endif

    // Build the octree with the desired resolution
    ORROctree octree;
    if ( normals_in->size () )
        octree.build (*points_in, voxel_size, &*normals_in);
    else
        octree.build (*points_in, voxel_size);

    // Get the first full leaf in the octree (arbitrary order)
    std::vector<ORROctree::Node*>::iterator leaf = octree.getFullLeaves ().begin ();

    // Get the average points in every full octree leaf
    octree.getFullLeavesPoints (*points_out);
    // Get the average normal at the points in each leaf
    if ( normals_in->size () )
        octree.getNormalsOfFullLeaves (*normals_out);

    // The visualizer
    PCLVisualizer viz;

    // Register a keyboard callback
    CallbackParameters params(octree, viz, leaf);
    viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (&params));

    // Add the point clouds
    viz.addPointCloud (points_in, "cloud in");
    viz.addPointCloud (points_out, "cloud out");
    if ( normals_in->size () )
        viz.addPointCloudNormals<PointXYZ,Normal> (points_out, normals_out, 1, 6.0f, "normals out");

    // Change the appearance
    viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
    viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud out");
    viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud out");

    // Convert the octree to a VTK poly-data object
//  show_octree(&octree, viz, true/*show full leaves only*/);

    updateViewer (octree, viz, leaf);

    // Enter the main loop
    while (!viz.wasStopped ())
    {
        //main loop of the visualizer
        viz.spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}