void start(const Mode mode)
{
this->mode = mode;
running = true;
std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info";
std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info";
// std::cout << "test1..." << std::endl;
image_transport::TransportHints hints("raw");
subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints);
subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints);
subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize);
subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize);
// std::cout << "test2..." << std::endl;
syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
spinner.start();
std::cout << "waiting for kinect2_bridge..." << std::endl;
std::chrono::milliseconds duration(1);
while(!updateImage || !updateCloud)
{
if(!ros::ok())
{
return;
}
std::this_thread::sleep_for(duration);
}
std::cout << "kinect2_bridge is running.." << std::endl;
cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>());
cloud->height = color.rows;
cloud->width = color.cols;
cloud->is_dense = false;
cloud->points.resize(cloud->height * cloud->width);
createLookup(this->color.cols, this->color.rows);
//
// // Create the filtering object
// // pcl::PassThrough<pcl::PointXYZRGBA> pass;
// // pass.setInputCloud (cloud);
// // pass.setFilterFieldName ("z");
// // pass.setFilterLimits (0.0, 1.0);
// // //pass.setFilterLimitsNegative (true);
// // pass.filter (*cloud);
cloudViewer();
}
int
main (int argc, char **argv)
{
std::cout << "main function" << std::endl;
pcl::Grabber* interface;
ros::NodeHandle *nh;
ros::Subscriber sub;
ros::Publisher clusterPub, normalPub, planePub,msgPub;
bool spawnObject = true;
K2G *k2g;
processor freenectprocessor = OPENGL;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud;
std::cout << "ros node initialized" << std::endl;
ros::init(argc, argv, "hlpr_single_plane_segmentation",ros::init_options::NoSigintHandler);
nh = new ros::NodeHandle("~");
// nh->getParam("segmentation/viz", viz_);
// std::cout<<"viz is set to " << viz_ << endl;
parsedArguments pA;
if(parseArguments(argc, argv, pA, *nh) < 0)
return 0;
viz_ = pA.viz;
ridiculous_global_variables::ignore_low_sat = pA.saturation_hack;
ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value;
ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value;
RansacSinglePlaneSegmentation multi_plane_app;
pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr ncloud_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Label>::Ptr label_ptr (new pcl::PointCloud<pcl::Label>);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh);
if(viz_) {
viewer = cloudViewer(cloud_ptr);
multi_plane_app.setViewer(viewer);
}
float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0};
multi_plane_app.setWorkingVolumeThresholds(workSpace);
msgPub = nh->advertise<hlpr_perception_msgs::SegClusters>(segOutRostopic,5);
switch (pA.pc_source)
{
case 0:
{
std::cout << "Using ros topic as input" << std::endl;
sub = nh->subscribe<sensor_msgs::PointCloud2>(pA.ros_topic, 1, cloud_cb_ros_);
break;
}
case 1:
{
std::cout << "Using the openni device" << std::endl;
interface = new pcl::OpenNIGrabber ();
boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
break;
}
case 2:
default:
{
std::cout << "Using kinect v2" << std::endl;
freenectprocessor = static_cast<processor>(pA.freenectProcessor);
k2g = new K2G(freenectprocessor, true);
cloud = k2g->getCloud();
prev_cloud = cloud;
gotFirst = true;
break;
}
}
signal(SIGINT, interruptFn);
while (!interrupt && (!viz_ || !viewer->wasStopped ()))
{
if(pA.ros_node)
{
ros::spinOnce();
}
if(viz_)
viewer->spinOnce(20);
if(!gotFirst)
continue;
int selected_cluster_index = -1;
if(cloud_mutex.try_lock ())
{
if(pA.pc_source == 2)
{
cloud = k2g->getCloud();
fake_cloud_cb_kinectv2_(cloud);
}
if(viz_ && pA.justViewPointCloud)
{
pcl::PointCloud<PointT>::Ptr filtered_prev_cloud(new pcl::PointCloud<PointT>(*prev_cloud));
multi_plane_app.preProcPointCloud(filtered_prev_cloud);
if (!viewer->updatePointCloud<PointT> (filtered_prev_cloud, "cloud"))
{
viewer->addPointCloud<PointT> (filtered_prev_cloud, "cloud");
}
selected_cluster_index = -1;
}
else
{
pcl::PointCloud<PointT>::CloudVectorType clusters;
std::vector<pcl::PointCloud<pcl::Normal> > clusterNormals;
Eigen::Vector4f plane;
std::vector<size_t> clusterIndices;
std::vector<std::vector<int>> clusterIndicesStore;
selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore,
pA.pre_proc,
pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer
hlpr_perception_msgs::SegClusters msg;
//msg.header.stamp = ros::Time::now();
// Pull out the cluster indices and put in msg
for (int ti = 0; ti < clusterIndicesStore.size(); ti++){
hlpr_perception_msgs::ClusterIndex cluster_idx_msg;
for (int j = 0; j < clusterIndicesStore[ti].size(); j++){
std_msgs::Int32 temp_msg;
temp_msg.data = clusterIndicesStore[ti][j];
cluster_idx_msg.indices.push_back(temp_msg);
}
msg.cluster_ids.push_back(cluster_idx_msg);
//clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points
}
for(int i = 0; i < clusters.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusters[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.clusters.push_back(out);
}
for(int i = 0; i < clusterNormals.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusterNormals[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.normals.push_back(out);
}
std_msgs::Float32MultiArray planeMsg;
planeMsg.data.clear();
for(int i = 0; i < 4; i++)
planeMsg.data.push_back(plane[i]);
//planePub.publish(planeMsg);
msg.plane = planeMsg;
msgPub.publish(msg);
//clusterPub.publish(clusters[0]);
//normalPub.publish(clusterNormals[0]);
}
cloud_mutex.unlock();
/*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do
* some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color)
* or sending all the plausible ones to c6 and decide there
*/
}
if(selected_cluster_index < 0)
continue;
}
if(pA.pc_source == 1)
{
interface->stop ();
delete interface;
}
delete nh;
ros::shutdown();
return 0;
}
void
run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber ();
boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&OpenNIOrganizedMultiPlaneSegmentation::cloud_cb_, this, _1);
//make a viewer
pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
viewer = cloudViewer (init_cloud_ptr);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
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.03f);
ne.setNormalSmoothingSize (20.0f);
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (10000);
mps.setAngularThreshold (0.017453 * 2.0); //3 degrees
mps.setDistanceThreshold (0.02); //2cm
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
size_t prev_models_size = 0;
char name[1024];
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
if (prev_cloud && cloud_mutex.try_lock ())
{
regions.clear ();
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
double normal_start = pcl::getTime ();
ne.setInputCloud (prev_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 (prev_cloud);
mps.segmentAndRefine (regions);
double plane_extract_end = pcl::getTime ();
std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl;
std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
if (!viewer->updatePointCloud<PointT> (prev_cloud, "cloud"))
viewer->addPointCloud<PointT> (prev_cloud, "cloud");
removePreviousDataFromScreen (prev_models_size);
//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_%lu", i);
viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name);
contour->points = regions[i].getContour ();
sprintf (name, "plane_%02zu", i);
pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
viewer->addPointCloud (contour, color, name);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
}
prev_models_size = regions.size ();
cloud_mutex.unlock ();
}
}
interface->stop ();
}
