void addCentroids(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
 {
   //    pcl::PointCloud< pcl::PointXYZRGBA>::Ptr voxel_centroid_cloud = supervoxels->getVoxelCentroidCloud();
   visualizer.addPointCloud(supervoxelcloud, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.95, name);
 }
void 
keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
{
  if (event.keyUp())
  {
    double opacity;
    switch (event.getKeyCode())
    {
      case '1':
        viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges");
        break;
      case '2':
        viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges");
        break;
      case '3':
        viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges");
        break;
      case '4':
        viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges");
        break;
      case '5':
        viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges");
        break;
    }
  }
}
 void addVoxels(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
 {
   pcl::PointCloud< pcl::PointXYZRGBA>::Ptr colored_voxel_cloud = supervoxels->getColoredVoxelCloud();
   visualizer.addPointCloud(colored_voxel_cloud, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.8, name);
 }
    void viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
      static bool first_time = true;
      double psize = 1.0,opacity = 1.0,linesize =1.0;
      std::string cloud_name ("cloud");
      boost::mutex::scoped_lock l(m_mutex);
      if (new_cloud)
      {
        //typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler;
        typedef pcl::visualization::PointCloudColorHandlerGenericField <pcl::PointXYZRGBNormal> ColorHandler;
        //ColorHandler Color_handler (normal_cloud);
        ColorHandler Color_handler (normal_cloud,"curvature");
        if (!first_time)
        {
          viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
          viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
          viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
          //viz.removePointCloud ("normalcloud");
          viz.removePointCloud ("cloud");
        }
        else
          first_time = false;

        //viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, 139, 0.1, "normalcloud");
        viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string("cloud"), 0);
        viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
        viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
        viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
        new_cloud = false;
      }
    }
Exemple #5
0
template <typename PointT> void
tviewer::PointCloudObject<PointT>::addDataToVisualizer (pcl::visualization::PCLVisualizer& v)
{
  v.addPointCloud (data_, name_);
  v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, name_);
  v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, visibility_, name_);
  if (use_fixed_color_ != 0)
  {
    float r, g, b;
    std::tie (r, g, b) = getRGBFromColor (color_);
    v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, r, g, b, name_);
  }
}
 void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, bool firstRun)
 {
   if(firstRun)
   {
     visualizer.addPointCloud(cloud, "cloud");
     visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
   }
   else
   {
     visualizer.updatePointCloud(cloud, "cloud");
     visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
   }
 }
    void OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame)
    {
        _viewer.removeAllPointClouds(0);
        _viewer.addPointCloud(mapFrame.makeShared(), "map");
        _viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map");
        _viewer.spinOnce();

    }
  void filterAdjacency(pcl::visualization::PCLVisualizer &visualizer)
  {
    visualizer.removeAllPointClouds();
    visualizer.removeAllShapes();
    std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
    supervoxels->getSupervoxelAdjacency(supervoxel_adjacency);

    pcl::PointCloud< pcl::PointXYZRGBA>::Ptr filtered_supervoxel(new pcl::PointCloud< pcl::PointXYZRGBA>);
    for(std::multimap<uint32_t, uint32_t>::iterator label_itr = supervoxel_adjacency.begin(); label_itr != supervoxel_adjacency.end();)
    {
      //First get the label
      uint32_t supervoxel_label = label_itr->first;
      //Now get the supervoxel corresponding to the label
      pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr supervoxel = supervoxel_clusters.at(supervoxel_label);

      int threshold = 7 - supervoxel->centroid_.z * 0.375;

      int count = supervoxel_adjacency.count(supervoxel_label);
      if(count >= threshold)
      {
        //Now we need to iterate through the adjacent supervoxels and make a point cloud of them
        pcl::PointCloud< pcl::PointXYZRGBA>::Ptr adjacent_supervoxel_centers(new pcl::PointCloud< pcl::PointXYZRGBA>);

        std::multimap<uint32_t, uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range(supervoxel_label).first;
        for(; adjacent_itr != supervoxel_adjacency.equal_range(supervoxel_label).second; ++adjacent_itr)
        {

          uint32_t snd_label = adjacent_itr->second;
          int snd_count = supervoxel_adjacency.count(snd_label);
          pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr neighbor_supervoxel = supervoxel_clusters.at(snd_label);
          int snd_threshold = 7 - neighbor_supervoxel->centroid_.z * 0.375;
          if(snd_count >= snd_threshold)
          {
            adjacent_supervoxel_centers->push_back(neighbor_supervoxel->centroid_);
            filtered_supervoxel->push_back(neighbor_supervoxel->centroid_);
          }
        }
        //Now we make a name for this polygon
        std::stringstream ss;
        ss << "supervoxel_" << supervoxel_label;
        //This function generates a "star" polygon mesh from the points given
        addSupervoxelConnectionsToViewer(supervoxel->centroid_, *adjacent_supervoxel_centers, ss.str(), visualizer);

      }
      label_itr = supervoxel_adjacency.upper_bound(supervoxel_label);
    }
    const std::string foo = "fassdfasdf";
    visualizer.addPointCloud(filtered_supervoxel, foo);
    visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, foo);
  }
 void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
 {
     double pointSize = 1.0;
     if(firstRun)
     {
         visualizer.addPointCloud(dispCloudPtr_, std::string("stuff"));
         visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
     }
     else
     {
         visualizer.updatePointCloud(dispCloudPtr_, std::string("stuff"));
         visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
     }
 }
  void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
  {

    const std::string cloudname = this->name;

    if(firstRun)
    {
      visualizer.addPointCloud(dispCloud, cloudname);
      visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
    }
    else
    {
      visualizer.updatePointCloud(dispCloud, cloudname);
      visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
    }

  }
Exemple #11
0
void viewerUpdate(pcl::visualization::PCLVisualizer& viewer)
{
    std::stringstream ss;
    ss << "Rawlog entry: " << rawlogEntry;
    viewer.removeShape ("text", 0);
    viewer.addText (ss.str(), 10,50, "text", 0);

    static mrpt::system::TTimeStamp last_time = INVALID_TIMESTAMP;

    {  // Mutex protected
    	std::lock_guard<std::mutex> lock(td_cs);
    	if (td.new_timestamp!=last_time)
    	{
    		last_time = td.new_timestamp;
			viewer.removePointCloud("cloud", 0);
			viewer.addPointCloud (td.new_cloud,"cloud",0);
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.0);

			const size_t N = td.new_cloud->size();
			std::cout << "Showing new point cloud of size=" << N << std::endl;

			static bool first = true;
			if (N && first)
			{
				first = false;
			//viewer.resetCameraViewpoint("cloud");
			}

#if 0
			std::cout << mrpt::format(
				"camera: clip = %f %f\n"
				"        focal = %f %f %f\n"
				"        pos   = %f %f %f\n"
				"        view  = %f %f %f\n",
				viewer.camera_.clip[0],viewer.camera_.clip[1],
				viewer.camera_.focal[0],viewer.camera_.focal[1],viewer.camera_.focal[2],
				viewer.camera_.pos[0],viewer.camera_.pos[1],viewer.camera_.pos[2],
				viewer.camera_.view[0],viewer.camera_.view[1],viewer.camera_.view[2]);
#endif
    	}
    }
}
    void
    viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
      boost::mutex::scoped_lock lock (mtx_);
      if (!keypoints_ && !cloud_)
      {
        boost::this_thread::sleep(boost::posix_time::seconds(1));
        return;
      }

      FPS_CALC ("visualization");
      viz.removePointCloud ("raw");
      pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler (cloud_);
      viz.addPointCloud<pcl::PointXYZRGBA> (cloud_, color_handler, "raw");

      if (!viz.updatePointCloud<pcl::PointXYZ> (keypoints_, "keypoints"))
      {
        viz.addPointCloud<pcl::PointXYZ> (keypoints_, "keypoints");
        viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5.0, "keypoints");
        viz.resetCameraViewpoint ("keypoints");
      }
    }
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         float th_dd, int max_search)
{
  CloudPtr cloud (new Cloud);
  fromROSMsg (*input, *cloud);

  pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
  pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
  ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
  ne.setNormalSmoothingSize (10.0f);
  ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
  ne.setInputCloud (cloud);
  ne.compute (*normal);

  TicToc tt;
  tt.tic ();

  //OrganizedEdgeBase<PointXYZRGBA, Label> oed;
  //OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
  //OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
  OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
  oed.setInputNormals (normal);
  oed.setInputCloud (cloud);
  oed.setDepthDisconThreshold (th_dd);
  oed.setMaxSearchNeighbors (max_search);
  oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
  PointCloud<Label> labels;
  std::vector<PointIndices> label_indices;
  oed.compute (labels, label_indices);
  print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");

  // Make gray point clouds
  for (int idx = 0; idx < (int)cloud->points.size (); idx++)
  {
    uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3;
    cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray;
  }

  // Display edges in PCLVisualizer
  viewer.setSize (640, 480);
  viewer.addCoordinateSystem (0.2f);
  viewer.addPointCloud (cloud, "original point cloud");
  viewer.registerKeyboardCallback(&keyboard_callback);

  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), 
    occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), 
    nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
    high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
    rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);

  pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
  pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
  pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
  pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
  pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);

  const int point_size = 2;
  viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");

  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();
    pcl_sleep(0.1);
  }

  // Combine point clouds and edge labels
  sensor_msgs::PointCloud2 output_edges;  
  toROSMsg (labels, output_edges);
  concatenateFields (*input, output_edges, output);
}
 void addNormals(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
 {
   pcl::PointCloud< pcl::PointNormal>::Ptr sv_normal_cloud = supervoxels->makeSupervoxelNormalCloud(supervoxel_clusters);
   visualizer.addPointCloudNormals<pcl::PointNormal> (sv_normal_cloud, 1, 0.05f, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, normalsColor[0], normalsColor[1], normalsColor[2], name);
 }
void run(pcl::RFFaceDetectorTrainer & fdrf, typename pcl::PointCloud<PointInT>::Ptr & scene_vis, pcl::visualization::PCLVisualizer & vis, bool heat_map,
    bool show_votes, const std::string & filename)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::copyPointCloud (*scene_vis, *scene);

  fdrf.setInputCloud (scene);

  if (heat_map)
  {
    pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
    fdrf.setFaceHeatMapCloud (intensity_cloud);
  }

  fdrf.detectFaces ();

  typedef typename pcl::traits::fieldList<PointInT>::type FieldListM;

  double rgb_m;
  bool exists_m;
  pcl::for_each_type < FieldListM > (pcl::CopyIfFieldExists<PointInT, double> (scene_vis->points[0], "rgb", exists_m, rgb_m));

  std::cout << "Color exists:" << static_cast<int> (exists_m) << std::endl;
  if (exists_m)
  {
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr to_visualize (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::copyPointCloud (*scene_vis, *to_visualize);

    pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB > handler_keypoints (to_visualize);
    vis.addPointCloud < pcl::PointXYZRGB > (to_visualize, handler_keypoints, "scene_cloud");
  } else
  {
    vis.addPointCloud (scene_vis, "scene_cloud");
  }

  if (heat_map)
  {
    pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
    fdrf.getFaceHeatMap (intensity_cloud);

    pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity");
    vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map");
  }

  if (show_votes)
  {
    //display votes_
    /*pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(new pcl::PointCloud<pcl::PointXYZ>());
     fdrf.getVotes(votes_cloud);
     pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0);
     vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud");
     vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
     vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
     vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/

    pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
    fdrf.getVotes2 (votes_cloud);
    pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity");
    vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud");
    vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
  }

  vis.addCoordinateSystem (0.1, "global");

  std::vector<Eigen::VectorXd> heads;
  fdrf.getDetectedFaces (heads);
  face_detection_apps_utils::displayHeads (heads, vis);

  if (SHOW_GT)
  {
    //check if there is ground truth data
    std::string pose_file (filename);
    boost::replace_all (pose_file, ".pcd", "_pose.txt");

    Eigen::Matrix4d pose_mat;
    pose_mat.setIdentity (4, 4);
    bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat);

    if (result)
    {
      Eigen::Vector3d ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
      Eigen::Vector3d trans_vector = Eigen::Vector3d (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3));
      std::cout << ea << std::endl;
      std::cout << trans_vector << std::endl;

      pcl::PointXYZ center_point;
      center_point.x = trans_vector[0];
      center_point.y = trans_vector[1];
      center_point.z = trans_vector[2];
      vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere");

      pcl::ModelCoefficients cylinder_coeff;
      cylinder_coeff.values.resize (7); // We need 7 values
      cylinder_coeff.values[0] = center_point.x;
      cylinder_coeff.values[1] = center_point.y;
      cylinder_coeff.values[2] = center_point.z;

      cylinder_coeff.values[3] = ea[0];
      cylinder_coeff.values[4] = ea[1];
      cylinder_coeff.values[5] = ea[2];

      Eigen::Vector3d vec = Eigen::Vector3d::UnitZ () * -1.;
      Eigen::Matrix3d matrixxx;

      matrixxx = Eigen::AngleAxisd (ea[0], Eigen::Vector3d::UnitX ()) * Eigen::AngleAxisd (ea[1], Eigen::Vector3d::UnitY ())
          * Eigen::AngleAxisd (ea[2], Eigen::Vector3d::UnitZ ());

      //matrixxx = pose_mat.block<3,3>(0,0);
      vec = matrixxx * vec;

      cylinder_coeff.values[3] = vec[0];
      cylinder_coeff.values[4] = vec[1];
      cylinder_coeff.values[5] = vec[2];

      cylinder_coeff.values[6] = 0.01;
      vis.addCylinder (cylinder_coeff, "cylinder");
    }
  }

  vis.setRepresentationToSurfaceForAllActors ();

  if (VIDEO)
  {
    vis.spinOnce (50, true);
  } else
  {
    vis.spin ();
  }

  vis.removeAllPointClouds ();
  vis.removeAllShapes ();
}
Exemple #16
0
  void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
  {
    const std::string &cloudname = this->name;

    if(firstRun)
    {
      visualizer.addPointCloud(cloud, cloudname);
      visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
    }
    else
    {
      visualizer.updatePointCloud(cloud, cloudname);
      visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
      visualizer.removeAllShapes();
    }

    visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0.2, 0, 0), 1, 0, 0, "X");
    visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0.2, 0), 0, 1, 0, "Y");
    visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0, 0.2), 0, 0, 1, "Z");

    tf::Vector3 origin = worldToCam * tf::Vector3(0, 0, 0);
    tf::Vector3 lineX = worldToCam * tf::Vector3(0.2, 0, 0);
    tf::Vector3 lineY = worldToCam * tf::Vector3(0, 0.2, 0);
    tf::Vector3 lineZ = worldToCam * tf::Vector3(0, 0, 0.2);

    pcl::PointXYZ pclOrigin(origin.x(), origin.y(), origin.z());
    pcl::PointXYZ pclLineX(lineX.x(), lineX.y(), lineX.z());
    pcl::PointXYZ pclLineY(lineY.x(), lineY.y(), lineY.z());
    pcl::PointXYZ pclLineZ(lineZ.x(), lineZ.y(), lineZ.z());

    visualizer.addLine(pcl::PointXYZ(0, 0, 0), pclOrigin, 1, 1, 1, "line");
    visualizer.addLine(pclOrigin, pclLineX, 1, 0, 0, "lineX");
    visualizer.addLine(pclOrigin, pclLineY, 0, 1, 0, "lineY");
    visualizer.addLine(pclOrigin, pclLineZ, 0, 0, 1, "lineZ");

    for(int i = 0; i < regions.size(); ++i)
    {
      const Region &region = regions[i];
      tf::Transform transform = worldToCam * region.transform;

      std::ostringstream oss;
      oss << "region_" << i;

      tf::Vector3 originB = transform * tf::Vector3(0, 0, 0);
      tf::Vector3 lineXB = transform * tf::Vector3(0.2, 0, 0);
      tf::Vector3 lineYB = transform * tf::Vector3(0, 0.2, 0);
      tf::Vector3 lineZB = transform * tf::Vector3(0, 0, 0.2);

      pcl::PointXYZ pclOriginB(originB.x(), originB.y(), originB.z());
      pcl::PointXYZ pclLineXB(lineXB.x(), lineXB.y(), lineXB.z());
      pcl::PointXYZ pclLineYB(lineYB.x(), lineYB.y(), lineYB.z());
      pcl::PointXYZ pclLineZB(lineZB.x(), lineZB.y(), lineZB.z());

      visualizer.addLine(pclOrigin, pclOriginB, 1, 1, 1, "line_" + oss.str());
      visualizer.addLine(pclOriginB, pclLineXB, 1, 0, 0, "lineX_" + oss.str());
      visualizer.addLine(pclOriginB, pclLineYB, 0, 1, 0, "lineY_" + oss.str());
      visualizer.addLine(pclOriginB, pclLineZB, 0, 0, 1, "lineZ_" + oss.str());

      Eigen::Vector3d translation;
      Eigen::Quaterniond rotation;

      tf::vectorTFToEigen(transform.getOrigin(), translation);
      tf::quaternionTFToEigen(transform.getRotation(), rotation);

      visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.width, region.height, region.depth, oss.str());
    }
  }