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;
}
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;
}
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);
}
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;
}
}
}
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();
}
/* 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);
}
}
}
//************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;
}
}
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);
};
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;
}
}
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_);
}
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) ));
}
}
}
}
}
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);
}
}
}
}
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;
}
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;
};
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;
}
