//int MeshOn2D(const std::vector<ModelT> &model_set, const std::vector<poseT> &poses, cv::Mat &map2d, float fx = FOCAL_X, float fy = FOCAL_Y); float segAcc(const std::vector<ModelT> &model_set, const std::vector<poseT> &poses, pcl::PointCloud<PointT>::Ptr cloud) { pcl::PointCloud<PointT>::Ptr link_cloud(new pcl::PointCloud<PointT>()); pcl::PointCloud<PointT>::Ptr node_cloud(new pcl::PointCloud<PointT>()); for( int i = 0 ; i < model_set.size() ; i++ ) { uint32_t cur_label; pcl::PointCloud<PointT>::Ptr cur_cloud(new pcl::PointCloud<PointT>()); if( model_set[i].model_label == "link" ) { cur_label = 1; cur_cloud = link_cloud; } else if( model_set[i].model_label == "node" ) { cur_label = 2; cur_cloud = node_cloud; } pcl::copyPointCloud(*model_set[i].model_cloud, *cur_cloud); for( pcl::PointCloud<PointT>::iterator it = cur_cloud->begin() ; it < cur_cloud->end() ; it++ ) it->rgba = cur_label; } Eigen::Quaternionf calibrate_rot(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f (1, 0, 0))); pcl::PointCloud<PointT>::Ptr all_cloud(new pcl::PointCloud<PointT>()); for(std::vector<poseT>::const_iterator it = poses.begin() ; it < poses.end() ; it++) { for( int i = 0 ; i < model_set.size() ; i++ ) { if(model_set[i].model_label == it->model_name ) { pcl::PointCloud<PointT>::Ptr cur_cloud(new pcl::PointCloud<PointT>()); if( it->model_name == "link" ) pcl::copyPointCloud(*link_cloud, *cur_cloud); else if( it->model_name == "node" ) pcl::copyPointCloud(*node_cloud, *cur_cloud); pcl::transformPointCloud(*cur_cloud, *cur_cloud, it->shift, it->rotation*calibrate_rot); all_cloud->insert(all_cloud->end(), cur_cloud->begin(), cur_cloud->end()); } } } pcl::search::KdTree<PointT> tree; tree.setInputCloud (all_cloud); uint8_t tmp_color = 255; uint32_t red = tmp_color << 16; uint32_t blue = tmp_color; int pos_count = 0; float T = 0.02*0.02; for ( pcl::PointCloud<PointT>::iterator it = cloud->begin() ; it < cloud->end() ; it++ ) { std::vector<int> indices (1); std::vector<float> sqr_distances (1); int nres = tree.nearestKSearch(*it, 1, indices, sqr_distances); if( it->rgba > 255 ) it->rgba = 1; else if( it->rgba > 0 ) it->rgba = 2; else it->rgba = 0; if ( nres == 1 && sqr_distances[0] < T ) { if(it->rgba == all_cloud->at(indices[0]).rgba) pos_count++; } else if( it->rgba == 0 || sqr_distances[0] > T ) pos_count++; if( nres == 1 && sqr_distances[0] < T ) { if( all_cloud->at(indices[0]).rgba == 1 ) it->rgba = red; else if( all_cloud->at(indices[0]).rgba == 2 ) it->rgba = blue; } else it->rgba = 0; } return (pos_count +0.0) / cloud->size(); }
void callback(const sensor_msgs::PointCloud2 &pc) { double t1, t2, avg_t; ROS_INFO("Received point cloud! Applying method %d",method_id); pcl::PointCloud<PointT>::Ptr scene_pc(new pcl::PointCloud<PointT>()); pcl::PCLPointCloud2 pcl_pc; pcl_conversions::toPCL(pc, pcl_pc); pcl::fromPCLPointCloud2(pcl_pc, *scene_pc); if( scene_pc->empty() == true ) { ROS_ERROR("Recieved empty point cloud message!"); return; } pcl::PointCloud<myPointXYZ>::Ptr scene_xyz(new pcl::PointCloud<myPointXYZ>()); pcl::copyPointCloud(*scene_pc, *scene_xyz); if( view_flag == true ) { viewer->removeAllPointClouds(); viewer->addPointCloud(scene_xyz, "whole_scene"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.5, 0.5, 0.5, "whole_scene"); } std::vector<poseT> all_poses; std::vector<poseT> link_poses, node_poses; std::cout << " ... processing:" << std::endl; switch(method_id) { case 0: t1 = get_wall_time(); objrec->StandardRecognize(scene_xyz, all_poses); t2 = get_wall_time(); break; case 1: { t1 = get_wall_time(); int pose_num = 0; int iter = 0; pcl::PointCloud<myPointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<myPointXYZ>()); filtered_cloud = scene_xyz; while(true) { //std::cerr<< "Recognizing Attempt --- " << iter << std::endl; objrec->StandardRecognize(filtered_cloud, all_poses); if( all_poses.size() <= pose_num ) break; else pose_num = all_poses.size(); pcl::PointCloud<myPointXYZ>::Ptr model_cloud = objrec->FillModelCloud(all_poses); filtered_cloud = FilterCloud(filtered_cloud, model_cloud); iter++; } t2 = get_wall_time(); //std::cerr<< "Recognizing Done!!!" << std::endl; break; } case 2: { //std::vector<poseT> tmp_poses; t1 = get_wall_time(); objrec->GreedyRecognize(scene_xyz, all_poses); t2 = get_wall_time(); //all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses); break; } case 3: { pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>()); pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>()); splitCloud(scene_pc, link_cloud, node_cloud); t1 = get_wall_time(); std::cout << " ... starting at " << t1 << std::endl; //std::vector<poseT> link_poses, node_poses; linkrec->StandardRecognize(link_cloud, link_poses); noderec->StandardRecognize(node_cloud, node_poses); t2 = get_wall_time(); std::cout << " ... done at " << t2 << std::endl; all_poses.insert(all_poses.end(), link_poses.begin(), link_poses.end()); all_poses.insert(all_poses.end(), node_poses.begin(), node_poses.end()); break; } case 4: { pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>()); pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>()); splitCloud(scene_pc, link_cloud, node_cloud); t1 = get_wall_time(); std::cout << " ... starting at " << t1 << std::endl; int pose_num = 0; std::vector<poseT> tmp_poses; int iter = 0; while(true) { //std::cerr<< "Recognizing Attempt --- " << iter << std::endl; list<AcceptedHypothesis> acc_hypotheses; std::cout << " ... generating" << std::endl; linkrec->genHypotheses(link_cloud, acc_hypotheses); noderec->genHypotheses(node_cloud, acc_hypotheses); std::cout << " ... merging" << std::endl; linkrec->mergeHypotheses(scene_xyz, acc_hypotheses, tmp_poses); if( tmp_poses.size() <= pose_num ) { break; } else { pose_num = tmp_poses.size(); std::cout << "Number of merged hypotheses: " << tmp_poses.size() << std::endl; } pcl::PointCloud<myPointXYZ>::Ptr link_model = linkrec->FillModelCloud(tmp_poses); pcl::PointCloud<myPointXYZ>::Ptr node_model = noderec->FillModelCloud(tmp_poses); std::cout << " ... filtering" << std::endl; if( link_model->empty() == false ) { link_cloud = FilterCloud(link_cloud, link_model); } if( node_model->empty() == false) { node_cloud = FilterCloud(node_cloud, node_model); } iter++; } t2 = get_wall_time(); std::cout << " ... done at " << t2 << std::endl; //std::cerr<< "Recognizing Done!!!" << std::endl; all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses); break; } case 5: { pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>()); pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>()); splitCloud(scene_pc, link_cloud, node_cloud); t1 = get_wall_time(); linkrec->GreedyRecognize(link_cloud, link_poses); noderec->GreedyRecognize(node_cloud, node_poses); t2 = get_wall_time(); std::vector<poseT> tmp_poses; tmp_poses.insert(tmp_poses.end(), link_poses.begin(), link_poses.end()); tmp_poses.insert(tmp_poses.end(), node_poses.begin(), node_poses.end()); //all_poses = tmp_poses; all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses); break; } default: ROS_ERROR("Code %d not recognized!",method_id); return; } if( view_flag == true ) { switch(method_id) { case 0: case 1: case 2: objrec->visualize(viewer, all_poses); break; case 3: case 4: case 5: linkrec->visualize(viewer, all_poses); noderec->visualize(viewer, all_poses); break; } viewer->spin(); } geometry_msgs::PoseArray links; geometry_msgs::PoseArray nodes; links.header.frame_id = pc.header.frame_id; nodes.header.frame_id = pc.header.frame_id; // publish poses as TF if (method_id < 3) { for (poseT &p: all_poses) { geometry_msgs::Pose pose; pose.position.x = p.shift[0]; pose.position.y = p.shift[1]; pose.position.z = p.shift[2]; pose.orientation.x = p.rotation.x(); pose.orientation.y = p.rotation.y(); pose.orientation.z = p.rotation.z(); pose.orientation.w = p.rotation.w(); links.poses.push_back(pose); } pub_link.publish(links); } else { for (poseT &p: all_poses) { geometry_msgs::Pose pose; pose.position.x = p.shift[0]; pose.position.y = p.shift[1]; pose.position.z = p.shift[2]; pose.orientation.x = p.rotation.x(); pose.orientation.y = p.rotation.y(); pose.orientation.z = p.rotation.z(); pose.orientation.w = p.rotation.w(); links.poses.push_back(pose); } for (poseT &p: all_poses) { geometry_msgs::Pose pose; pose.position.x = p.shift[0]; pose.position.y = p.shift[1]; pose.position.z = p.shift[2]; pose.orientation.x = p.rotation.x(); pose.orientation.y = p.rotation.y(); pose.orientation.z = p.rotation.z(); pose.orientation.w = p.rotation.w(); nodes.poses.push_back(pose); } pub_link.publish(links); pub_link.publish(nodes); } std::cout << "Time 1 = " << t1 << std::endl; std::cout << "Time 2 = " << t2 << std::endl; ROS_INFO("... done."); }