void PeopleDetector::addNewCloudToViewer(PointCloudT::Ptr cloud, pcl::visualization::PCLVisualizer::Ptr viewer_obj) { viewer_obj->removeAllPointClouds(); viewer_obj->removeAllShapes(); pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); viewer_obj->addPointCloud<PointT> (cloud, rgb, "input_cloud"); }
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."); }