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