bool process_ros(duplo_v0::Process_PCD::Request &req, duplo_v0::Process_PCD::Response &res) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudz(new pcl::PointCloud<pcl::PointXYZRGB>); //Filtered cloud pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_inliers(new pcl::PointCloud<pcl::PointXYZRGB>); //Inliers after removing outliers pcl::PointCloud<pcl::PointXYZRGB>::Ptr planar_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster1(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::fromROSMsg(req.pcd_in,*cloud); /****************** Filter out the non-table points ******************/ if (pass_through(cloud,cloudz) != 0) { std::cerr << "Pass-through filter error" << std::endl; return false; } /* ********* Segmentation of the cloud into table and object clouds **********/ planar_seg(cloudz,planar_cloud,object_cloud,"table_try.pcd","object_try.pcd"); /********************** Cluster objects based on Euclidean distance **********/ //vector<double> volumes = cluster(object_cloud); int num_clusters_found = cluster(object_cloud); if (num_clusters_found == 0) the_chosen_one.data.clear(); res.n_clusters = num_clusters_found; processed_pointcloud = true; return true; }
IMGFOR(src_seg,x,y) { if (in_mask(x,y)) { in_quality++; NetInt32 addr = cluster1(x,y); ColorMap::iterator it = color_map.find(addr); if (it == color_map.end()) { color_map[addr] = 0; color_map2[addr] = 0; it = color_map.find(addr); } (*it).second++; if (!mask_small(x,y)) { ref_ct++; color_map2[addr]++; } } }
bool ZMeshFilterManifold::apply(const Eigen::MatrixXf& input, const std::vector<bool>& tags) { init(input); int inputSize = input.rows();//pMesh_->get_num_of_vertex_list(); Eigen::MatrixXf initAttributes = input; sum_w_ki_Psi_blur_.resize(inputSize, rangeDim_); sum_w_ki_Psi_blur_0_.resize(inputSize); min_pixel_dist_to_manifold_squared_.resize(inputSize); // un-initilized min_pixel_dist_to_manifold_squared_.fill(FLT_MAX); sum_w_ki_Psi_blur_.fill(0); sum_w_ki_Psi_blur_0_.fill(0); verticeClusterIds.resize(inputSize); verticeClusterIds.fill(1); // 1. low-pass filtering //ZMeshFilterGaussian filterGaussian(pMesh_); float spatialSigma = filterPara_.spatial_sigma * 1.f; float rangeSigma = cos(filterPara_.range_sigma*Z_PI/180.f); pGaussianFilter_->setPara(ZMeshFilterParaNames::SpatialSigma, spatialSigma); pGaussianFilter_->setPara(ZMeshFilterParaNames::RangeSigma, rangeSigma); pGaussianFilter_->setKernelFunc(NULL); // only using spatial filter //pGaussianFilter_->apply(initPositions, std::vector<bool>(inputSize, true)); CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(initAttributes, spatialDim_, rangeDim_)); Eigen::MatrixXf eta1 = pGaussianFilter_->getResult(); //std::cout << " input=\n" << input.block(0,0,10,rangeDim_+spatialDim_) << "\n"; //std::cout << " eta1=\n" << eta1.block(0,0,10,rangeDim_+spatialDim_) << "\n"; //ZFileHelper::saveEigenMatrixToFile("eta1.txt", eta1); std::vector<bool> cluster1(inputSize, true); int currentTreeLevel = 1; CHECK_FALSE_AND_RETURN(buildManifoldAndPerformFiltering(input, eta1, cluster1, spatialSigma, rangeSigma, currentTreeLevel)); //std::cout << "sum_w_ki_Psi_blur=\n" << sum_w_ki_Psi_blur_.block(0,0,10,rangeDim_) << "\n"; //std::cout << "Sum_w_ki_Psi_blur norm():" << sum_w_ki_Psi_blur_.norm() << "\n"; //std::cout << "sum_w_ki_Psi_blur_0=\n" << sum_w_ki_Psi_blur_0_.head(10) << "\n"; //std::vector<Eigen::Vector3f> tilde_g(vSize); Eigen::MatrixXf tilde_g(inputSize, rangeDim_); for (int i=0; i<inputSize; i++) { if (g_isZero(sum_w_ki_Psi_blur_0_(i))) std::cout << "Zero! " << i << "\n"; for (int j=0; j<rangeDim_; j++) { tilde_g(i, j) = sum_w_ki_Psi_blur_(i, j)/sum_w_ki_Psi_blur_0_(i); } } //ZFileHelper::saveEigenMatrixToFile("SumWeight.txt", sum_w_ki_Psi_blur_0_); Eigen::MatrixXf sumNewRange(inputSize, rangeDim_); sumNewRange.fill(0); rangeWeights_.resize(wki_Psi_blurs_.size(), 1.f); for (int i=0; i<wki_Psi_blurs_.size(); i++) { //float weight = 1.f*(i*10+1); float weight = 1.f/(i+1); //float weight = 1.f - log(1.f*(i+1)); //float weight = exp(1.f*(i+1)); //float weight = i==0 ? 1.f : 0.f; sumNewRange += wki_Psi_blurs_[i]*weight; } // for (int i=0; i<inputSize; i++) // { // //sumNewRange.row(i) /= sum_w_ki_Psi_blur_0_(i); // sumNewRange.row(i).normalize(); // } //for (int i=0; i<wki_Psi_blurs_.size(); i++) //{ // std::stringstream ss; // ss << "wkiPsiBlurs" << i << ".txt"; // //ZFileHelper::saveEigenMatrixToFile(ss.str().c_str(), wki_Psi_blurs_[i]); // std::stringstream ss2; // ss2 << "wkiPsiBlurs0" << i << ".txt"; // ZFileHelper::saveEigenVectorToFile(ss2.str().c_str(), wki_Psi_blur_0s_[i]); //} //ZFileHelper::saveEigenMatrixToFile("sumNewRange.txt", sumNewRange); output_ = input; // output_.block(0, spatialDim_, inputSize, rangeDim_) = tilde_g; // std::cout << output_; // ZFileHelper::saveEigenMatrixToFile("input.txt", input); // ZFileHelper::saveEigenMatrixToFile("minPixelDist2Mani.txt", min_pixel_dist_to_manifold_squared_); // ZFileHelper::saveEigenMatrixToFile("tildeG.txt", tilde_g); // Eigen::VectorXf alpha(inputSize); // alpha.fill(0); // for (int i=0; i<inputSize; i++) // { // alpha[i] = exp(min_pixel_dist_to_manifold_squared_(i)*(-0.5)/filterPara_.range_sigma/filterPara_.range_sigma); // } // for (int i=0; i<inputSize; i++) // { // Eigen::VectorXf newRange = input.row(i).tail(rangeDim_) + (tilde_g.row(i)-input.row(i).tail(rangeDim_))*alpha(i); // newRange.normalize(); // output_.row(i).tail(rangeDim_) = newRange; // } output_.block(0, spatialDim_, inputSize, rangeDim_) = sumNewRange; // ZFileHelper::saveEigenMatrixToFile("sumWkiPsiBlur.txt", sum_w_ki_Psi_blur_); // ZFileHelper::saveEigenMatrixToFile("tilderG.txt", tilde_g); // ZFileHelper::saveEigenMatrixToFile("output.txt", output_); std::cout << "Filter done!\n"; //std::cout << "output=\n" << output_.block(0,0,10,rangeDim_+spatialDim_) << "\n"; //std::cout << " Output-input=\n" << (output_-input).block(0,0,10,rangeDim_+spatialDim_) << "\n"; std::cout << ZConsoleTools::red << " Error: " << (output_-input).norm() << "\n" << ZConsoleTools::white; return true; }