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

void
tviewer::ArrowArrayObject::addDataToVisualizer (pcl::visualization::PCLVisualizer& v)
{
  for (size_t i = 0; i < data_->size (); ++i)
  {
    pcl::PointXYZ p1, p2;
    auto& arrow = data_->at (i);
    p1.getVector3fMap () = invert_direction_ ? arrow.target : arrow.source;
    p2.getVector3fMap () = invert_direction_ ? arrow.source : arrow.target;
    float r, g, b;
    std::tie (r, g, b) = getRGBFromColor (arrow.color);
    arrows_in_visualizer_.push_back (createId (i));
    // Counter-intuitively, the arrow head is attached to the first point, so
    // we pass the target first.
    v.addArrow (p2, p1, r, g, b, false, arrows_in_visualizer_.back ());
  }
}

void PbMapMaker::viz_cb (pcl::visualization::PCLVisualizer& viz)
{
  if (mPbMap.globalMapPtr->empty())
  {
    mrpt::system::sleep(10);
    return;
  }

  { mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);

    // Render the data
    {
      viz.removeAllShapes();
      viz.removeAllPointClouds();

      char name[1024];

      if(graphRepresentation)
      {
        for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
        {
          pcl::PointXYZ center(2*mPbMap.vPlanes[i].v3center[0], 2*mPbMap.vPlanes[i].v3center[1], 2*mPbMap.vPlanes[i].v3center[2]);
          double radius = 0.1 * sqrt(mPbMap.vPlanes[i].areaVoxels);
          sprintf (name, "sphere%u", static_cast<unsigned>(i));
          viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);

          if( !mPbMap.vPlanes[i].label.empty() )
              viz.addText3D (mPbMap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], mPbMap.vPlanes[i].label);
          else
          {
            sprintf (name, "P%u", static_cast<unsigned>(i));
            viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
          }

          // Draw edges
          if(!configPbMap.graph_mode) // Nearby neighbors
            for(set<unsigned>::iterator it = mPbMap.vPlanes[i].nearbyPlanes.begin(); it != mPbMap.vPlanes[i].nearbyPlanes.end(); it++)
            {
              if(*it > mPbMap.vPlanes[i].id)
                break;

              sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
              pcl::PointXYZ center_it(2*mPbMap.vPlanes[*it].v3center[0], 2*mPbMap.vPlanes[*it].v3center[1], 2*mPbMap.vPlanes[*it].v3center[2]);
              viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
            }
          else
            for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[i].neighborPlanes.begin(); it != mPbMap.vPlanes[i].neighborPlanes.end(); it++)
            {
              if(it->first > mPbMap.vPlanes[i].id)
                break;

              sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
              pcl::PointXYZ center_it(2*mPbMap.vPlanes[it->first].v3center[0], 2*mPbMap.vPlanes[it->first].v3center[1], 2*mPbMap.vPlanes[it->first].v3center[2]);
              viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);

              sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
              char commonObs[8];
              sprintf (commonObs, "%u", it->second);
              pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
              viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
            }

        }
      }
      else
      { // Regular representation

        if (!viz.updatePointCloud (mPbMap.globalMapPtr, "cloud"))
          viz.addPointCloud (mPbMap.globalMapPtr, "cloud");

        if(mpPbMapLocaliser != NULL)
          if(mpPbMapLocaliser->alignedModelPtr){
            if (!viz.updatePointCloud (mpPbMapLocaliser->alignedModelPtr, "model"))
              viz.addPointCloud (mpPbMapLocaliser->alignedModelPtr, "model");}

        sprintf (name, "PointCloud size %u", static_cast<unsigned>( mPbMap.globalMapPtr->size() ) );
        viz.addText(name, 10, 20);

        for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
        {
          Plane &plane_i = mPbMap.vPlanes[i];
          sprintf (name, "normal_%u", static_cast<unsigned>(i));
          pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
          pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
          pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
                              plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
                              plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
          viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);

          // Draw Ppal diretion
//          if( plane_i.elongation > 1.3 )
//          {
//            sprintf (name, "ppalComp_%u", static_cast<unsigned>(i));
//            pcl::PointXYZ pt3 = pcl::PointXYZ ( plane_i.v3center[0] + (0.2f * plane_i.v3PpalDir[0]),
//                                                plane_i.v3center[1] + (0.2f * plane_i.v3PpalDir[1]),
//                                                plane_i.v3center[2] + (0.2f * plane_i.v3PpalDir[2]));
//            viz.addArrow (pt3, plane_i.pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
//          }

//          if( !plane_i.label.empty() )
//.........这里部分代码省略.........

    void
    process ()
    {
      std::cout << "threshold: " << threshold_ << std::endl;
      std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
      unsigned char red [6] = {255,   0,   0, 255, 255,   0};
      unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
      unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};

      pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
      ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
      ne.setMaxDepthChangeFactor (0.02f);
      ne.setNormalSmoothingSize (20.0f);
      
      typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
      refinement_compare->setDistanceThreshold (threshold_, depth_dependent_);
      
      pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
      mps.setMinInliers (5000);
      mps.setAngularThreshold (0.017453 * 3.0); //3 degrees
      mps.setDistanceThreshold (0.03); //2cm
      mps.setRefinementComparator (refinement_compare);
      
      std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
      typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
      typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>);
      char name[1024];

      typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
      double normal_start = pcl::getTime ();
      ne.setInputCloud (cloud);
      ne.compute (*normal_cloud);
      double normal_end = pcl::getTime ();
      std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;

      double plane_extract_start = pcl::getTime ();
      mps.setInputNormals (normal_cloud);
      mps.setInputCloud (cloud);
      if (refine_)
        mps.segmentAndRefine (regions);
      else
        mps.segment (regions);
      double plane_extract_end = pcl::getTime ();
      std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl;
      std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;

      typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);

      viewer.removeAllPointClouds (0);
      viewer.removeAllShapes (0);
      pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
      viewer.addPointCloud<PointT> (cloud, single_color, "cloud");
      
      pcl::PlanarPolygon<PointT> approx_polygon;
      //Draw Visualization
      for (size_t i = 0; i < regions.size (); i++)
      {
        Eigen::Vector3f centroid = regions[i].getCentroid ();
        Eigen::Vector4f model = regions[i].getCoefficients ();
        pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
        pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
                                           centroid[1] + (0.5f * model[1]),
                                           centroid[2] + (0.5f * model[2]));
        sprintf (name, "normal_%d", unsigned (i));
        viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name));

        contour->points = regions[i].getContour ();        
        sprintf (name, "plane_%02d", int (i));
        pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
        viewer.addPointCloud (contour, color, name);

        pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_);
        approx_contour->points = approx_polygon.getContour ();
        std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl;
        typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
        
//        sprintf (name, "approx_plane_%02d", int (i));
//        viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
        for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx)
        {
          sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx));
          viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
        }
      }
    }

void  drawArrow(pcl::visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor (0.0, 0.0, 0.0);
	viewer.removeShape("line", 0);
	viewer.addArrow(o1, o2, 1.0, 0.0, 0.0, "line", 0);
}

void PbMapVisualizer::viz_cb (pcl::visualization::PCLVisualizer& viz)
{
  if (pbmap.globalMapPtr->empty())
  {
    mrpt::system::sleep(10);
    return;
  }

  // Render the data
  {
    viz.removeAllShapes();
    viz.removeAllPointClouds();

    char name[1024];

    if(graphRepresentation)
    {
//      cout << "show graphRepresentation\n";
      for(size_t i=0; i<pbmap.vPlanes.size(); i++)
      {
        pcl::PointXYZ center(2*pbmap.vPlanes[i].v3center[0], 2*pbmap.vPlanes[i].v3center[1], 2*pbmap.vPlanes[i].v3center[2]);
        double radius = 0.1 * sqrt(pbmap.vPlanes[i].areaVoxels);
//        cout << "radius " << radius << endl;
        sprintf (name, "sphere%u", static_cast<unsigned>(i));
        viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);

        if( !pbmap.vPlanes[i].label.empty() )
            viz.addText3D (pbmap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], pbmap.vPlanes[i].label);
        else
        {
          sprintf (name, "P%u", static_cast<unsigned>(i));
          viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
        }

        // Draw edges
//        for(set<unsigned>::iterator it = pbmap.vPlanes[i].nearbyPlanes.begin(); it != pbmap.vPlanes[i].nearbyPlanes.end(); it++)
//        {
//          if(*it > pbmap.vPlanes[i].id)
//            break;
//
//          sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
//          pcl::PointXYZ center_it(2*pbmap.vPlanes[*it].v3center[0], 2*pbmap.vPlanes[*it].v3center[1], 2*pbmap.vPlanes[*it].v3center[2]);
//          viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
//        }
        for(map<unsigned,unsigned>::iterator it = pbmap.vPlanes[i].neighborPlanes.begin(); it != pbmap.vPlanes[i].neighborPlanes.end(); it++)
        {
          if(it->first > pbmap.vPlanes[i].id)
            break;

          sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
          pcl::PointXYZ center_it(2*pbmap.vPlanes[it->first].v3center[0], 2*pbmap.vPlanes[it->first].v3center[1], 2*pbmap.vPlanes[it->first].v3center[2]);
          viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);

          sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
          char commonObs[8];
          sprintf (commonObs, "%u", it->second);
          pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
          viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
        }
      }
    }
    else
    { // Regular representation
      if (!viz.updatePointCloud (pbmap.globalMapPtr, "cloud"))
        viz.addPointCloud (pbmap.globalMapPtr, "cloud");

      sprintf (name, "PointCloud size %u", static_cast<unsigned>( pbmap.globalMapPtr->size() ) );
      viz.addText(name, 10, 20);

      for(size_t i=0; i<pbmap.vPlanes.size(); i++)
      {
        Plane &plane_i = pbmap.vPlanes[i];
//        sprintf (name, "normal_%u", static_cast<unsigned>(i));
        name[0] = *(mrpt::format("normal_%u", static_cast<unsigned>(i)).c_str());
        pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
        pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
        pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
                            plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
                            plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
        viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);

        if( !plane_i.label.empty() )
          viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label);
        else
        {
          sprintf (name, "n%u", static_cast<unsigned>(i));
          viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
        }

        sprintf (name, "approx_plane_%02d", int (i));
        viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name);
      }
    }
  }
}