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