void AdaptiveManifoldFilterN::buildManifoldsAndPerformFiltering(vector<Mat>& eta, Mat1b& cluster, int treeLevel) { CV_DbgAssert((int)eta.size() == jointCnNum); //splatting Size etaSize = eta[0].size(); CV_DbgAssert(etaSize == srcSize || etaSize == smallSize); if (etaSize == srcSize) { compute_w_k(eta, w_k, sigma_r_over_sqrt_2, treeLevel); etaFull = eta; downsample(eta, eta); } else { upsample(eta, etaFull); compute_w_k(etaFull, w_k, sigma_r_over_sqrt_2, treeLevel); } //blurring Psi_splat_small.resize(srcCnNum); for (int si = 0; si < srcCnNum; si++) { Mat tmp; multiply(srcCn[si], w_k, tmp); downsample(tmp, Psi_splat_small[si]); } downsample(w_k, Psi_splat_0_small); vector<Mat>& Psi_splat_small_blur = Psi_splat_small; Mat& Psi_splat_0_small_blur = Psi_splat_0_small; float rf_ss = (float)(sigma_s_ / getResizeRatio()); float rf_sr = (float)(sigma_r_over_sqrt_2); RFFilterPass(eta, Psi_splat_small, Psi_splat_0_small, Psi_splat_small_blur, Psi_splat_0_small_blur, rf_ss, rf_sr); //slicing { Mat tmp; for (int i = 0; i < srcCnNum; i++) { upsample(Psi_splat_small_blur[i], tmp); multiply(tmp, w_k, tmp); add(sum_w_ki_Psi_blur_[i], tmp, sum_w_ki_Psi_blur_[i]); } upsample(Psi_splat_0_small_blur, tmp); multiply(tmp, w_k, tmp); add(sum_w_ki_Psi_blur_0_, tmp, sum_w_ki_Psi_blur_0_); } //build new manifolds if (treeLevel < curTreeHeight) { Mat1b cluster_minus, cluster_plus; computeClusters(cluster, cluster_minus, cluster_plus); vector<Mat> eta_minus(jointCnNum), eta_plus(jointCnNum); { Mat1f teta = 1.0 - w_k; computeEta(teta, cluster_minus, eta_minus); computeEta(teta, cluster_plus, eta_plus); } //free memory to continue deep recursion eta.clear(); cluster.release(); buildManifoldsAndPerformFiltering(eta_minus, cluster_minus, treeLevel + 1); buildManifoldsAndPerformFiltering(eta_plus, cluster_plus, treeLevel + 1); } }
bool seg_cb(bwi_perception::ButtonDetection::Request &req, bwi_perception::ButtonDetection::Response &res) { //get the point cloud by aggregating k successive input clouds waitForCloudK(15); cloud = cloud_aggregated; //**Step 1: z-filter and voxel filter**// // Create the filtering object pcl::PassThrough<PointT> pass; pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (0.0, 1.15); pass.filter (*cloud); // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<PointT> vg; pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>); vg.setInputCloud (cloud); vg.setLeafSize (0.0025f, 0.0025f, 0.0025f); vg.filter (*cloud_filtered); //publish point cloud for debugging ROS_INFO("Publishing point cloud..."); /*pcl::toROSMsg(*cloud_filtered,cloud_ros); cloud_ros.header.frame_id = cloud->header.frame_id; cloud_pub.publish(cloud_ros);*/ ROS_INFO("After voxel grid filter: %i points",(int)cloud_filtered->points.size()); //**Step 2: plane fitting**// //find palne //one cloud contains plane other cloud contains other objects pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); // Create the segmentation object pcl::SACSegmentation<PointT> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.02); // Create the filtering object pcl::ExtractIndices<PointT> extract; // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); // Extract the plane extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_plane); //for everything else, cluster extraction; segment extraction //extract everything else extract.setNegative (true); extract.filter (*cloud_blobs); //get the plane coefficients Eigen::Vector4f plane_coefficients; plane_coefficients(0)=coefficients->values[0]; plane_coefficients(1)=coefficients->values[1]; plane_coefficients(2)=coefficients->values[2]; plane_coefficients(3)=coefficients->values[3]; //**Step 3: Eucledian Cluster Extraction**// std::vector<PointCloudT::Ptr > clusters = computeClusters(cloud_blobs,0.04); ROS_INFO("clustes found: %i", (int)clusters.size()); clusters_on_plane.clear(); //if clusters are touching the table put them in a vector for (unsigned int i = 0; i < clusters.size(); i++){ Eigen::Vector4f centroid_i; pcl::compute3DCentroid(*clusters.at(i), centroid_i); pcl::PointXYZ center; center.x=centroid_i(0);center.y=centroid_i(1);center.z=centroid_i(2); double distance = pcl::pointToPlaneDistance(center, plane_coefficients); if (distance < 0.1 /*&& clusters.at(i).get()->points.size() >*/ ){ clusters_on_plane.push_back(clusters.at(i)); } } ROS_INFO("clustes_on_plane found: %i", (int)clusters_on_plane.size()); // if the clousters size == 0 return false if(clusters_on_plane.size() == 0) { cloud_mutex.unlock (); res.button_found = false; return true; } //**Step 4: detect the button among the remaining clusters**// int max_index = -1; double max_red = 0.0; // Find the max red value for (unsigned int i = 0; i < clusters_on_plane.size(); i++){ double red_i = computeAvgRedValue(clusters_on_plane.at(i)); //ROS_INFO("Cluster %i: %i points, red_value = %f",i,(int)clusters_on_plane.at(i)->points.size(),red_i); if (red_i > max_red){ max_red = red_i; max_index = i; } } //publish cloud if we think it's a button /*max_red > 170 && max_red < 250 && */ ROS_INFO("max_red=%f", max_red); if (max_index >= 0 && max_red > red_min) { ROS_INFO("Button_found"); pcl::toROSMsg(*clusters_on_plane.at(max_index),cloud_ros); cloud_ros.header.frame_id = cloud->header.frame_id; cloud_pub.publish(cloud_ros); //fill in response res.button_found = true; res.cloud_button = cloud_ros; /*Eigen::Vector4f centroid; pcl::compute3DCentroid(*clusters_on_plane.at(max_index), centroid); //transforms the pose into /map frame geometry_msgs::Pose pose_i; pose_i.position.x=centroid(0); pose_i.position.y=centroid(1); pose_i.position.z=centroid(2); pose_i.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,-3.14/2); geometry_msgs::PoseStamped stampedPose; stampedPose.header.frame_id = cloud->header.frame_id; stampedPose.header.stamp = ros::Time(0); stampedPose.pose = pose_i;*/ //geometry_msgs::PoseStamped stampOut; //listener.waitForTransform(cloud->header.frame_id, "m1n6s200_link_base", ros::Time(0), ros::Duration(3.0)); //listener.transformPose("m1n6s200_link_base", stampedPose, stampOut); //stampOut.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,-3.14/2,0); //pose_pub.publish(stampOut); } else { res.button_found = false; } cloud_mutex.unlock (); return true; }