//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 RegionGrowingMultiplePlaneSegmentation::segment( const sensor_msgs::PointCloud2::ConstPtr& msg, const sensor_msgs::PointCloud2::ConstPtr& normal_msg) { boost::mutex::scoped_lock lock(mutex_); if (!done_initialization_) { return; } vital_checker_->poke(); { jsk_recognition_utils::ScopedWallDurationReporter r = timer_.reporter(pub_latest_time_, pub_average_time_); pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg(*msg, *cloud); pcl::fromROSMsg(*normal_msg, *normal); // concatenate fields pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::concatenateFields(*cloud, *normal, *all_cloud); pcl::PointIndices::Ptr indices (new pcl::PointIndices); for (size_t i = 0; i < all_cloud->points.size(); i++) { if (!isnan(all_cloud->points[i].x) && !isnan(all_cloud->points[i].y) && !isnan(all_cloud->points[i].z) && !isnan(all_cloud->points[i].normal_x) && !isnan(all_cloud->points[i].normal_y) && !isnan(all_cloud->points[i].normal_z) && !isnan(all_cloud->points[i].curvature)) { if (all_cloud->points[i].curvature < max_curvature_) { indices->indices.push_back(i); } } } pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true); // vector of pcl::PointIndices pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters); cec.setInputCloud (all_cloud); cec.setIndices(indices); cec.setClusterTolerance(cluster_tolerance_); cec.setMinClusterSize(min_size_); //cec.setMaxClusterSize(max_size_); { boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex); setCondifionFunctionParameter(angular_threshold_, distance_threshold_); cec.setConditionFunction( &RegionGrowingMultiplePlaneSegmentation::regionGrowingFunction); //ros::Time before = ros::Time::now(); cec.segment (*clusters); // ros::Time end = ros::Time::now(); // ROS_INFO("segment took %f sec", (before - end).toSec()); } // Publish raw cluster information // pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices> jsk_recognition_msgs::ClusterPointIndices ros_clustering_result; ros_clustering_result.cluster_indices = pcl_conversions::convertToROSPointIndices(*clusters, msg->header); ros_clustering_result.header = msg->header; pub_clustering_result_.publish(ros_clustering_result); // estimate planes std::vector<pcl::PointIndices::Ptr> all_inliers; std::vector<pcl::ModelCoefficients::Ptr> all_coefficients; jsk_recognition_msgs::PolygonArray ros_polygon; ros_polygon.header = msg->header; for (size_t i = 0; i < clusters->size(); i++) { pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]); ransacEstimation(cloud, cluster, *plane_inliers, *plane_coefficients); if (plane_inliers->indices.size() > 0) { jsk_recognition_utils::ConvexPolygon::Ptr convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>( cloud, plane_inliers, plane_coefficients); if (convex) { if (min_area_ > convex->area() || convex->area() > max_area_) { continue; } { // check direction consistency of coefficients and convex Eigen::Vector3f coefficient_normal(plane_coefficients->values[0], plane_coefficients->values[1], plane_coefficients->values[2]); if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) { convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex()); } } // Normal should direct to origin { Eigen::Vector3f p = convex->getPointOnPlane(); Eigen::Vector3f n = convex->getNormal(); if (p.dot(n) > 0) { convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex()); Eigen::Vector3f new_normal = convex->getNormal(); plane_coefficients->values[0] = new_normal[0]; plane_coefficients->values[1] = new_normal[1]; plane_coefficients->values[2] = new_normal[2]; plane_coefficients->values[3] = convex->getD(); } } geometry_msgs::PolygonStamped polygon; polygon.polygon = convex->toROSMsg(); polygon.header = msg->header; ros_polygon.polygons.push_back(polygon); all_inliers.push_back(plane_inliers); all_coefficients.push_back(plane_coefficients); } } } jsk_recognition_msgs::ClusterPointIndices ros_indices; ros_indices.cluster_indices = pcl_conversions::convertToROSPointIndices(all_inliers, msg->header); ros_indices.header = msg->header; pub_inliers_.publish(ros_indices); jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients; ros_coefficients.header = msg->header; ros_coefficients.coefficients = pcl_conversions::convertToROSModelCoefficients( all_coefficients, msg->header); pub_coefficients_.publish(ros_coefficients); pub_polygons_.publish(ros_polygon); } }