bool ZMeshFilterManifold::buildManifoldAndPerformFiltering(const Eigen::MatrixXf& input,
const Eigen::MatrixXf& etaK, const std::vector<bool>& clusterK,
float sigma_s, float sigma_r, int currentTreeLevel)
{
int inputSize = input.rows();;
static std::string fileName("etaK.txt");
fileName += "0";
ZFileHelper::saveEigenMatrixToFile(fileName.c_str(), etaK);
// splatting: project the vertices onto the current manifold etaK
// NOTE: the sigma should be recursive!!
float sigmaR_over_sqrt_2 = sigma_r/sqrt(2.0);
Eigen::MatrixXf diffX = input.block(0, spatialDim_, inputSize, rangeDim_) - etaK.block(0, spatialDim_, inputSize, rangeDim_);
Eigen::VectorXf gaussianDistWeight(inputSize);
//std::cout << "diffX=\n" << diffX.block(0,0,10,rangeDim_+spatialDim_) << "\n";
for (int i=0; i<inputSize; i++)
{
gaussianDistWeight(i) = ZKernelFuncs::GaussianKernelFunc(diffX.row(i), sigmaR_over_sqrt_2);
}
Eigen::MatrixXf psiSplat(inputSize, spatialDim_+rangeDim_+1);
Eigen::VectorXf psiSplat0 = gaussianDistWeight;
//////////////////////////////////////////////////////////////////////////
// for debug
//std::stringstream ss;
static int etaI = 1;
//ss << "gaussianWeights" << etaI << ".txt";
//std::cout << ZConsoleTools::green << etaI << "\n" << ZConsoleTools::white;
etaI++;
//ZFileHelper::saveEigenVectorToFile(ss.str().c_str(), gaussianDistWeight);
//////////////////////////////////////////////////////////////////////////
psiSplat.block(0,0,inputSize,spatialDim_) = input.block(0,0,inputSize,spatialDim_);
for (int i=0; i<inputSize; i++)
{
//psiSplat.block(i, spatialDim_, 1, rangeDim_) = input.block(i, spatialDim_, 1, rangeDim_)*gaussianDistWeight(i);
psiSplat.block(i, spatialDim_, 1, rangeDim_) = etaK.block(i, spatialDim_, 1, rangeDim_)*gaussianDistWeight(i);
}
psiSplat.col(spatialDim_+rangeDim_) = gaussianDistWeight;
// save min distance to later perform adjustment of outliers -- Eq.(10)
// UNDO
// update min_pixel_dist_to_manifold_square_
Eigen::VectorXf pixelDist2Manifold(inputSize);
for (int i=0; i<inputSize; i++)
{
if (clusterK[i])
pixelDist2Manifold(i) = diffX.row(i).squaredNorm();
else
pixelDist2Manifold(i) = FLT_MAX;
}
min_pixel_dist_to_manifold_squared_ = min_pixel_dist_to_manifold_squared_.cwiseMin(pixelDist2Manifold);
// Blurring
Eigen::MatrixXf wkiPsiBlur(inputSize, rangeDim_);
Eigen::VectorXf wkiPsiBlur0(inputSize);
//Eigen::MatrixXf psiSplatAnd0(inputSize, spatialDim_+rangeDim_+1);
//psiSplatAnd0.block(0, 0, inputSize, spatialDim_+rangeDim_) = psiSplat.block(0, 0, inputSize, spatialDim_+rangeDim_);
//psiSplatAnd0.col(spatialDim_+rangeDim_) = psiSplat0;
//psiSplatAnd0.block(0, 0, inputSize, spatialDim_+rangeDim_) = input;
//psiSplatAnd0.col(spatialDim_+rangeDim_).fill(1.f);
//pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelFuncNormal3);
pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelFuncA);
//pGaussianFilter_->setKernelFunc(NULL);
//pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelSphericalFunc);
pGaussianFilter_->setPara(ZMeshFilterParaNames::SpatialSigma, sigma_s);
pGaussianFilter_->setPara(ZMeshFilterParaNames::RangeSigma, sigmaR_over_sqrt_2);
//pGaussianFilter_->apply(psiSplatAnd0, clusterK);
//CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(psiSplatAnd0, spatialDim_, rangeDim_+1, Eigen::VectorXf(), clusterK));
CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(psiSplat, spatialDim_, rangeDim_+1, gaussianDistWeight, clusterK));
Eigen::MatrixXf wkiPsiBlurAnd0 = pGaussianFilter_->getResult();
wkiPsiBlur = wkiPsiBlurAnd0.block(0, spatialDim_, inputSize, rangeDim_);
wkiPsiBlur0 = wkiPsiBlurAnd0.col(spatialDim_+rangeDim_);
//ZFileHelper::saveEigenMatrixToFile("")
// std::cout << "wkiPsiBlurAnd0=\n" << wkiPsiBlurAnd0.block(0,0,10,rangeDim_+spatialDim_+1) << "\n";
// std::cout << "wkiPsiBlur0=\n" << wkiPsiBlur0.head(10) << "\n";
//std::cout << "pixelDist2Manifold=\n" << pixelDist2Manifold.head(10) << "\n";
//std::cout << "min_pixel_dist_to_manifold_squared=\n" << min_pixel_dist_to_manifold_squared_.head(10) << "\n";
// for debug
// for (int i=0; i<inputSize; i++)
// {
// if (!g_isInfinite(wkiPsiBlur(i,0)))
// std::cout << "(" << i << "," << 0 << ")\n";
// if (!g_isInfinite(wkiPsiBlur(i,1)))
// std::cout << "(" << i << "," << 1 << ")\n";
// //if (!g_isInfinite(wkiPsiBlur(i,2)))
// // std::cout << "(" << i << "," << 2 << ")\n";
// }
//std::cout << wkiPsiBlur.norm() << "\n";
Eigen::VectorXf rangeDiff(inputSize);
for (int i=0; i<inputSize; i++)
{
Eigen::VectorXf n0 = wkiPsiBlur.row(i);
Eigen::VectorXf n1 = input.row(i).tail(rangeDim_);
n0.normalize();
n1.normalize();
rangeDiff(i) = 1.f-n0.dot(n1);
}
static bool bSaved = false;
if (!bSaved)
{
ZFileHelper::saveEigenVectorToFile("rangeDiff.txt", rangeDiff);
ZFileHelper::saveEigenVectorToFile("gaussian.txt", gaussianDistWeight);
ZFileHelper::saveEigenMatrixToFile("splat.txt", psiSplat);
ZFileHelper::saveEigenMatrixToFile("blur.txt", wkiPsiBlur);
bSaved = true;
}
// Slicing
Eigen::VectorXf wki = gaussianDistWeight;
for (int i=0; i<inputSize; i++)
{
if (!clusterK[i]) continue;
sum_w_ki_Psi_blur_.row(i) += wkiPsiBlur.row(i)*wki(i);
sum_w_ki_Psi_blur_0_(i) += wkiPsiBlur0(i)*wki(i);
}
//////////////////////////////////////////////////////////////////////////
// for debug
wki_Psi_blurs_.push_back(Eigen::MatrixXf(inputSize, rangeDim_));
wki_Psi_blur_0s_.push_back(Eigen::VectorXf(inputSize));
Eigen::MatrixXf& lastM = wki_Psi_blurs_[wki_Psi_blurs_.size()-1];
lastM.fill(0);
Eigen::VectorXf& lastV = wki_Psi_blur_0s_[wki_Psi_blur_0s_.size()-1];
lastV.fill(0);
for (int i=0; i<inputSize; i++)
{
if (!clusterK[i]) continue;
lastM.row(i) = wkiPsiBlur.row(i)*wki(i);
lastV(i) = wkiPsiBlur0(i)*wki(i);
}
std::cout << sum_w_ki_Psi_blur_.norm() << "\n";
//////////////////////////////////////////////////////////////////////////
// compute two new manifolds eta_minus and eta_plus
// test stopping criterion
if (currentTreeLevel<filterPara_.tree_height)
{
// algorithm 1, Step 2: compute the eigenvector v1
Eigen::VectorXf v1 = computeMaxEigenVector(diffX, clusterK);
// algorithm 1, Step 3: Segment vertices into two clusters
std::vector<bool> clusterMinus(inputSize, false);
std::vector<bool> clusterPlus(inputSize, false);
int countMinus=0;
int countPlus =0;
for (int i=0; i<inputSize; i++)
{
float dot = diffX.row(i).dot(v1);
if (dot<0 && clusterK[i]) {countMinus++; verticeClusterIds[i] =etaI+0.5; clusterMinus[i] = true;}
if (dot>=0 && clusterK[i]) {countPlus++; verticeClusterIds[i] =etaI-0.5; clusterPlus[i] = true;}
}
std::cout << "Minus manifold: " << countMinus << "\n";
std::cout << "Plus manifold: " << countPlus << "\n";
// Eigen::MatrixXf diffXManifold(inputSize, spatialDim_+rangeDim_);
// diffXManifold.block(0, 0, inputSize, spatialDim_) = input.block(0, 0, inputSize, spatialDim_);
// diffXManifold.block(0, spatialDim_, inputSize, rangeDim_) = diffX;
// algorithm 1, Step 4: Compute new manifolds by weighted low-pass filtering -- Eq. (7)(8)
Eigen::VectorXf theta(inputSize);
theta.fill(1);
theta = theta - wki.cwiseProduct(wki);
pGaussianFilter_->setKernelFunc(NULL);
CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(input, spatialDim_, rangeDim_, theta, clusterMinus));
Eigen::MatrixXf etaMinus = pGaussianFilter_->getResult();
CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(input, spatialDim_, rangeDim_, theta, clusterPlus));
Eigen::MatrixXf etaPlus = pGaussianFilter_->getResult();
// algorithm 1, Step 5: recursively build more manifolds
CHECK_FALSE_AND_RETURN(buildManifoldAndPerformFiltering(input, etaMinus, clusterMinus, sigma_s, sigma_r, currentTreeLevel+1));
CHECK_FALSE_AND_RETURN(buildManifoldAndPerformFiltering(input, etaPlus, clusterPlus, sigma_s, sigma_r, currentTreeLevel+1));
}
return true;
}
