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