Пример #1
0
	bool ZMeshBilateralFilter::apply(const Eigen::MatrixXf& input, const Eigen::VectorXf& weights, const std::vector<bool>& tags)
	{
		if (pAnnSearch_==NULL)
			return false;
		if (getRangeDim()+getSpatialDim()!=input.cols())
			return false;

		int nSize = input.rows();
		output_ = input;
		pAnnSearch_->setFlags(tags);

		float searchRad = filterPara_.spatial_sigma*sqrt(3.0);
		for (int i=0; i<nSize; i++)
		{
			if (!tags[i]) continue;

			Eigen::VectorXf v(input.row(i));
			Eigen::VectorXi nnIdx;
			Eigen::VectorXf nnDists;

			// query
			Eigen::VectorXf queryV(v.head(spatialDim_));
			Eigen::VectorXf rangeV(v.tail(rangeDim_));
			int queryNum = pAnnSearch_->queryFRPoint(queryV, searchRad, 0, nnIdx, nnDists);
			//int queryNum = queryMeshTool_->query(i, 20, nnIdx, nnDists);

			// convolute
			Eigen::VectorXf sumRange(rangeDim_);
			sumRange.fill(0);
			float sumWeight = 0;
			for (int j=1; j<queryNum; j++)
			{
				int idx = nnIdx[j];
				if (!tags[idx]) continue;
				Eigen::VectorXf rangeU(input.row(idx).tail(rangeDim_));
				float distWeight = ZKernelFuncs::GaussianKernelFunc(sqrt(nnDists(j)), filterPara_.spatial_sigma);
				// if kernelFuncA_==NULL, then only using spatial filter
				float rangeWeidht = kernelFuncA_ ? kernelFuncA_(rangeV, rangeU, filterPara_.range_sigma) : 1.f;
				float weight = rangeWeidht*distWeight*weights(idx);
				//if (i==1)
				//	std::cout << rangeU << " * " << distWeight << "*" << rangeWeidht << "*" << weights(idx) << "\n";
				sumWeight += weight;
				sumRange += rangeU*weight;
			}
			if (!g_isZero(sumWeight))
				output_.row(i).tail(rangeDim_) = sumRange*(1.f/sumWeight);
		}

		return true;
	}
Пример #2
0
void normalize_each_row(Eigen::MatrixXf &matrix)
{
    //const float row_max_value = 255.0f;
    const float row_max_value = 1.0f;

    for(int row=0; row < matrix.rows(); row += 1)
    {
        const float t_min = matrix.row(row).minCoeff();
        const float t_max = matrix.row(row).maxCoeff();
        matrix.row(row).array() -= t_min;
        matrix.row(row) *= row_max_value/ (t_max - t_min);
    } // end of "for each row"

    return;
}
Пример #3
0
	void gaussNewtonFromSamplesWeighed(const Eigen::VectorXf &xb, const Eigen::VectorXf &rb, const Eigen::MatrixXf &X, const Eigen::VectorXf &weights, const Eigen::VectorXf &residuals, float regularization, Eigen::VectorXf &out_result)
	{
		//Summary:
		//out_result=xb - G rb
		//xb is the best sample, rb is the best sample residual vector
		//G=AB'inv(BB'+kI)
		//A.col(i)=weights[i]*(X.row(i)-best sample)'
		//B.col(i)=weights[i]*(residuals - rb)'
		//k=regularization

		//Get xb, r(xb)
		//cv::Mat xb=X.row(bestIndex);
		//cv::Mat rb=residuals.row(bestIndex);

		//Compute A and B
		MatrixXf A=X.transpose();
		MatrixXf B=residuals.transpose();
		for (int i=0; i<A.cols(); i++)
		{
			A.col(i)=weights[i]*(X.row(i).transpose()-xb);
			B.col(i)=weights[i]*(residuals.row(i).transpose()-rb);
		}
		MatrixXf I=MatrixXf::Identity(B.rows(),B.rows());
		I=I*regularization;
		MatrixXf G=(A*B.transpose())*(B*B.transpose()+I).inverse();
		out_result=xb - G * rb;
	}
void detectSiftMatchWithVLFeat(const char* img1_path, const char* img2_path, Eigen::MatrixXf &match) {

    int *m = 0;
    double *kp1 = 0, *kp2 = 0;
    vl_uint8 *desc1 = 0, *desc2 = 0;

    int nkp1 = detectSiftAndCalculateDescriptor(img1_path, kp1, desc1);
    int nkp2 = detectSiftAndCalculateDescriptor(img2_path, kp2, desc2);
    cout << "num kp1: " << nkp1 << endl;
    cout << "num kp2: " << nkp2 << endl;
    int nmatch = matchDescriptorWithRatioTest(desc1, desc2, nkp1, nkp2, m);
    cout << "num match: " << nmatch << endl;
    match.resize(nmatch, 6);
    for (int i = 0; i < nmatch; i++) {
        int index1 = m[i*2+0];
        int index2 = m[i*2+1];
        match.row(i) << kp1[index1*4+1], kp1[index1*4+0], 1, kp2[index2*4+1], kp2[index2*4+0], 1;
    }

    free(kp1);
    free(kp2);
    free(desc1);
    free(desc2);
    free(m);
}
Пример #5
0
void coordinateDescentLasso(
        const Eigen::MatrixXf &data,
        const Eigen::VectorXf &output,
        Eigen::VectorXf &weights, 
        const float lambda,
        const int nIters,
        const bool verbose)
{
    const int nExamples = data.rows();
    const int nFeatures = data.cols();
    for(int iter = 0; iter < nIters; ++iter){
        const int featureInd = iter % nFeatures;
        float rho = 0;
        for(int i = 0; i < nExamples; ++i)
            rho += residualWithoutKWeight(
                    weights,
                    data.row(i).transpose(),
                    output[i],
                    featureInd) * data(i, featureInd);
        auto column = data.col(featureInd);
        float sumOfColumn = (column.transpose() * column).sum();
        weights[featureInd] = coordinateDescentStepLasso(weights[featureInd], sumOfColumn, rho, lambda);
        if(verbose){
            const float err = rss(weights, data, output);
            std::cout << "iter " << iter << " err " << err << std::endl;
            std::cout << weights << std::endl;
        }
    }
}
Пример #6
0
LIMA_TENSORFLOWSPECIFICENTITIES_EXPORT Eigen::MatrixXi viterbiDecode(
  const Eigen::MatrixXf& score,
  const Eigen::MatrixXf& transitionParams){
  if(score.size()==0){
    std::cerr<<"The output is empty. Check the inputs.";
    return Eigen::MatrixXi();
  }
  if(transitionParams.size()==0){
    std::cerr<<"The transition matrix is empty. Check it.";
    return Eigen::MatrixXi();
  }
  //1. Initialization of matrices
  Eigen::MatrixXf trellis=  Eigen::MatrixXf::Zero(score.rows(),score.cols());
  Eigen::MatrixXi backpointers= Eigen::MatrixXi::Zero(score.rows(),score.cols());
  trellis.row(0)=score.row(0);
  Eigen::MatrixXf v(transitionParams.rows(),transitionParams.cols());

  //2.Viterbi algorithm
  for(auto k=1;k<score.rows();++k)
  {
    for(auto i=0;i<transitionParams.rows();++i){
      for(auto j=0;j<transitionParams.cols();++j){
        v(i,j)=trellis(k-1,i)+transitionParams(i,j);
      }
    }
    trellis.row(k)=score.row(k)+v.colwise().maxCoeff();//equivalent to np.max() function
    for(auto i=0;i<backpointers.cols();++i)
    {
      v.col(i).maxCoeff(&backpointers(k,i));//equivalent to np.argmax() function
    }
  }
  //In Eigen, vector is just a particular matrix with one row or one column
  Eigen::VectorXi viterbi(score.rows());
  trellis.row(trellis.rows()-1).maxCoeff(&viterbi(0));
  Eigen::MatrixXi bp = backpointers.colwise().reverse();
  for(auto i=0;i<backpointers.rows()-1;++i)
  {
    viterbi(i+1)=bp(i,viterbi(i));
  }

  float viterbi_score=trellis.row(trellis.rows()-1).maxCoeff();
  LIMA_UNUSED(viterbi_score);
  //std::cout<<"Viterbi score of the sentence: "<<viterbi_score<<std::endl;

  return viterbi.colwise().reverse();
}
Пример #7
0
/* return word nearest neighbors in the embedding space */
void getnn(FILE* fout, Eigen::MatrixXf m, const int idx){
    // find nearest neighbour
    Eigen::VectorXf dist = (m.rowwise() - m.row(idx)).rowwise().squaredNorm();
    std::vector<int> sortidx = REDSVD::Util::ascending_order(dist);
    for (int i=1;i<top;i++){
        fprintf(fout, "%s, ", tokename[sortidx[i]]);
    }
    fprintf(fout, "%s\n", tokename[sortidx[top]]);
}
void meanSubtract(Eigen::MatrixXf &data)
{
    double x_sum = data.row(0).sum();
    double y_sum = data.row(1).sum();
    x_sum /= data.cols();
    y_sum /= data.cols();
    Eigen::MatrixXf xsum = Eigen::MatrixXf::Constant(1, data.cols(), x_sum); 
    Eigen::MatrixXf ysum = Eigen::MatrixXf::Constant(1, data.cols(), y_sum); 
    for (int i = 0; i < data.rows(); i++)
    {
        if (i % 2 == 0)
        {
            data.row(i) = data.row(i) - xsum;
        }
        else
        {
            data.row(i) = ysum - data.row(i);
        }
    }
}
Пример #9
0
	//************This method could be used only when there are many steps, otherwise it might be removed*******************************************//
	//Create a combined mxb matrix for trajectories from A to B to A
	Eigen::MatrixXf CreateCombinedMXBMatrix(Eigen::MatrixXf matrixA, Eigen::MatrixXf matrixB, float increment, int i, int j, int offset)
	{
	    //Create a matrix with zmp trajectory from A to B
	    Eigen::MatrixXf mxbMatrixBA = MXB(matrixA.row(i), matrixB.row(j), increment, offset);

		Eigen::MatrixXf noZMPMovement(mxbMatrixBA.rows(), mxbMatrixBA.cols());
		for(int i = 0; i < mxbMatrixBA.rows(); i++)
		{
			noZMPMovement.row(i) = mxbMatrixBA.bottomRows(1);
		}

        Eigen::MatrixXf tempMatrix(2*mxbMatrixBA.rows(), mxbMatrixBA.cols());
        tempMatrix << mxbMatrixBA, noZMPMovement;
        mxbMatrixBA.swap(tempMatrix);

	    //Append both step trajectories
	    if(matrixA.rows() > i+1)
	    {
			//Create a matrix with zmp trajectory from B to A
	        Eigen::MatrixXf mxbMatrixAB = MXB(matrixB.row(j), matrixA.row(i+1), increment, offset);

			for(int i = 0; i < mxbMatrixAB.rows(); i++)
			{
				noZMPMovement.row(i) = mxbMatrixAB.bottomRows(1);
			}

	        Eigen::MatrixXf tempMatrix2(2*mxbMatrixAB.rows(), mxbMatrixAB.cols());
	        tempMatrix2 << mxbMatrixAB, noZMPMovement;
	        mxbMatrixAB.swap(tempMatrix2);

	        Eigen::MatrixXf mxbMatrix(mxbMatrixBA.rows()+mxbMatrixAB.rows(), mxbMatrixBA.cols());
	        mxbMatrix << mxbMatrixBA, mxbMatrixAB;

	        return mxbMatrix;
	    }
	    else
	    {
	        return mxbMatrixBA;
	    }
	}
Пример #10
0
void
writeLDRFile(const std::string& file, const Eigen::MatrixXf& data)
{
    FILE *ldrFile = fopen(file.c_str(), "wb");

    for (int r = 0; r < data.rows(); r++)
    {
        Eigen::VectorXf v = data.row(r);
        float pBuffer[] = {v[0], v[1], v[2], v[3], v[4], v[5]};
        fwrite(pBuffer, 1, sizeof(pBuffer), ldrFile);
    }

    fclose(ldrFile);
};
Пример #11
0
void saveModel(std::string modelname, std::map<std::string, size_t>& word2id, Eigen::MatrixXf& inner, Eigen::MatrixXf& outer) {

    std::ofstream sink(modelname.c_str());

    if (!sink.good()) {
        std::cerr << "Error opening file " << modelname << std::endl;
        std::exit(-1);
    }

    sink << word2id.size() << std::endl;
    for (std::pair<const std::string, size_t>& kv : word2id) {
        sink << kv.first << "\t" << kv.second << std::endl;
    }

    sink << "inner vector" << std::endl;
    for (size_t i = 0; i < inner.rows(); ++i) {
        sink << inner.row(i) << std::endl;
    }

    sink << "outer vector" << std::endl;
    for (size_t i = 0; i < outer.rows(); ++i) {
        sink << outer.row(i) << std::endl;
    }
}
Пример #12
0
	Eigen::VectorXf ZMeshFilterManifold::computeMaxEigenVector(const Eigen::MatrixXf& inputMat, const std::vector<bool>& clusterK)
	{
		//Eigen::VectorXf ret(rangeDim_);
		Eigen::VectorXf ret(3);
		ret.fill(0);
		int count = 0;
		for (int i=0; i<clusterK.size(); i++) if (clusterK[i]) count++;
		Eigen::MatrixXf realInput(count, rangeDim_);
		//Eigen::MatrixXf realInput(count, 3);
		count = 0;
		for (int i=0; i<clusterK.size(); i++)
		{
			if (clusterK[i])
			{
				realInput.row(count) = inputMat.row(i);
				//realInput.row(count) = MATH::spherical2Cartesian(inputMat.row(i));
				count++;
			}
		}
		Eigen::MatrixXf mat = realInput.transpose()*realInput;
		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenSolver(mat);
		if (eigenSolver.info()!=Eigen::Success) {
			std::cerr << "Error in SVD decomposition!\n";
			return ret;
		}
		Eigen::VectorXf eigenValues = eigenSolver.eigenvalues();
		Eigen::MatrixXf eigenVectors = eigenSolver.eigenvectors();
		int maxIdx = 0;
		float maxValue = eigenValues(maxIdx);
		for (int i=1; i<eigenValues.size(); i++)
		{
			if (eigenValues(i)>maxValue)
			{
				maxIdx = i;
				maxValue = eigenValues(maxIdx);
			}
		}
		ret = eigenVectors.col(maxIdx);
		return ret;
// 		Eigen::VectorXf retSph = MATH::cartesian2Spherical(ret, 1);
// 		return retSph.head(rangeDim_);
	}
Пример #13
0
void Match
(
  const sfm::SfM_Data & sfm_data,
  const sfm::Regions_Provider & regions_provider,
  const Pair_Set & pairs,
  float fDistRatio,
  PairWiseMatchesContainer & map_PutativesMatches // the pairwise photometric corresponding points
)
{
  C_Progress_display my_progress_bar( pairs.size() );

  // Collect used view indexes
  std::set<IndexT> used_index;
  // Sort pairs according the first index to minimize later memory swapping
  using Map_vectorT = std::map<IndexT, std::vector<IndexT>>;
  Map_vectorT map_Pairs;
  for (Pair_Set::const_iterator iter = pairs.begin(); iter != pairs.end(); ++iter)
  {
    map_Pairs[iter->first].push_back(iter->second);
    used_index.insert(iter->first);
    used_index.insert(iter->second);
  }

  using BaseMat = Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

  // Init the cascade hasher
  CascadeHasher cascade_hasher;
  if (!used_index.empty())
  {
    const IndexT I = *used_index.begin();
    std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
    const size_t dimension = regionsI.get()->DescriptorLength();
    cascade_hasher.Init(dimension);
  }

  std::map<IndexT, HashedDescriptions> hashed_base_;

  // Compute the zero mean descriptor that will be used for hashing (one for all the image regions)
  Eigen::VectorXf zero_mean_descriptor;
  {
    Eigen::MatrixXf matForZeroMean;
    for (int i =0; i < used_index.size(); ++i)
    {
      std::set<IndexT>::const_iterator iter = used_index.begin();
      std::advance(iter, i);
      const IndexT I = *iter;
      std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
      const ScalarT * tabI =
        reinterpret_cast<const ScalarT*>(regionsI.get()->DescriptorRawData());
      const size_t dimension = regionsI.get()->DescriptorLength();
      if (i==0)
      {
        matForZeroMean.resize(used_index.size(), dimension);
        matForZeroMean.fill(0.0f);
      }
      if (regionsI.get()->RegionCount() > 0)
      {
        Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.get()->RegionCount(), dimension);
        matForZeroMean.row(i) = CascadeHasher::GetZeroMeanDescriptor(mat_I);
      }
    }
    zero_mean_descriptor = CascadeHasher::GetZeroMeanDescriptor(matForZeroMean);
  }

  // Index the input regions
#ifdef OPENMVG_USE_OPENMP
  #pragma omp parallel for schedule(dynamic)
#endif
  for (int i =0; i < used_index.size(); ++i)
  {
    std::set<IndexT>::const_iterator iter = used_index.begin();
    std::advance(iter, i);
    const IndexT I = *iter;
    std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
    const ScalarT * tabI =
      reinterpret_cast<const ScalarT*>(regionsI.get()->DescriptorRawData());
    const size_t dimension = regionsI.get()->DescriptorLength();

    Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.get()->RegionCount(), dimension);
    HashedDescriptions hashed_description = cascade_hasher.CreateHashedDescriptions(mat_I,
      zero_mean_descriptor);
#ifdef OPENMVG_USE_OPENMP
    #pragma omp critical
#endif
    {
      hashed_base_[I] = std::move(hashed_description);
    }
  }

  // Perform matching between all the pairs
  for (Map_vectorT::const_iterator iter = map_Pairs.begin();
    iter != map_Pairs.end(); ++iter)
  {
    const IndexT I = iter->first;
    const std::vector<IndexT> & indexToCompare = iter->second;

    std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
    if (regionsI.get()->RegionCount() == 0)
    {
      my_progress_bar += indexToCompare.size();
      continue;
    }

    const std::vector<features::PointFeature> pointFeaturesI = regionsI.get()->GetRegionsPositions();
    const ScalarT * tabI =
      reinterpret_cast<const ScalarT*>(regionsI.get()->DescriptorRawData());
    const size_t dimension = regionsI.get()->DescriptorLength();
    Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.get()->RegionCount(), dimension);

#ifdef OPENMVG_USE_OPENMP
    #pragma omp parallel for schedule(dynamic)
#endif
    for (int j = 0; j < (int)indexToCompare.size(); ++j)
    {
      size_t J = indexToCompare[j];
      std::shared_ptr<features::Regions> regionsJ = regions_provider.get(J);

      if (regionsI.get()->Type_id() != regionsJ.get()->Type_id())
      {
#ifdef OPENMVG_USE_OPENMP
        #pragma omp critical
#endif
        ++my_progress_bar;
        continue;
      }

      // Matrix representation of the query input data;
      const ScalarT * tabJ = reinterpret_cast<const ScalarT*>(regionsJ.get()->DescriptorRawData());
      Eigen::Map<BaseMat> mat_J( (ScalarT*)tabJ, regionsJ.get()->RegionCount(), dimension);

      IndMatches pvec_indices;
      using ResultType = typename Accumulator<ScalarT>::Type;
      std::vector<ResultType> pvec_distances;
      pvec_distances.reserve(regionsJ.get()->RegionCount() * 2);
      pvec_indices.reserve(regionsJ.get()->RegionCount() * 2);

      // Match the query descriptors to the database
      cascade_hasher.Match_HashedDescriptions<BaseMat, ResultType>(
        hashed_base_[J], mat_J,
        hashed_base_[I], mat_I,
        &pvec_indices, &pvec_distances);

      std::vector<int> vec_nn_ratio_idx;
      // Filter the matches using a distance ratio test:
      //   The probability that a match is correct is determined by taking
      //   the ratio of distance from the closest neighbor to the distance
      //   of the second closest.
      matching::NNdistanceRatio(
        pvec_distances.begin(), // distance start
        pvec_distances.end(),   // distance end
        2, // Number of neighbor in iterator sequence (minimum required 2)
        vec_nn_ratio_idx, // output (indices that respect the distance Ratio)
        Square(fDistRatio));

      matching::IndMatches vec_putative_matches;
      vec_putative_matches.reserve(vec_nn_ratio_idx.size());
      for (size_t k=0; k < vec_nn_ratio_idx.size(); ++k)
      {
        const size_t index = vec_nn_ratio_idx[k];
        vec_putative_matches.emplace_back(pvec_indices[index*2].j_, pvec_indices[index*2].i_);
      }

      // Remove duplicates
      matching::IndMatch::getDeduplicated(vec_putative_matches);

      // Remove matches that have the same (X,Y) coordinates
      const std::vector<features::PointFeature> pointFeaturesJ = regionsJ.get()->GetRegionsPositions();
      matching::IndMatchDecorator<float> matchDeduplicator(vec_putative_matches,
        pointFeaturesI, pointFeaturesJ);
      matchDeduplicator.getDeduplicated(vec_putative_matches);

#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
      {
        ++my_progress_bar;
        if (!vec_putative_matches.empty())
        {
          map_PutativesMatches.insert( make_pair( make_pair(I,J), std::move(vec_putative_matches) ));
        }
      }
    }
  }
}
Пример #14
0
int main(int argc, char** argv)
{
    std::string path;
    po::options_description desc("Calculates one point cloud for classification\n======================================\n**Allowed options");

    desc.add_options()
            ("help,h", "produce help message")
            ("path,p", po::value<std::string>(&path)->required(), "Path to folders with pics")

            ;

    po::variables_map vm;
    po::parsed_options parsed = po::command_line_parser(argc,argv).options(desc).allow_unregistered().run();
    po::store(parsed, vm);
    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return false;
    }

    try
    {
        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
        return false;
    }

//    std::string pretrained_binary_proto = "/home/martin/github/caffe/models/bvlc_alexnet/bvlc_alexnet.caffemodel";
//    std::string feature_extraction_proto = "/home/martin/github/caffe/models/bvlc_alexnet/deploy.prototxt";
    std::string pretrained_binary_proto = "/home/martin/github/caffe/models/bvlc_reference_caffenet/bvlc_reference_caffenet.caffemodel";
    std::string feature_extraction_proto = "/home/martin/github/caffe/models/bvlc_reference_caffenet/deploy.prototxt";
    std::string mean_file = "/home/martin/github/caffe/data/ilsvrc12/imagenet_mean.binaryproto";
    //    std::vector<std::string> extract_feature_blob_names;
    //    extract_feature_blob_names.push_back("fc7");



    Eigen::MatrixXf all_model_signatures_, test;

    v4r::CNN_Feat_Extractor<pcl::PointXYZRGB,float>::Parameter estimator_param;
    //estimator_param.init(argc, argv);
    v4r::CNN_Feat_Extractor<pcl::PointXYZRGB,float>::Ptr estimator;
    //v4r::CNN_Feat_Extractor<pcl::PointXYZRGB,float> estimator;
    estimator.reset(new v4r::CNN_Feat_Extractor<pcl::PointXYZRGB, float>(estimator_param));
    //estimator.reset();
    //estimator->setExtractFeatureBlobNames(extract_feature_blob_names);
    estimator->setFeatureExtractionProto(feature_extraction_proto);
    estimator->setPretrainedBinaryProto(pretrained_binary_proto);
    estimator->setMeanFile(mean_file);
    //estimator->init();
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    //pcl::PointCloud<pcl::PointXYZRGB> test(new pcl::PointCloud<pcl::PointXYZRGB>);
    char end = path.back();
    if(end!='/')
        path.append("/");
    std::vector<std::string> objects(v4r::io::getFoldersInDirectory(path));

    objects.erase(find(objects.begin(),objects.end(),"svm"));
    std::vector<std::string> paths,Files;
    std::vector<std::string> Temp;
    std::string fs, fp, fo;
    Eigen::VectorXi models;
    std::vector<std::string> modelnames;
    std::vector<int> indices;


    v4r::svmClassifier::Parameter paramSVM;
    paramSVM.knn_=1;
    paramSVM.do_cross_validation_=0;
    v4r::svmClassifier classifier(paramSVM);

    if(!(v4r::io::existsFile(path+"svm/Signatures.txt")&&v4r::io::existsFile(path+"svm/Labels.txt"))){
        for(size_t o=0;o<objects.size();o++){
        //for(size_t o=0;o<1;o++){
            fo = path;
            fo.append(objects[o]);
            fo.append("/");
            paths.clear();
            paths = v4r::io::getFilesInDirectory(fo,".*.JPEG",false);
            modelnames.push_back(objects[o]);
            int old_rows = all_model_signatures_.rows();
            all_model_signatures_.conservativeResize(all_model_signatures_.rows()+paths.size(),4096);
            for(size_t i=0;i<paths.size();i++){
//          for(size_t i=0;i<3;i++){
                fp = fo;
                fp.append(paths[i]);
                std::cout << "Teaching File: " << fp << std::endl;


                //            int rows = image.rows;
                //            int cols = image.cols;

                //            int a,b;
                //            if(rows>256)
                //                a = floor(rows/2)-128;
                //            else
                //                a=0;
                //            if(cols<256)
                //                b = floor(cols/2)-128;
                //            else
                //                b=0;
                //            if(rows<256||cols<256)
                //                continue;
                //cv::Rect r(b,a,256,256);
                //            cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );// Create a window for display.
                //            cv::imshow( "Display window", image );
                //            cv::waitKey();
                //estimator.setIm(image);
                //estimator.setIndices(indices);
                //estimator->compute(image,signature);
                //test = signature.transpose();
                cv::Mat image = cv::imread(fp);
                Eigen::MatrixXf signature;
                if(image.rows != 256 || image.cols != 256)
                    cv::resize( image, image, cv::Size(256, 256));
                //bool success = estimator->computeCNN(signature);
                estimator->compute(image,signature);
                //all_model_signatures_.conservativeResize(all_model_signatures_.rows()+1,4096);


                all_model_signatures_.row(old_rows+i) = signature.row(0);
                models.conservativeResize(models.rows()+1);
                models(models.rows()-1) = o;


                // std::cout<<all_model_signatures_ <<std::endl;
                std::cout<<"ok" <<std::endl;

            }





        }

        std::string f_file = path;
        v4r::io::writeDescrToFile(f_file.append("svm/Signatures.txt"),all_model_signatures_);
        f_file = path;
        v4r::io::writeLabelToFile(f_file.append("svm/Labels.txt"),models);
    }
    else{
        all_model_signatures_ = v4r::io::readDescrFromFile(path+"svm/Signatures.txt",0,4096);
        models = v4r::io::readLabelFromFile(path+"svm/Labels.txt",0);
        std::cout << "Loading Descriptors and Labels from " << path << "svm." <<std::endl;
    }



    fs = path;
    fs.append("svm/Class.model");
    classifier.setOutFilename(fs);

    //test.resize(all_model_signatures_.cols(),all_model_signatures_.rows());
    //test = all_model_signatures_.transpose();
    //classifier.shuffleTrainingData(all_model_signatures_, models);
    classifier.param_.do_cross_validation_=0;
    int testC = classifier.param_.svm_.C;
    classifier.param_.svm_.probability=1;
    classifier.setNumClasses(objects.size());
    classifier.train(all_model_signatures_, models);

    classifier.saveModel(fs);








}
void ModelRachel::SimulateModel(DF_OUTPUT df[], MAT_FLOAT T_ext_mpc, MAT_FLOAT T_ext_spot, MAT_FLOAT O_mpc, MAT_FLOAT O_spot, PARAMS& ParamsIn,
		const int& time_step_mpc, const int& time_step_spot, const int& total_rooms, int time_instances_mpc, int time_instances_spot,
		const int& control_type, const int& horizon) {

	int k = 0;
	float response = 0;

	int n_mpc = time_instances_mpc;
	int n_spot = time_instances_spot;

	PARAMS ParamsErr;
	ParamsErr = ErrorInParams(ParamsIn, ParamsIn.CommonErrors.err_bparams);

	ComputeCoefficients(time_step_mpc, total_rooms, ParamsIn);

	/* Output of the Program */

	// Temperature Matrices
	Eigen::MatrixXf T = Eigen::MatrixXf::Zero(n_spot, total_rooms);
	Eigen::MatrixXf TR1 = Eigen::MatrixXf::Zero(n_spot, total_rooms);
	Eigen::MatrixXf TR2 = Eigen::MatrixXf::Zero(n_spot, total_rooms);

	// Delta Matrices
	Eigen::MatrixXf DeltaTR1 = Eigen::MatrixXf::Zero(n_spot, total_rooms);
	Eigen::MatrixXf DeltaTR2 = Eigen::MatrixXf::Zero(n_spot, total_rooms);

	// PPV
	Eigen::MatrixXf PPV = Eigen::MatrixXf::Zero(n_spot, total_rooms);

	// AHU Parameters
	Eigen::MatrixXf r = Eigen::MatrixXf::Zero(n_spot, 1);
	Eigen::MatrixXf MixedAirTemperature = Eigen::MatrixXf::Zero(n_spot, 1);
	Eigen::MatrixXf PowerAHU = Eigen::MatrixXf::Zero(n_spot, 1);

	// Initialization
	size_t step_size_mpc = (horizon * 60 * 60) / time_step_mpc;
	size_t step_size_spot = (horizon * 60 * 60) / time_step_spot;

	std::cout << "Step Size MPC: " << step_size_mpc << ", Step Size SPOT: " << step_size_spot << "\n";

	// std::cout << duration << "\n" << horizon << "\n" << n << "\n" << step_size << "\n";

	MAT_FLOAT T_ext_blk = MAT_FLOAT::Ones(step_size_mpc, 1);
	MAT_FLOAT T_ext_eblk = MAT_FLOAT::Ones(step_size_mpc, 1);

	MAT_FLOAT O_blk = MAT_FLOAT::Ones(step_size_mpc, total_rooms);

	time_t start_time = df[k].t;
	struct tm *date = gmtime(&start_time);
	int Time_IH = (date->tm_min)/10;

	ControlBox cb;
	ControlVariables CV = cb.DefaultControl(total_rooms, ParamsIn);

	MAT_FLOAT SPOT_State = MAT_FLOAT::Zero(n_spot, total_rooms);
	SPOT_State.row(k) = CV.SPOT_CurrentState;

	T.row(k) = Eigen::VectorXf::Ones(total_rooms) * 21;
	TR1.row(k) = T.row(k) + DeltaTR1.row(k);
	TR2.row(k) = T.row(k) + DeltaTR2.row(k);

	PPV.row(k) = O_spot.row(k).array() * ((ParamsIn.PMV_Params.P1 * TR1.row(k))
					- (ParamsIn.PMV_Params.P2 * MAT_FLOAT::Zero(1, total_rooms))
					+ (ParamsIn.PMV_Params.P3 * MAT_FLOAT::Zero(1, total_rooms))
					- (ParamsIn.PMV_Params.P4 * MAT_FLOAT::Ones(1, total_rooms))).array();

	/* Update Output Frame */
	df[k].response = response;
	for (size_t room = 0; room < (size_t) total_rooms; room++) {
		df[k].ppv[room] = PPV(k, room);
		df[k].tspot[room] = TR1(k, room);
		df[k].tnospot[room] = TR2(k, room);
	}
	std::cout << "Room: \n";

	int time_step_ratio = time_step_mpc / time_step_spot;
	int k_spot_prev = k * time_step_ratio;	// Converting MPC previous index to SPOT index
	int k_spot = k * time_step_ratio;		// Converting MPC current index to SPOT index

	std::cout << TR2.row(k_spot) << "\n";
	for(size_t j = 1; j < time_step_ratio; j = j + 1) {
		/* Update Output Frame */
		df[k_spot_prev+j].weather_err = T_ext_blk(k_spot_prev);		// External Temperature
		df[k_spot_prev+j].power = PowerAHU(k_spot_prev);
		df[k_spot_prev+j].r = r(k_spot_prev);
		df[k_spot_prev+j].tmix = MixedAirTemperature(k_spot_prev);
		df[k_spot+j].response = response;

		for (size_t room = 0; room < (size_t) total_rooms; room++) {
			df[k_spot+j].ppv[room] = PPV(k_spot, room);
			df[k_spot+j].tspot[room] = TR1(k_spot, room);
			df[k_spot+j].tnospot[room] = TR2(k_spot, room);
			df[k_spot_prev+j].spot_status[room] = SPOT_State(k_spot_prev, room);
		}

		PowerAHU(k_spot_prev+j) = PowerAHU(k_spot_prev);
		r(k_spot_prev+j) = r(k_spot_prev);
		MixedAirTemperature(k_spot_prev+j) = MixedAirTemperature(k_spot_prev);

		for (size_t room = 0; room < (size_t) total_rooms; room++) {
			PPV(k_spot+j, room) = PPV(k_spot, room);
			TR1(k_spot+j, room) = TR1(k_spot, room);
			TR2(k_spot+j, room) = TR2(k_spot, room);
			SPOT_State(k_spot_prev+j, room) = SPOT_State(k_spot_prev, room);
		}
	}

	for(size_t k = 1; k <= (size_t) (n_mpc - step_size_mpc); k = k + 1) {
		k_spot = k * time_step_ratio;		// Converting MPC current index to SPOT index
		k_spot_prev = (k-1) * time_step_ratio;	// Converting MPC previous index to SPOT index

		start_time = df[k_spot_prev].t;	// Previous MPC Index

		std::cout << "Start Time: " << df[k_spot_prev].t << "\n";	// Previous MPC Index
		struct tm *date = gmtime(&start_time);
		Time_IH = (date -> tm_min)/10;

		T_ext_blk = T_ext_mpc.block(k-1, 0, step_size_mpc, 1);		// Previous MPC Index
		O_blk = O_mpc.block(k-1, 0, step_size_mpc, total_rooms);	// Every Time Step of MPC

		switch (control_type) {
		case 1:
			CV = cb.DefaultControl(total_rooms, ParamsIn);
			break;
		case 2:
			CV = cb.ReactiveControl(total_rooms, TR1.row(k-1), O_mpc.row(k-1), k-1, SPOT_State.row(k-1), ParamsIn);
			break;
		case 3:
			T_ext_eblk = ErrorInWeather(T_ext_blk, ParamsIn.CommonErrors.err_text);
			std::cout << "C: " << ParamsIn.CommonRoom.C << " >> " << ParamsErr.CommonRoom.C << std::endl;
			std::cout << "C_: " << ParamsIn.CommonRoom.C_ << " >> " << ParamsErr.CommonRoom.C_ << std::endl;
			std::cout << "alpha_o: " << ParamsIn.CommonRoom.alpha_o << " >> " << ParamsErr.CommonRoom.alpha_o << std::endl;
			std::cout << "alpha_r: " << ParamsIn.CommonRoom.alpha_r << " >> " << ParamsErr.CommonRoom.alpha_r << std::endl;
			//std::cout << T_ext_blk << std::endl;
			//std::cout << T_ext_eblk << std::endl;

			response = cb.MPCControl(step_size_mpc, time_step_mpc, T_ext_blk, O_blk, TR2.row(k_spot - 1),
					DeltaTR1.row(k_spot - 1), ParamsErr, horizon, Time_IH, CV); // Every time step of SPOT except last argument
			break;
		default:
			break;
		}

		/* Temperature Change in the Room Due to HVAC */
		SPOT_State.row(k_spot_prev) = CV.SPOT_CurrentState;
		r.row(k_spot_prev) << CV.r;

		// Impact of Weather
		WI_CRT = TR2.row(k_spot-1) * CoWI_CRT_Matrix;
		WI_OAT = T_ext_mpc.row(k-1) * CoWI_OAT_Matrix;

		// Impact of HVAC
		HI_CRT = TR2.row(k_spot-1) * CV.SAV_Matrix * CoHI_CRT_Matrix;
		HI_SAT = CV.SAT * CV.SAV_Matrix * CoHI_SAT_Matrix;

		// Impact of Equipments
		EI_OLEL = O_mpc.row(k-1) * CoEI_OLEL_Matrix;

		T.row(k_spot) = WI_CRT + WI_OAT + HI_CRT + HI_SAT + EI_OLEL;

		//std::cout << TR2 << "\n";
		std::cout << "1: " << WI_CRT << ", 2: " << WI_OAT << ", 3: " << HI_CRT << ", 4: " << HI_SAT << ", 5: " << EI_OLEL << "\n";
		// exit(1);

		/* Temperature Change in SPOT Region*/

		// Impact of Region Coupling
		RC_CiRT = DeltaTR1.row(k_spot-1) * CoRC_CiRT_Matrix;

		// Impact of SPOT
		SI_SCS = SPOT_State.row(k_spot_prev) * CoSI_SCS_Matrix; // CV.SPOT_CurrentState * CoSI_SCS_Matrix;

		// Impact of Occupants
		OI_OHL = O_mpc.row(k-1) * CoOI_OHL_Matrix;

		DeltaTR1.row(k_spot) = RC_CiRT + SI_SCS + OI_OHL;
		TR1.row(k_spot) = T.row(k_spot) + DeltaTR1.row(k_spot);

		/* Temperature Change in Non-SPOT Region*/

		// Impact of Region Coupling
		RC_CiR1T = DeltaTR1.row(k_spot-1) * CoRC_CiR1T_Matrix;

		DeltaTR2.row(k_spot) = RC_CiR1T;
		TR2.row(k_spot) = T.row(k_spot) + DeltaTR2.row(k_spot);

		PPV.row(k_spot) = O_mpc.row(k-1).array() * ((ParamsIn.PMV_Params.P1 * TR1.row(k_spot))
			- (ParamsIn.PMV_Params.P2 * MAT_FLOAT::Zero(1, total_rooms))
			+ (ParamsIn.PMV_Params.P3 * MAT_FLOAT::Zero(1, total_rooms))
			- (ParamsIn.PMV_Params.P4 * MAT_FLOAT::Ones(1, total_rooms))).array();

		MixedAirTemperature.row(k_spot_prev) << GetMixedAirTemperature(TR2.row(k_spot-1), T_ext_mpc.row(k-1), r.row(k_spot_prev).value());
		PowerAHU.row(k_spot_prev) << GetAHUPower(MixedAirTemperature.row(k_spot_prev).value(),
					CV.SPOT_CurrentState, CV.SAT_Value, CV.SAV_Matrix, ParamsIn);

		std::cout << T_ext_mpc(k-1) << " => " << df[k_spot_prev].weather << std::endl;

		/* Update Output Frame */
		df[k_spot_prev].weather_err = T_ext_mpc(k-1);		// External Temperature
		df[k_spot_prev].power = PowerAHU(k_spot_prev);
		df[k_spot_prev].r = r(k_spot_prev);
		df[k_spot_prev].tmix = MixedAirTemperature(k_spot_prev);
		df[k_spot].response = response;

		for (size_t room = 0; room < (size_t) total_rooms; room++) {
			df[k_spot].ppv[room] = PPV(k_spot, room);
			df[k_spot].tspot[room] = TR1(k_spot, room);
			df[k_spot].tnospot[room] = TR2(k_spot, room);
			df[k_spot_prev].spot_status[room] = SPOT_State(k_spot_prev, room);
		}

		//		std::cout << "TR2: " << TR2.row(k) << "\n";
		//		std::cout << "Delta_TR1: " << DeltaTR1.row(k) << "\n";
		// std::cout << "SAT Prev: " << CV.SAT_Value << "\n";
		//		std::cout << "SPOT Current State: " << CV.SPOT_CurrentState << "\n";

		//		std::cout << "T Ext: " << T_ext_blk.row(1) << "\n";
		//		std::cout << "Occupancy: " << O.row(k) << "\n";

		//std::cout << "Mixed Air Temperature: " << MixedAirTemperature.row(k) << "\n";
		//std::cout << "R: " << r.row(k) << "\n";
		//std::cout << "TR2: " << TR2.row(k) << "\n";
		//std::cout << "T_ext: " << T_ext_blk.row(k) << "\n";

		std::cout << "Power AHU: " << PowerAHU.row(k_spot-1) << "\n";
		std::cout << "Current Value of K SPOT: " << k_spot << "\n";

		for(size_t j = 1; j < time_step_ratio; j = j + 1) {
			CV = cb.ReactiveControl(total_rooms, TR1.row(k-1), O_mpc.row(k-1), k-1, SPOT_State.row(k-1), ParamsIn);

			/* Update Output Frame */
			df[k_spot_prev+j].weather_err = T_ext_mpc(k-1);		// External Temperature
			df[k_spot_prev+j].power = PowerAHU(k_spot_prev);
			df[k_spot_prev+j].r = r(k_spot_prev);
			df[k_spot_prev+j].tmix = MixedAirTemperature(k_spot_prev);
			df[k_spot+j].response = response;

			for (size_t room = 0; room < (size_t) total_rooms; room++) {
				df[k_spot+j].ppv[room] = PPV(k_spot, room);
				df[k_spot+j].tspot[room] = TR1(k_spot, room);
				df[k_spot+j].tnospot[room] = TR2(k_spot, room);
				df[k_spot_prev+j].spot_status[room] = SPOT_State(k_spot_prev, room);
			}

			PowerAHU(k_spot_prev+j) = PowerAHU(k_spot_prev);
			r(k_spot_prev+j) = r(k_spot_prev);
			MixedAirTemperature(k_spot_prev+j) = MixedAirTemperature(k_spot_prev);

			for (size_t room = 0; room < (size_t) total_rooms; room++) {
				PPV(k_spot+j, room) = PPV(k_spot, room);
				TR1(k_spot+j, room) = TR1(k_spot, room);
				TR2(k_spot+j, room) = TR2(k_spot, room);
				SPOT_State(k_spot_prev+j, room) = SPOT_State(k_spot_prev, room);
			}
		}
	}

}
Пример #16
0
    Eigen::MatrixXf CoMIK::getJacobianOfCoM(RobotNodePtr node)
    {
        // Get number of degrees of freedom
        size_t nDoF = rns->getAllRobotNodes().size();

        // Create matrices for the position and the orientation part of the jacobian.
        Eigen::MatrixXf position = Eigen::MatrixXf::Zero(3, nDoF);

        const std::vector<RobotNodePtr> parentsN = bodyNodeParents[node];

        // Iterate over all degrees of freedom
        for (size_t i = 0; i < nDoF; i++)
        {
            RobotNodePtr dof = rns->getNode(i);// bodyNodes[i];

            // Check if the tcp is affected by this DOF
            if (find(parentsN.begin(), parentsN.end(), dof) != parentsN.end())
            {
                // Calculus for rotational joints is different as for prismatic joints.
                if (dof->isRotationalJoint())
                {
                    // get axis
                    boost::shared_ptr<RobotNodeRevolute> revolute
                        = boost::dynamic_pointer_cast<RobotNodeRevolute>(dof);
                    THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
                    // todo: find a better way of handling different joint types
                    Eigen::Vector3f axis = revolute->getJointRotationAxis(coordSystem);

                    // For CoM-Jacobians only the positional part is necessary
                    Eigen::Vector3f toTCP = node->getCoMLocal() + node->getGlobalPose().block(0, 3, 3, 1)
                                            - dof->getGlobalPose().block(0, 3, 3, 1);
                    position.block(0, i, 3, 1) = axis.cross(toTCP);
                }
                else if (dof->isTranslationalJoint())
                {
                    // -> prismatic joint
                    boost::shared_ptr<RobotNodePrismatic> prismatic
                        = boost::dynamic_pointer_cast<RobotNodePrismatic>(dof);
                    THROW_VR_EXCEPTION_IF(!prismatic, "Internal error: expecting prismatic joint");
                    // todo: find a better way of handling different joint types
                    Eigen::Vector3f axis = prismatic->getJointTranslationDirection(coordSystem);

                    position.block(0, i, 3, 1) = axis;
                }
            }
        }

        if (target.rows() == 2)
        {
            Eigen::MatrixXf result(2, nDoF);
            result.row(0) = position.row(0);
            result.row(1) = position.row(1);
            return result;
        }
        else if (target.rows() == 1)
        {
            VR_INFO << "One dimensional CoMs not supported." << endl;
        }

        return position;
    }
Пример #17
0
bool LogisticRegressionVT::LogisticVTImpl::TestCovariate(const Matrix& Xnull,
                                                         const Vector& yVec,
                                                         const Matrix& Xcol) {
  // const int n = Xnull.rows;
  const int d = Xnull.cols;
  const int k = Xcol.cols;
  copy(Xnull, &cov);  // Z = n by d = [z_1^T; z_2^T; ...]
  copy(Xcol, &geno);  // S = n by k = [S_1^T, S_2^T, ...]
  copy(yVec, &y);
  copy(this->null.GetPredicted(), &res);  // n by 1
  v = res.array() * (1. - res.array());
  res = y - res;
  Eigen::MatrixXf vsz(d, k);  // \sum_i v_i S_ki Z_i = n by d matrix
  Eigen::MatrixXf tmp;

  // calculate U and V
  const Eigen::MatrixXf& S = geno;
  const Eigen::MatrixXf& Z = cov;

  // U = (S.transpose() * (res.asDiagonal())).rowsum();
  U = res.transpose() * S;  // 1 by k matrix
                            // for (int i = 0; i < d; ++i) {
  //   vsz.col(i) = (Z * (v.array() *
  //   S.col().array()).matrix().asDiagonal()).rowsum();
  // }
  vsz = cov.transpose() * v.asDiagonal() * S;  // vsz: d by k matrix
  // const double zz = (v.array() *
  // (Z.array().square().rowise().sum()).array()).sum();
  Eigen::MatrixXf zz =
      cov.transpose() * v.asDiagonal() * cov;  // zz: d by d matrix

  // V.size(k, 1);
  //   V(i, 1) = (v.array() * (S.col(i).array().square())).sum() -
  //   vsz.row(i).transpose() * vsz.row(i) /   zz;
  // }
  V = (v.asDiagonal() * (S.array().square().matrix()))
          .colwise()
          .sum();  // - // 1 by k
  tmp = ((vsz).array() * (zz.ldlt().solve(vsz)).array()).colwise().sum();
  V -= tmp;

  // V = (v.asDiagonal() * (S.array().square().matrix())).colwise().sum() -
  // ((vsz).array() * (zz.ldlt().solve(vsz)).array()).colwise().sum();

  // Uk is n by k matrix
  // Uk.size(n, k);
  // for (int i = 0; i < k; ++i) {
  //   Uk.col(i) = res * (S.col(i) - vsz.col(i).transpose()) * Z.col(i) / zz;
  // }
  Uk = res.asDiagonal() * (S - Z * zz.ldlt().solve(vsz));
  // Vkk.size(k, k);
  // for (int i = 0; i < k; ++i) {
  //   for (int j = 0; j <= 1; ++j) {
  //     Vkk(i, j) = Uk.col(i) .transpose() * Uk.col(j);
  //   }
  //   if (i != j) {
  //     Vkk(j, i) = Vkk(i, j);
  //   } else {
  //     if (Vkk(i,i) == 0.0) {
  //       return false; // variance term should be larger than zero.
  //     }
  //   }
  //  }
  Vkk = Uk.transpose() * Uk;

  Eigen::MatrixXf t = U.array() / V.array().sqrt();
  Eigen::RowVectorXf tmp2 = t.row(0).cwiseAbs();
  tmp2.maxCoeff(&maxIndex);

  rep(-tmp2(maxIndex), k, &lower);
  rep(tmp2(maxIndex), k, &upper);
  makeCov(Vkk);
  if (mvnorm.getBandProbFromCor(k, (double*)lower.data(), (double*)upper.data(),
                                (double*)cor.data(), &pvalue)) {
    fprintf(stderr, "Cannot get MVN pvalue.\n");
    return false;
  }

  copy(U, &retU);
  copy(V, &retV);
  copy(t, &retT);
  copy(Vkk, &retCov);
  pvalue = 1.0 - pvalue;
  return true;
};
Пример #18
0
	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;
	}