예제 #1
0
    void ObjectDetectionViewer::onUpdate(PCLVisualizer& visualizer) {
        Table::Collection tables;
        Object::Collection objects;
        {
            mutex::scoped_lock(resultMutex);
            tables = this->detectedTables;
            objects = this->detectedObjects;
        }
            
        visualizer.removeAllShapes();

        char strbuf[20];
        int tableId = 0;
        for (std::vector<Table::Ptr>::iterator table = tables->begin(); table != tables->end(); ++table) {
            sprintf(strbuf, "table%d", tableId++); 
            visualizer.addPolygon<Point>((*table)->getConvexHull(), 255, 0, 0, strbuf);
        }

        int objectId = 0;
        for (std::vector<Object::Ptr>::iterator object = objects->begin(); object != objects->end(); ++object) {
            sprintf(strbuf, "object%d", objectId++);
            visualizer.addPolygon<Point>((*object)->getBaseConvexHull(), 0, 255, 0, strbuf);
        }

        // TODO: print processing time in lower left corner (together with FPS)
    }
예제 #2
0
void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // // Container for original & filtered data
  // PCLPointCloud2* cloud = new PCLPointCloud2; 
  // PCLPointCloud2ConstPtr cloudPtr(cloud);
  PCLPointCloud2 result;

  // // Convert to PCL data type
  // pcl_conversions::toPCL(*input, *cloud);

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  PointCloud<PointT> cloud;
  fromROSMsg (*input, cloud);

   // flann::Index<flann::ChiSquareDistance<float> > index();

  //Create the detection algorithm
  Detection det(cloud.makeShared());

  //Detects the objects and run Recognition
  {

    boost::mutex::scoped_lock(db_mutex);

    Object::Collection objects = det.run();

    vector<ObjectModel> models;
    for(vector<Object::Ptr>::iterator obj = objects->begin(); obj != objects->end(); ++obj){
      ObjectModel model(*obj);
      models.push_back(model);
    }

    Recognition rec(models,"reference_objects");

    rec.run();
  }

  // Convert to ROS data type
  sensor_msgs::PointCloud2 output;
  pcl_conversions::moveFromPCL(result, output);

  // Publish the data
  pub.publish (output);
}