Ejemplo n.º 1
0
    void
    viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
      mtx_.lock ();
      if (!cloud_ || !normals_)
      {
        mtx_.unlock ();
        return;
      }

      CloudConstPtr temp_cloud;
      pcl::PointCloud<pcl::Normal>::Ptr temp_normals;
      temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
      temp_normals.swap (normals_);
      mtx_.unlock ();

      if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
      {
        viz.addPointCloud (temp_cloud, "OpenNICloud");
        viz.resetCameraViewpoint ("OpenNICloud");
      }
      // Render the data
      if (new_cloud_)
      {
        viz.removePointCloud ("normalcloud");
        viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, temp_normals, 100, 0.05f, "normalcloud");
        new_cloud_ = false;
      }
    }
Ejemplo n.º 2
0
void Inspector::updateDepth(const openni_wrapper::Image& image,
                            const openni_wrapper::DepthImage& depth)
{
  frame_.depth_->setZero();
  ushort data[depth.getHeight() * depth.getWidth()];
  depth.fillDepthImageRaw(depth.getWidth(), depth.getHeight(), data);
  int i = 0;
  for(size_t y = 0; y < depth.getHeight(); ++y) {
    for(size_t x = 0; x < depth.getWidth(); ++x, ++i) {
      if(data[i] == depth.getNoSampleValue() || data[i] == depth.getShadowValue())
        continue;
      frame_.depth_->coeffRef(y, x) = data[i];
    }
  }

  if(dddm_ && use_intrinsics_)
    dddm_->undistort(frame_.depth_.get());

  frame_.img_ = oniToCV(image);
    
  Cloud::Ptr cloud(new Cloud);
  FrameProjector proj;
  proj.width_ = 640;
  proj.height_ = 480;
  proj.cx_ = proj.width_ / 2;
  proj.cy_ = proj.height_ / 2;
  proj.fx_ = 525;
  proj.fy_ = 525;
  proj.frameToCloud(frame_, cloud.get());
  pcd_vis_.updatePointCloud(cloud, "cloud");
  pcd_vis_.spinOnce(5);
}
Ejemplo n.º 3
0
//Draw the current particles
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
  ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
  if (particles && new_cloud_)
    {
      //Set pointCloud with particle's points
      pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
      for (size_t i = 0; i < particles->points.size (); i++)
	{
	  pcl::PointXYZ point;
          
	  point.x = particles->points[i].x;
	  point.y = particles->points[i].y;
	  point.z = particles->points[i].z;
	  particle_cloud->points.push_back (point);
	}

      //Draw red particles 
      {
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);

	if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
	  viz.addPointCloud (particle_cloud, red_color, "particle cloud");
      }
      return true;
    }
  else
    {
      return false;
    }
}
Ejemplo n.º 4
0
    void
    viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
      boost::mutex::scoped_lock lock (mtx_);
      if (!cloud_)
      {
        boost::this_thread::sleep(boost::posix_time::seconds(1));
        return;
      }

      CloudConstPtr temp_cloud;
      temp_cloud.swap (cloud_); //here we set cloud_ to null, so that

      if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
      {
        viz.addPointCloud (temp_cloud, "OpenNICloud");
        viz.resetCameraViewpoint ("OpenNICloud");
      }
      // Render the data 
      if (new_cloud_ && normals_)
      {
        viz.removePointCloud ("normalcloud");
        viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, normals_, 200, 0.05, "normalcloud");
        new_cloud_ = false;
      }
    }
Ejemplo n.º 5
0
 bool
 drawParticles (pcl::visualization::PCLVisualizer& viz)
 {
   ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
   if (particles)
   {
     if (visualize_particles_)
     {
       pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
       for (size_t i = 0; i < particles->points.size (); i++)
       {
         pcl::PointXYZ point;
         
         point.x = particles->points[i].x;
         point.y = particles->points[i].y;
         point.z = particles->points[i].z;
         particle_cloud->points.push_back (point);
       }
       
       {
         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color (particle_cloud, 250, 99, 71);
         if (!viz.updatePointCloud (particle_cloud, blue_color, "particle cloud"))
           viz.addPointCloud (particle_cloud, blue_color, "particle cloud");
       }
     }
     return true;
   }
   else
   {
     PCL_WARN ("no particles\n");
     return false;
   }
 }
Ejemplo n.º 6
0
    void
    viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
      if (!cloud_ || !new_cloud_)
      {
        boost::this_thread::sleep (boost::posix_time::milliseconds (1));
        return;
      }

      {
        boost::mutex::scoped_lock lock (mtx_);
        FPS_CALC ("visualization");
        CloudPtr temp_cloud;
        temp_cloud.swap (cloud_pass_);

        if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
        {
          viz.addPointCloud (temp_cloud, "OpenNICloud");
          viz.resetCameraViewpoint ("OpenNICloud");
        }
        // Render the data 
        if (new_cloud_ && cloud_hull_)
        {
          viz.removePointCloud ("hull");
          viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
        }
        new_cloud_ = false;
      }
    }
Ejemplo n.º 7
0
//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
  boost::mutex::scoped_lock lock (mtx_);
    
  if (!cloud_pass_)
    {
      std::this_thread::sleep_for(1s);
      return;
   }

  //Draw downsampled point cloud from sensor    
  if (new_cloud_ && cloud_pass_downsampled_)
    {
      CloudPtr cloud_pass;
      cloud_pass = cloud_pass_downsampled_;
    
      if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
	{
	  viz.addPointCloud (cloud_pass, "cloudpass");
	  viz.resetCameraViewpoint ("cloudpass");
	}
      bool ret = drawParticles (viz);
      if (ret)
        drawResult (viz);
    }
  new_cloud_ = false;
}
Ejemplo n.º 8
0
 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");
   }
 }
Ejemplo n.º 9
0
    void viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
//    cout << "PbMapMaker::viz_cb(...)\n";
      if (cloud1->empty())
      {
        boost::this_thread::sleep (boost::posix_time::milliseconds (10));
        return;
      }

      {
//        boost::mutex::scoped_lock lock (viz_mutex);

//        viz.removeAllShapes();
        viz.removeAllPointClouds();

        { //mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
          boost::mutex::scoped_lock updateLock(visualizationMutex);

//          if (!viz.updatePointCloud (cloud, "sphereCloud"))
//            viz.addPointCloud (sphereCloud, "sphereCloud");

          if (!viz.updatePointCloud (cloud1, "cloud1"))
            viz.addPointCloud (cloud1, "cloud1");

          if (!viz.updatePointCloud (cloud2, "cloud2"))
            viz.addPointCloud (cloud2, "cloud2");

          if (!viz.updatePointCloud (cloud3, "cloud3"))
            viz.addPointCloud (cloud3, "cloud3");

          if (!viz.updatePointCloud (cloud4, "cloud4"))
            viz.addPointCloud (cloud4, "cloud4");

          if (!viz.updatePointCloud (cloud5, "cloud5"))
            viz.addPointCloud (cloud5, "cloud5");

          if (!viz.updatePointCloud (cloud6, "cloud6"))
            viz.addPointCloud (cloud6, "cloud6");

          if (!viz.updatePointCloud (cloud7, "cloud7"))
            viz.addPointCloud (cloud7, "cloud7");

          if (!viz.updatePointCloud (cloud8, "cloud8"))
            viz.addPointCloud (cloud8, "cloud8");
        updateLock.unlock();
        }
      }
    }
 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"));
     }
 }
Ejemplo n.º 11
0
  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);
    }

  }
Ejemplo n.º 12
0
//Draw model reference point cloud
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
  ParticleXYZRPY result = tracker_->getResult ();
  Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);

  //move close to camera a little for better visualization
  transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
  CloudPtr result_cloud (new Cloud ());
  pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);

  //Draw blue model reference point cloud
  {
    pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);

    if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
      viz.addPointCloud (result_cloud, blue_color, "resultcloud");
  }
}
Ejemplo n.º 13
0
  void
  drawResult (pcl::visualization::PCLVisualizer& viz)
  {
    ParticleXYZRPY result = tracker_->getResult ();
    Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
    // move a little bit for better visualization
    transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
    RefCloudPtr result_cloud (new RefCloud ());

    if (!visualize_non_downsample_)
      pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
    else
      pcl::transformPointCloud<RefPointType> (*reference_, *result_cloud, transformation);

    {
      pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color (result_cloud, 0, 0, 255);
      if (!viz.updatePointCloud (result_cloud, red_color, "resultcloud"))
        viz.addPointCloud (result_cloud, red_color, "resultcloud");
    }
    
  }
Ejemplo n.º 14
0
template <typename PointT> void
tviewer::PointCloudObject<PointT>::refreshDataInVisualizer (pcl::visualization::PCLVisualizer& v)
{
  v.updatePointCloud (data_, name_);
}
Ejemplo n.º 15
0
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() )
//            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));
//            sprintf (name, "n%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(plane_i.semanticGroup));
            viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
          }

//          sprintf (name, "planeRaw_%02u", static_cast<unsigned>(i));
//          viz.addPointCloud (plane_i.planeRawPointCloudPtr, name);// contourPtr, planePointCloudPtr, polygonContourPtr

//          if(!configPbMap.makeClusters)
//          {
//          sprintf (name, "plane_%02u", static_cast<unsigned>(i));
//          pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[i%10], grn[i%10], blu[i%10]);
////          pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
//          viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
//          viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
//          }
//          else
//          {
//            sprintf (name, "plane_%02u", static_cast<unsigned>(i));
//            pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
//            viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
//            viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
//          }

//          sprintf (name, "planeBorder_%02u", static_cast<unsigned>(i));
//          pcl::visualization::PointCloudColorHandlerCustom <PointT> color2 (plane_i.contourPtr, 255, 255, 255);
//          viz.addPointCloud (plane_i.contourPtr, color2, name);// contourPtr, planePointCloudPtr, polygonContourPtr

//          //Edges
//          if(mPbMap.edgeCloudPtr->size() > 0)
//          {
//            sprintf (name, "planeEdge_%02u", static_cast<unsigned>(i));
//            pcl::visualization::PointCloudColorHandlerCustom <PointT> color4 (mPbMap.edgeCloudPtr, 255, 255, 0);
//            viz.addPointCloud (mPbMap.edgeCloudPtr, color4, name);// contourPtr, planePointCloudPtr, polygonContourPtr
//            viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
//
//            sprintf (name, "edge%u", static_cast<unsigned>(i));
//            viz.addLine (mPbMap.edgeCloudPtr->points.front(), mPbMap.edgeCloudPtr->points.back(), ared[3], agrn[3], ablu[3], 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);
        }

//if(configPbMap.makeClusters)
//  for(map<unsigned, std::vector<unsigned> >::iterator it=clusterize->groups.begin(); it != clusterize->groups.end(); it++)
//    for(size_t i=0; i < it->second.size(); i++)
//    {
//      unsigned planeID = it->second[i];
//      Plane &plane_i = mPbMap.vPlanes[planeID];
//      sprintf (name, "plane_%02u", static_cast<unsigned>(planeID));
//      pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[planeID%10], grn[planeID%10], blu[planeID%10]);
//      viz.addPointCloud (plane_i.planePointCloudPtr, color, name);// contourPtr, planePointCloudPtr, polygonContourPtr
//      viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, name);
//    }

        // Draw recognized plane labels
        if(mpPbMapLocaliser != NULL)
          for(map<string, pcl::PointXYZ>::iterator it = mpPbMapLocaliser->foundPlaces.begin(); it != mpPbMapLocaliser->foundPlaces.end(); it++)
            viz.addText3D (it->first, it->second, 0.3, 0.9, 0.9, 0.9, it->first);

      }
    }
  }
}
Ejemplo n.º 16
0
  void
  viz_cb (pcl::visualization::PCLVisualizer& viz)
  {
    boost::mutex::scoped_lock lock (mtx_);
    
    if (!cloud_pass_)
    {
      boost::this_thread::sleep (boost::posix_time::seconds (1));
      return;
    }
    
    if (new_cloud_ && cloud_pass_downsampled_)
    {
      CloudPtr cloud_pass;
      if (!visualize_non_downsample_)
        cloud_pass = cloud_pass_downsampled_;
      else
        cloud_pass = cloud_pass_;
      
      if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
        {
          viz.addPointCloud (cloud_pass, "cloudpass");
          viz.resetCameraViewpoint ("cloudpass");
        }
    }

    if (new_cloud_ && reference_)
    {
      bool ret = drawParticles (viz);
      if (ret)
      {
        drawResult (viz);
        
        // draw some texts
        viz.removeShape ("N");
        viz.addText ((boost::format ("number of Reference PointClouds: %d") % tracker_->getReferenceCloud ()->points.size ()).str (),
                     10, 20, 20, 1.0, 1.0, 1.0, "N");
        
        viz.removeShape ("M");
        viz.addText ((boost::format ("number of Measured PointClouds:  %d") % cloud_pass_downsampled_->points.size ()).str (),
                     10, 40, 20, 1.0, 1.0, 1.0, "M");
        
        viz.removeShape ("tracking");
        viz.addText ((boost::format ("tracking:        %f fps") % (1.0 / tracking_time_)).str (),
                     10, 60, 20, 1.0, 1.0, 1.0, "tracking");
        
        viz.removeShape ("downsampling");
        viz.addText ((boost::format ("downsampling:    %f fps") % (1.0 / downsampling_time_)).str (),
                     10, 80, 20, 1.0, 1.0, 1.0, "downsampling");
        
        viz.removeShape ("computation");
        viz.addText ((boost::format ("computation:     %f fps") % (1.0 / computation_time_)).str (),
                     10, 100, 20, 1.0, 1.0, 1.0, "computation");

        viz.removeShape ("particles");
        viz.addText ((boost::format ("particles:     %d") % tracker_->getParticles ()->points.size ()).str (),
                     10, 120, 20, 1.0, 1.0, 1.0, "particles");
        
      }
    }
    new_cloud_ = false;
  }
Ejemplo n.º 17
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());
    }
  }
Ejemplo n.º 18
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);
      }
    }
  }
}