C++ PCLVisualizer::spin方法代码示例

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();
}

int main (int argc, char *argv[])
{

  if (argc < 3) {
    cout << "Enter the two files for registration ..\n";
    return -1;
  }

  pcl::console::setVerbosityLevel (pcl::console::L_DEBUG);

  string sourcefile = argv[1];
  string targetfile = argv[2];

  CloudPtr cloud1 ( new Cloud );
  CloudPtr cloud2 ( new Cloud );

  readPCDBinaryFile (sourcefile.c_str (), cloud1);
  readPCDBinaryFile (targetfile.c_str (), cloud2);


  //pcl::IterativeClosestPointNonLinear <Point, Point> icp;
  pcl::IterativeClosestPoint <Point, Point> icp;
  icp.setInputSource (cloud1);
  icp.setInputTarget (cloud2);
  icp.setMaximumIterations (2500);
  icp.setTransformationEpsilon (0.0000001);


  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
  //Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity ();
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  CloudPtr output (new Cloud);
  icp.align (*output, init_guess);

  //pcl::transformPointCloud (*cloud1, *output, icp.getFinalTransformation ());

  std::cout << "ICP NL has converged:" << icp.hasConverged ()
            << " score: " << icp.getFitnessScore () << std::endl;

  cout << "--------- Final transformation ---------------\n";
  cout << icp.getFinalTransformation () << "\n\n";


  CloudPtr cloud1_ig (new Cloud);
  pcl::transformPointCloud (*cloud1, *cloud1_ig, init_guess);

  PCLVisualizer* p = new PCLVisualizer (argc, argv, "Registration");
  int vp1 = 1;
  p->createViewPort (0.0, 0.0, 0.5, 1.0, vp1);
  int vp2 = 2;
  p->createViewPort (0.5, 0.0, 1.0, 1.0, vp2);
  p->setBackgroundColor (113.0/255, 113.0/255, 154.0/255);


  int color[3] = { 255, 0, 0};
  displayPointCloud (p, cloud1_ig, color, (char *) "opcloud1", vp1);

  color[0] = 0; color[1] = 255; color[2] = 0;
  displayPointCloud (p, cloud2, color, (char *) "opcloud2", vp1);

  color[0] = 0; color[1] = 255; color[2] = 0;
  displayPointCloud (p, output, color, (char *) "opcloud11", vp2);

  color[0] = 255; color[1] = 0; color[2] = 0;
  displayPointCloud (p, cloud2, color, (char *) "opcloud12", vp2);

  p->spin ();

  return 0;
}

void
visualize (const ModelLibrary::HashTable* hash_table)
{
  const ModelLibrary::HashTableCell* cells = hash_table->getVoxels ();
  size_t max_num_entries = 0;
  int id3[3], num_cells = hash_table->getNumberOfVoxels ();
  double cell_center[3];

  vtkPoints* sphere_centers = vtkPoints::New (VTK_DOUBLE);
  vtkDoubleArray* scale = vtkDoubleArray::New ();
  scale->SetNumberOfComponents(1);

  // Get the positions of the spheres (one sphere per full cell)
  for (int i = 0 ; i < num_cells ; ++i, ++cells)
  {
    // Does the cell have any entries?
    if (cells->size ())
    {
      // Insert the center of the current voxel in the point set
      hash_table->compute3dId (i, id3);
      hash_table->computeVoxelCenter (id3, cell_center);
      sphere_centers->InsertNextPoint (cell_center);

      // Save the number of entries
      scale->InsertNextValue (static_cast<double> (cells->size ()));

      // Get the max
      if (cells->size () > max_num_entries)
        max_num_entries = cells->size ();
    }
  }

  PCLVisualizer vis;
  vis.setBackgroundColor (0.1, 0.1, 0.1);

  // Is there something to visualize?
  if (max_num_entries)
  {
    // Compute the factor which maps all the scale values in (0, 1]
    double factor = 1.0/static_cast<double> (max_num_entries);
    // Set the true scale
    for (vtkIdType i = 0 ; i < scale->GetNumberOfTuples () ; ++i)
      scale->SetValue(i, factor*scale->GetValue (i));

    // Input for the glyph object: the centers + scale
    vtkPolyData *positions = vtkPolyData::New ();
    positions->SetPoints (sphere_centers);
    positions->GetPointData ()->SetScalars (scale);
    // The spheres
    vtkSphereSource* sphere_src = vtkSphereSource::New ();
    sphere_src->SetPhiResolution(8);
    sphere_src->SetThetaResolution(8);
    sphere_src->SetRadius(0.5*hash_table->getVoxelSpacing ()[0]);

    // Now that we have the points and the corresponding scalars, build the glyph object
    vtkGlyph3D *glyph = vtkGlyph3D::New ();
    glyph->SetScaleModeToScaleByScalar ();
    glyph->SetColorModeToColorByScalar ();
    glyph->SetInput (positions);
    glyph->SetSource (sphere_src->GetOutput ());
    glyph->Update ();

    vtkSmartPointer<vtkPolyData> glyph_output (glyph->GetOutput ());
    vis.addModelFromPolyData(glyph_output);

    // Cleanup
    glyph->Delete ();
    positions->Delete ();
    sphere_src->Delete ();
  }

  vis.spin();

  // Cleanup
  sphere_centers->Delete();
  scale->Delete();
}