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