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;
}
