text:
//============================================================================
// Name        : TDEModel.cpp
// Author      : Jordan Frank (jordan.frank@cs.mcgill.ca)
// Copyright   : MIT
// Description : Implementation for the time-delay embedding model.
//============================================================================

#include <stdlib.h>
#include <ctype.h>
#include <iostream>
#include <fstream>
#include <Tisean/tsa.h>
#include <ANN/ANN.h>
#include <opencv/cxcore.h>
#include <boost/random.hpp>
//#include <opencv/cvaux.h>

#include "Utils.h"
#include "BuildTree.h"
#include "TDEModel.h"

using namespace std;
using namespace boost;

TDEModel::TDEModel(Settings* settings) {
    ANNcoord *data, *projecteddata;

    length = settings->length;
    embdim = settings->embdim;
    delay = settings->delay;
    use_pca = settings->pcaembset;

    get_embedding(settings, data, length);

    if (use_pca) {
    	cerr << "Computing PCA bases.\n";
    	computePCABases(data, length, embdim, settings->pcaembdim);
    	projecteddata = projectData(data, length, embdim);
    	delete [] data;
    	data = projecteddata;
    }
    else {
    	avg = NULL;
    	bases = NULL;
    }
    /*
	for (uint i = 0; i < (unsigned)bases->cols; i++) {
		cout << data[i] << " " ;
	}
	cout << endl;
	*/
    get_ann_points(dataPts, data, length, settings->pcaembdim);
    kdTree = new ANNkd_tree(dataPts,length,settings->pcaembdim);
    settings->length = length;
    delete [] data;
}

TDEModel::TDEModel(ifstream* model_file, uint verbosity) {
	int avgsize, basesrows, basescols, i, j;
	*model_file >> delay;
	*model_file >> embdim;
	*model_file >> avgsize;

	if (avgsize > 0) {
		use_pca = 1;
		avg = cvCreateMat(1,avgsize,MAT_TYPE);
		ANNcoord* ptr = (ANNcoord*)avg->data.ptr;
		for (i = 0; i < avgsize; i++) {
			*model_file >> *ptr++;
		}
	}
	else {
		use_pca = 0;
		avg = NULL;
		bases = NULL;
	}
	*model_file >> basesrows >> basescols;
	if (verbosity > 0) {
		if (basescols > 0)
			cerr << "Model params: d=" << delay << " m=" << embdim << " p=" << basescols << endl;
		else
			cerr << "Model params: d=" << delay << " m=" << embdim << endl;
	}
	if (use_pca) {
		bases = cvCreateMat(basesrows,basescols,MAT_TYPE);
		ANNcoord* ptr = (ANNcoord*)bases->data.ptr;
		for (i = 0; i < basesrows; i++) {
			for (j = 0; j < basescols; j++) {
				*model_file >> *ptr++;
			}
		}
	}

	kdTree = new ANNkd_tree(*model_file);
	dataPts = kdTree->thePoints();
	length = kdTree->nPoints();

	if (verbosity > 0)
		cerr << "Loaded " << length << " points." << endl;
	model_file->close();
	delete model_file;
}

TDEModel::~TDEModel() {
	if (avg != NULL) cvReleaseMat(&avg);
	if (bases != NULL) cvReleaseMat(&bases);
	annDeallocPts(dataPts);
	delete kdTree;
}

void TDEModel::DumpTree(char* outfile) {
    ofstream fout(outfile);
    fout << delay << endl;
    fout << embdim << endl;
    if (avg == NULL) {
    	fout << 0 << endl;
    }
    else {
    	fout << avg->cols << endl;
    	for (int i = 0; i < avg->cols; i++) {
    		fout << " " << CV_MAT_ELEM(*avg, ANNcoord, 0, i);
    	}
    }
	fout << endl;
    if (bases == NULL) {
    	fout << "0 0" << endl;
    }
    else {
    	fout << bases->rows << " " << bases->cols << endl;
    	for (int i = 0; i < bases->rows; i++) {
        	fout << CV_MAT_ELEM(*bases, ANNcoord, i, 0);
    		for (int j = 1; j < bases->cols; j++) {
    			fout << " " << CV_MAT_ELEM(*bases, ANNcoord, i, j);
    		}
    		fout << endl;
    	}
    }
    kdTree->Dump(ANNtrue, fout);
    fout.close();
}

void TDEModel::getKNN(ANNpoint ap, uint k, ANNidxArray nn_idx, ANNdistArray dists) {
	kdTree->annkSearch(ap, k, nn_idx, dists);
//    for (uint i = 0; i < k; i++) {
//            cout << "Point " << i+1 << ": [" << dataPts[nn_idx[i]][0] << "," << dataPts[nn_idx[i]][1] << "," << dataPts[nn_idx[i]][2] << "], Dist: " << sqrt(dists[i]) << endl;
//    }
}

void TDEModel::simulateTrajectory(ANNpoint s0, ANNpointArray trajectory, uint dim, ulong N) {
    ANNidxArray nn_idx;
    ANNdistArray dists;
    uint i,j,k;
    variate_generator<mt19937, normal_distribution<> > generator(mt19937(time(0)), normal_distribution<>(0.0,0.1));
    uint neighbours = 4;
	// +1 in case one of the neighbours is the last point in the model.
    nn_idx = new ANNidx[neighbours+1];
    dists = new ANNdist[neighbours+1];

    for (i = 0; i < dim; i++) {
    	trajectory[0][i] = s0[i];
    }

    for (i = 1; i < N; i++) {
    	getKNN(trajectory[i-1], neighbours+1, nn_idx, dists);
    	for (j = 0; j < dim; j++) {
    		trajectory[i][j] = 0.0;
    		for (k = 0; k < neighbours; k++) {
    			if (nn_idx[k] == ANN_NULL_IDX) break;
    			else if (nn_idx[k] == (int)length-1) nn_idx[k] = nn_idx[neighbours];
    			trajectory[i][j] += dataPts[nn_idx[k]+1][j];
    		}
    		trajectory[i][j] = trajectory[i][j] / (ANNcoord)k + generator();
    	}
    }
}

ANNpoint TDEModel::getDataPoint(uint idx) {
	return dataPts[idx];
}

void TDEModel::computePCABases(ANNcoord *data, uint rows, uint cols, uint numbases) {
	CvMat **embedding, *cov, *eigenvectors, *eigenvalues, *vector;
	ANNcoord *basesdata;
	uint i, j, offset;

	cov = cvCreateMat(cols, cols, MAT_TYPE);
	eigenvectors = cvCreateMat(cols,cols,MAT_TYPE);
	eigenvalues = cvCreateMat(cols,cols,MAT_TYPE);
	embedding = new CvMat*[rows];
	for (i = 0; i < rows; i++) {
		vector = cvCreateMatHeader(1, cols, MAT_TYPE);
		cvInitMatHeader(vector, 1, cols, MAT_TYPE, data + i * cols);
		embedding[i] = vector;
	}
	avg = cvCreateMat(1,cols,MAT_TYPE);
	cvCalcCovarMatrix((const CvArr **)embedding, rows, cov, avg, CV_COVAR_NORMAL);
	cvSVD(cov, eigenvalues, eigenvectors, 0, CV_SVD_MODIFY_A);

	basesdata = new ANNcoord[cols*numbases];
	for (i = 0, offset = 0; i < cols; i++) {
		for (j = 0; j < numbases; j++) {
			basesdata[offset++] = ((ANNcoord*)eigenvectors->data.ptr)[i*cols+j];
		}
	}
	bases = cvCreateMatHeader(cols, numbases, MAT_TYPE);
	cvInitMatHeader(bases, cols, numbases, MAT_TYPE, basesdata);

	for (i = 0; i < rows; i++) {
    	cvReleaseMat(&embedding[i]);
    }
    delete [] embedding;
    cvReleaseMat(&cov);
    cvReleaseMat(&eigenvectors);
    cvReleaseMat(&eigenvalues);
}

ANNcoord* TDEModel::projectData(ANNcoord* data, uint rows, uint cols) {
	if (!use_pca) return data;
	ANNcoord* shifteddata = new ANNcoord[rows*cols];
	ANNcoord* projecteddata = new ANNcoord[rows*bases->cols];
	uint i, j, offset;

	for (i = 0, offset = 0; i < rows; i++) {
		for (j = 0; j < cols; j++) {
			shifteddata[offset] = data[offset] - ((ANNcoord*)avg->data.ptr)[j];
			offset++;
		}
	}
	CvMat projected = cvMat(rows, bases->cols, MAT_TYPE, projecteddata);
	CvMat dataMat = cvMat(rows, cols, MAT_TYPE, shifteddata);
	cvGEMM(&dataMat, bases, 1.0, NULL, 0.0, &projected, 0);
	/*
	for (i = 0; i < (unsigned)bases->cols; i++) {
		cout << projecteddata[i] << " " ;
	}
	cout << endl;
	*/
	delete [] shifteddata;
	return projecteddata;
}

max_stars_repo_name: jwf-zz/tdetools

max_issues_repo_name: jwf-zz/tdetools

max_forks_repo_name: jwf-zz/tdetools


text:
/*
Copyright 2014-2015 Glen Joseph Fernandes
(glenjofe@gmail.com)

Distributed under the Boost Software License, Version 1.0.
(http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_ALIGN_DETAIL_MAX_ALIGN_HPP
#define BOOST_ALIGN_DETAIL_MAX_ALIGN_HPP

#include <boost/align/detail/max_size.hpp>
#include <boost/align/alignment_of.hpp>

namespace boost {
namespace alignment {
namespace detail {

template<class A, class B>
struct max_align
    : max_size<alignment_of<A>::value, alignment_of<B>::value> { };

} /* detail */
} /* alignment */
} /* boost */

#endif

ext: hpp

max_stars_repo_name: kindlychung/mediasoup-sfu-cpp

max_issues_repo_name: kindlychung/mediasoup-sfu-cpp

max_forks_repo_name: kindlychung/mediasoup-sfu-cpp


text:
#ifndef SVD_HPP

#define SVD_HPP

#include "print.hpp"
#include "modify.hpp"
#include "sub.hpp"
#include "multiply.hpp"
#include "special.hpp"
#include "../workspace.hpp"
#include "../tools.hpp"
#include <mkl.h>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <vector>
#include <utility>

inline bool comparator ( const max_pair& l, const max_pair& r) { return l.first > r.first; }

namespace dqmc {
    namespace la {

	// extern "C" {
	//     void dgejsv_( char* joba, char* jobu, char* jobv, char* jobr, char* jobt,
	// 		  char* jobp,
	// 		  const int* m, const int* n, double* a, const int* lda,
	// 		  double* sva, double* u, const int* ldu,
	// 		  double* v, const int* ldv, double* work,
	// 		  const int* lwork, int* iwork, int* info );

	//     int LAPACKE_dgejsv( int matrix_layout, char joba, char jobu, char jobv,
	//     			char jobr, char jobt, char jobp, int m,
	//     			int n, const double* a,
	//     			int lda, double* sva, double* u,
	//     			int ldu, double* v, int ldv,
	//     			double* stat, int* istat );

	//     void dgesvj_( const char* joba, const char* jobu, const char* jobv,
	// 		  const int* m, const int* n, double* a,
	// 		  const int* lda, double* sva, const int* mv, double* v,
	// 		  const int* ldv, double* work, int* lwork,
	// 		  int* info );
	// }

	template <typename M, typename V>
	inline void close_svd(int sites, M&__restrict__ U, V&__restrict__ D,
			      M&__restrict__ T,
			      M&__restrict__ out) {
	    using namespace std;

	    for (int col = sites - 1; col >= 0; col--) {
		for (int row = sites - 1; row >= 0; row--) {
		    out(row, col) = 0.;
		    for (int k = sites - 1; k >= 0; k--) {
			if (U(row, k) != 0 && T(k, col) != 0 && D(k) != 0)
			    out(row, col) += ( U(row, k) * T(k, col)) * D(k);
		    }
		}
	    }
	}

	template <typename M, typename V>
	inline void close_svd_inv_diag(int sites, M&__restrict__ U, V&__restrict__ D,
				       M&__restrict__ T,
				       M& out) {
	    using namespace std;

	    for (int col = sites - 1; col >= 0; col--) {
		for (int row = sites - 1; row >= 0; row--) {
		    out(row, col) = 0.;
		    for (int k = sites - 1; k >= 0; k--) {
			if (U(row, k) != 0 && T(k, col) != 0 && D(k) != 0)
			    out(row, col) += ( U(row, k) * T(k, col)) / D(k);
		    }
		}
	    }
	}

	inline int decompose_dgejsv_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				       pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				       pvec_t& work, ivec& ivywork) {
	    using namespace std;

	    int info;
	    int lwork = work.size() - 10;

	    char f = 'F';
	    char j = 'J';
	    char n = 'N';
	    char p = 'P';
	    int rows = U.rows();
	    int cols = U.cols();
	    int T_rows = Tt.rows();
	    // arma::vec work(6);
	    ivec iwork(in.rows() + 3 * in.cols());
	    work.setZero();
	    iwork.setZero();

	    dgejsv_( &f, &f, &j, &n, &n, &p,
	    	     &rows, &cols, in.data(), &rows,
	    	     D.data(), U.data(), &rows,
	    	     Tt.data(), &T_rows, work.data(),
	    	     &lwork, iwork.data(), &info);
	    return info;
	    // info = LAPACKE_dgejsv_work( 102, f, f, j, n, n, p,
	    // 				rows, cols, in.data(), rows,
	    // 				D.data(), U.data(), rows,
	    // 				Tt.data(), T_rows, work.data(),
	    // 				work.size(),
	    // 				iwork.data());

	    // return info;
	}

	inline int decompose_dgejsv_col_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				       pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				       pvec_t& work, ivec& iwork) {
	    using namespace std;

	    int info;
	    int lwork = work.size() - 10; //std::max(1024, int(in.rows()*in.rows()*in.rows()));
	    // pvec_t work(lwork);
	    // ivec iwork(std::max(1024, int(in.rows() * in.cols())));

	    char c = 'C';
	    char u = 'U';
	    char j = 'J';
	    char n = 'N';
	    char p = 'P';
	    int rows = U.rows();
	    int cols = U.cols();
	    int T_rows = Tt.rows();

	    work.setZero();
	    iwork.setZero();
	    // info = LAPACKE_dgejsv_work( 102, c, j, j, n, n, p,
	    // 			   rows, cols, in.data(), rows,
	    // 			   D.data(), U.data(), rows,
	    // 			   Tt.data(), T_rows, work.data(),
	    // 				work.size(),
	    // 			   iwork.data());

	    dgejsv_( &c, &u, &j, &n, &n, &p, // 6
	    	     &rows, &cols, in.data(), &rows, //10
	    	     D.data(), U.data(), &rows, // 13
	    	     Tt.data(), &T_rows, work.data(),
	    	     &lwork, iwork.data(), &info); // 17
	    // std::cout << info << std::endl;
	    return info;
	}

	inline int decompose_jsvd_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				     pvec_t&__restrict__ D, pmat_t&__restrict__ Tt) {
	    using namespace std;

	    char joba = 'G';
	    char jobu = 'U';
	    char jobv = 'V';

	    U = in;

	    int rows = in.rows();
	    int cols = in.cols();
	    int U_rows = in.rows();
	    int T_rows = Tt.rows();

	    int lwork = max(1024, rows * cols);
	    pvec_t work(lwork);
	    work.setOnes();
	    int mv = 0;
	    int info;

	    dgesvj_(&joba, &jobu, &jobv, &rows, &cols, U.data(), &rows,
		    D.data(), &mv, Tt.data(), &T_rows, work.data(), &lwork, &info);

	    if (work(0) != 1.) {
		cout << "MKL ERROR: SVD HAVE TO BE SCALED! " << work(0) << endl;
		throw std::runtime_error("SVD Error");
	    }

	    return info;
	}

	inline int decompose_jsvd_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				     pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				     pvec_t&__restrict__ work) {
	    using namespace std;

	    char joba = 'G';
	    char jobu = 'U';
	    char jobv = 'V';

	    U = in;

	    int rows = in.rows();
	    int cols = in.cols();
	    int U_rows = in.rows();
	    int T_rows = Tt.rows();

	    int lwork = work.size();
	    //pvec_t work(lwork);
	    work.setOnes();
	    int mv = 0;
	    int info;

	    dgesvj_(&joba, &jobu, &jobv, &rows, &cols, U.data(), &rows,
		    D.data(), &mv, Tt.data(), &T_rows, work.data(), &lwork, &info);

	    if (work(0) != 1.) {
		cout << "MKL ERROR: SVD HAVE TO BE SCALED! " << work(0) << endl;
		throw std::runtime_error("SVD Error");
	    }

	    return info;
	}

	inline perm_mat row_sort(mat&__restrict in, mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat row_perm(row_ids);

	    out = row_perm * in;
	    return row_perm;
	}

	inline perm_mat col_sort(mat&__restrict in, mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.cols());
	    vector<max_pair> max_indices;
	    vec max_vals(in.cols());
	    for(int i = 0; i < in.cols(); ++i) {
		max_vals(i) = in.cwiseAbs().col(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec col_ids(in.cols());
	    for (std::vector<max_pair>::iterator it=max_indices.begin();
		 it != max_indices.end(); ++it) {
		col_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat col_perm(col_ids);

	    out = in * col_perm;
	    return col_perm;
	}

	inline void decompose_udt_full_piv(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);
	    Eigen::FullPivHouseholderQR<mat> qr(in);
	    in /= scale;

	    qr.compute(in);
	    U = qr.matrixQ().block(0, 0, in.rows(), in.cols());
	    mat upper = qr.matrixQR().triangularView<Upper>();// .block(0, 0, in.cols(), in.cols());
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) {
		    D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }

	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* qr.colsPermutation().transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_full_sort(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    mat scaled_in = mat::Zero(in.rows(), in.cols());
	    perm_mat row_perm = dqmc::la::row_sort(in, scaled_in);
	    perm_mat col_perm = dqmc::la::col_sort(scaled_in, in);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.cols());
	    ivec iwork(in.cols());

	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();

	    scaled_in.block(0, 0, in.rows(), in.cols()) = in/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    work.setZero();
	    taus.setZero();

	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    mat upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // if (work(0) > work.size()) {
	    // 	cout << "Need more lwork for dorgqr" << endl;
	    // }
	    lwork = work(0);
	    work.resize(lwork);
	    // cout << "in" << endl << scaled_in << endl << endl;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // cout << "out" << endl << scaled_in << endl << endl;
	    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);
	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose() * col_perm.transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_row_sort(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.cols());
	    ivec iwork(in.cols());

	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat scaled_in = mat::Zero(in.rows(), in.cols());

	    scaled_in.block(0, 0, in.rows(), in.cols()) = (row_perm * in)/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    work.setZero();
	    taus.setZero();

	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    mat upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // if (work(0) > work.size()) {
	    // 	cout << "Need more lwork for dorgqr" << endl;
	    // }
	    lwork = work(0);
	    work.resize(lwork);
	    // cout << "in" << endl << scaled_in << endl << endl;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // cout << "out" << endl << scaled_in << endl << endl;
	    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* col_perm.transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_row_sort_sqr_q(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.3);

	    vec taus(in.rows());
	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    ivec iwork(in.rows());
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat scaled_in = mat::Zero(in.rows(), in.rows());

	    scaled_in.block(0, 0, in.rows(), in.cols()) = (row_perm * in)/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    {
		{
		    mat upper = scaled_in.triangularView<Upper>();
		    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
		    vec d_inv = D;

		    for (int i = 0; i < in.cols(); ++i) {
			if (D(i) == 0.) {
			    std::cout << "culprit " << D(i) << " was "
				      << upper(i, i) << std::endl;
			    throw std::runtime_error("Invalid upper triangle"); }
			d_inv(i) = 1./D(i);
			if (D(i) < 0) { D(i) *= -1.;
			    d_inv(i) *= -1.; }
		    }
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(),
			    work.data(), &lwork, &info);
		    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
		    ivec col_ids(in.cols());
		    for (int i = 0; i < in.cols(); ++i) {
			col_ids(i) = iwork(i) - 1;
		    }

		    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
		    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
			* col_perm.transpose();
		    D *= scale;
		    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
		}
	    }
	}

#ifdef USE_DD
	inline void decompose_udt_row_sort_dd(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.rows());
	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    ivec iwork(in.rows());
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat temp = mat::Zero(in.rows(), in.cols());
	    dd_mat dd_temp = dd_mat::Zero(in.cols(), in.cols());
	    dd_mat scaled_in = dd_mat::Zero(in.rows(), in.cols());
	    temp = ((row_perm * in)/scale);
	    scaled_in.block(0, 0, in.rows(), in.cols()) = temp.cast<ddouble>();
	    Eigen::FullPivHouseholderQR<dd_mat> qr(scaled_in);
	    // cout << scaled_in << endl << endl;
	    qr.compute(scaled_in);
	    dd_temp = qr.matrixQR().block(0, 0, in.cols(), in.cols()).triangularView<Upper>();
	    mat upper = mat::Zero(in.cols(), in.cols());
	    // cout << "from dd_temp" << endl << endl << upper << endl << endl;
	    // cout << dd_temp.rows() << " x " << dd_temp.cols() << " - " <<
	    // 	upper.rows() << " x " << upper.cols() << endl;
	    dqmc::la::copy_from_dd(dd_temp, upper);
	    // cout << "from dd_temp" << endl << endl << upper << endl << endl;
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    //dd_mat qrQ  = qr.householderQ();
	    dd_mat qrQ  = qr.matrixQ();
	    dd_temp = qrQ.block(0, 0, in.rows(), in.cols());

	    dqmc::la::copy_from_dd(dd_temp, temp);
	    // temp = dd_temp.cast<double>();
	    U = row_perm.inverse() * temp;

	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* qr.colsPermutation().transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}
#endif

	inline void decompose_udt_row_sort(mat &__restrict__ in, mat &__restrict__ U, vec &__restrict__ D, mat &__restrict__ T,
					   mat &__restrict__ scaled_in, mat &__restrict__ T_temp,
					   ivec &__restrict__ row_ids, ivec &__restrict__ col_ids,
					   ivec &__restrict__ max_index, vec &__restrict__ max_vals, std::vector<max_pair>& max_pairs,
					   vec &__restrict__ taus, vec &__restrict__ work_old, ivec &__restrict__ iwork_old ) {
	    using namespace std;
	    using namespace Eigen;

	    max_pairs.clear();
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index(i) );
		max_pairs.push_back(max_pair(max_vals(i), i));
		// cout << i << endl;
	    }

	    // for (std::vector<max_pair>::iterator it=max_pairs.begin(); it != max_pairs.end(); ++it) {
	    // 	row_ids((*it).second) = std::distance(max_pairs.begin(), it);
	    // 	cout << (*it).second << " | " << std::distance(max_pairs.begin(), it) << "   ";
	    // }
	    // cout << endl;

	    std::sort(max_pairs.begin(), max_pairs.end(), comparator);
	    // cout << "Sorted" << endl;
	    // cout << row_ids.size();
	    for (std::vector<max_pair>::iterator it=max_pairs.begin(); it != max_pairs.end(); ++it) {
		row_ids((*it).second) = std::distance(max_pairs.begin(), it);
		// cout << (*it).second << " | " << std::distance(max_pairs.begin(), it) << "   ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);
	    // cout << "permutation" << endl;

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.3);
	    int lwork; // = work.size();
	    // vec work(in.rows() * in.rows() * in.rows());
	    // int lwork = work.size();

	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    ivec iwork(in.cols());

	    // cout << "Scaled" << endl;
	    // cout << row_perm.indices().transpose() << endl << endl;
	    // cout << row_perm.toDenseMatrix() << endl << endl;
	    // cout << in << endl << endl;
	    {
		{
		    scaled_in = (row_perm * in)/scale;
		    // cout << scaled_in << endl << endl;
		    iwork.setZero();
		    // cout << "QRing" << endl;
		    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
		    // ivec iwork(4*in.rows());
		    lwork = -1;
		    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
			    taus.data(), taus.data(),
			    &lwork, &info);
		    lwork = taus(0);
		    vec work(lwork);

		    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
			    taus.data(), work.data(),
			    &lwork, &info);
		}
	    }
	    {
		{

		    // cout << "To zero!" << endl;
		    // cout << "Getting diagonal" << endl;
		    D = scaled_in.diagonal();
		    vec d_inv = D;
		    // cout << "Gitgotthat" << endl;

		    for (int i = 0; i < in.cols(); ++i) {
			if (D(i) == 0.) {
			    std::cout << "culprit " << D(i) << std::endl;
			    throw std::runtime_error("Invalid upper triangle"); }

			d_inv(i) = 1./D(i);
			if (D(i) < 0) { D(i) *= -1.;
			    d_inv(i) *= -1.; }
		    }

		    for (int i = 0; i < in.cols(); ++i) col_ids(i) = iwork(i) - 1;
		    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
		    T_temp = scaled_in.triangularView<Upper>();
		    T = (d_inv.asDiagonal() * T_temp) * col_perm.transpose();
		    // cout << T << endl << endl;

		    D *= scale;
		    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
		    lwork = -1;
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(), taus.data(), &lwork, &info);
		    lwork = taus(0);
		    vec work(lwork);
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(), work.data(), &lwork, &info);
		    U = row_perm.inverse() * scaled_in;
		}
	    }
	    return;
	}

	inline void decompose_udt_col_piv(mat&__restrict__ in,
					  mat&__restrict__ U,
					  vec&__restrict__ D,
					  mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;
	    if (U.rows() != in.rows() || U.cols() != in.cols() || U.cols() == 0) {
		cout << U.rows() << " " << U.cols() << endl;
		throw std::runtime_error("dimensions of U are wrong");
	    }

	    if (D.size() != in.cols() || D.size() == 0) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || T.cols() == 0)
		throw std::runtime_error("dimensions of T are wrong");

	    mat_t scaled_in = mat_t::Zero(in.rows(), in.cols());

	    vec_t taus(in.cols());
	    ivec iwork(in.cols());

	    int lwork = -1;
	    int M = in.rows();
	    int N = in.cols();

	    perm_mat row_sort_perm = row_sort(in, scaled_in);
	    // scaled_in.block(0, 0, in.rows(), in.cols()) = in;
	    iwork.setZero();
	    lapack_int info = LAPACKE_dgeqp3( LAPACK_COL_MAJOR, M, N, scaled_in.data(),
					      M, iwork.data(), taus.data() );

	    mat_t upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    // std::cout << "culprit " << D(i) << " was "
		    // 	      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;

	    info = LAPACKE_dorgqr( LAPACK_COL_MAJOR, M, N, N,
				   scaled_in.data(), M, taus.data() );

	    U = (row_sort_perm.inverse() * scaled_in).block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
		if (col_ids(i) == -1) {
		    dqmc::tools::abort("Permutation fails");
		}
	    }
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);

	    T = (d_inv.asDiagonal()
		 * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose();
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

    }
}
#endif

max_stars_repo_name: pebroecker/DQMC

max_issues_repo_name: pebroecker/DQMC

max_forks_repo_name: pebroecker/DQMC


text:
// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

#include <string>
#include <sys/time.h>
#include <sys/resource.h>
#include <boost/tuple/tuple.hpp>

#include <tclap/CmdLine.h>
#include "tclap-wrappers.h"
#include <nuklei/CoViS3DObservationIO.h>
#include <nuklei/SerializedKernelObservationIO.h>
#include <nuklei/Serial.h>
#include <nuklei/nullable.h>

using namespace nuklei;

int info(int argc, char ** argv)
{
  NUKLEI_TRACE_BEGIN();

  /* Parse command line arguments */

  TCLAP::CmdLine cmd(INFOSTRING + "Info App." );

  /* Standard arguments */

  TCLAP::ValueArg<int> niceArg
  ("", "nice",
   "Proccess priority.",
   false, NICEINC, "int", cmd);

  /* Custom arguments */

  TCLAP::UnlabeledMultiArg<std::string> fileListArg
  ("file",
   "Nuklei-readable files.",
   true, "filename", cmd);

  TCLAP::SwitchArg nArg
  ("n", "n_points",
   "Print number of kernels/points/etc.", cmd);

  TCLAP::SwitchArg momentsArg
  ("m", "moments",
   "Print moments.", cmd);

  cmd.parse( argc, argv );

  NUKLEI_ASSERT(setpriority(PRIO_PROCESS, 0, niceArg.getValue()) == 0);

  for (std::vector<std::string>::const_iterator file = fileListArg.getValue().begin();
       file != fileListArg.getValue().end(); file++)
  {

    if (!nArg.getValue() && !momentsArg.getValue())
      std::cout << "[" << *file << "]\n";

    KernelCollection kc;
    Observation::Type type;

    readObservations(*file, kc, type);

    if (nArg.getValue())
    {
      std::cout << kc.size() << std::endl;
      continue;
    }

    if (momentsArg.getValue())
    {
      kernel::base::ptr m = kc.moments();
      std::cout << stringify(m->getLoc()) << " " << m->getLocH() << std::endl;
      continue;
    }

    std::cout << "reader: " << nameFromType<Observation>(type) << std::endl;

    std::cout << "number_of_points: " << kc.size() << std::endl;
    if (kc.size() > 0)
    {
      coord_t mean_loc_h = 0;
      coord_t mean_ori_h = 0;
      weight_t mean_weight = 0;
      coord_t mdist = 0;
      for (KernelCollection::const_iterator i = kc.begin(); i != kc.end(); ++i)
      {
        for (KernelCollection::const_iterator j = i; j != kc.end(); ++j)
        {
          mdist = std::max(mdist, (i->getLoc() - j->getLoc()).Length());
        }
        mean_loc_h += i->getLocH();
        mean_ori_h += i->getOriH();
        mean_weight += i->getWeight();
      }
      mean_loc_h /= kc.size();
      mean_ori_h /= kc.size();
      mean_weight /= kc.size();

      coord_t stdev_loc_h = 0;
      coord_t stdev_ori_h = 0;
      weight_t stdev_weight = 0;
      for (KernelCollection::const_iterator i = kc.begin(); i != kc.end(); ++i)
      {
        stdev_loc_h += std::pow(mean_loc_h - i->getLocH(), 2);
        stdev_ori_h += std::pow(mean_ori_h - i->getOriH(), 2);
        stdev_weight += std::pow(mean_weight - i->getWeight(), 2);
      }
      stdev_loc_h = std::sqrt(stdev_loc_h/kc.size());
      stdev_ori_h = std::sqrt(stdev_ori_h/kc.size());
      stdev_weight = std::sqrt(stdev_weight/kc.size());

      kernel::base::ptr m = kc.moments();
      std::cout << "mean_position: " << stringify(m->getLoc()) << std::endl;
      std::cout << "deviation_around_mean_position: " << m->getLocH() << std::endl;
      std::cout << "radius_of_bounding sphere: " << mdist/2 << std::endl;

      std::cout << "mean_weight: " << mean_weight << " +- " << stdev_weight << std::endl;
      std::cout << "mean_loc_h: " << mean_loc_h << " +- " << stdev_loc_h << std::endl;
      std::cout << "mean_ori_h: " << mean_ori_h << " +- " << stdev_ori_h << std::endl;

      if (file+1 != fileListArg.getValue().end())
        std::cout << std::endl;
    }
  }

  return 0;

  NUKLEI_TRACE_END();
}

ext: cpp

max_stars_repo_name: chungying/nuklei

max_issues_repo_name: chungying/nuklei

max_forks_repo_name: chungying/nuklei


text:
#ifndef _HF_THREAD_HPP_
#define _HF_THREAD_HPP_

#include <vector>
#include <boost/array.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread.hpp>

#include <boost/typeof/typeof.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

// typedef boost::posix_time::ptime ptime;
// typedef boost::posix_time::time_duration time_duration;
// // typedef boost::posix_time::microsec_clock microsec_clock;

#include "hf/hf.hpp"

#include <rysq.hpp>
#include "adapter/rysq.hpp"

namespace hf {

    struct timer {
	boost::posix_time::ptime start_;
	typedef boost::posix_time::microsec_clock microsec_clock;
	timer() : start_(microsec_clock::universal_time()) {}
	boost::posix_time::time_duration duration() const {
	    return microsec_clock::universal_time() - start_;
	}
	long total_seconds() const {
	    return duration().total_seconds();
	}
    };
    std::ostream& operator << (std::ostream &os, const timer &t) {
	return os << boost::posix_time::to_simple_string(t.duration());
    }

    template<typename T>
    class lockable {
    public:
	typedef T& reference;
	typedef const T& const_reference;
	explicit lockable(T &data) : data_(data) {}
	void lock() const { mutex_.lock(); }
	void unlock() const { mutex_.unlock(); }
	reference data(){return data_; }
	const_reference data()const {return data_; }
    private:
	mutable boost:: mutex mutex_;
	T &data_;
    };

    template<typename T>
    lockable<T> make_lockable(T &data) {
	return lockable <T>(data);
    }

    class fock_thread {
    public:

	template<class B, class C, class S, class D, class F, class Q>
	static boost::thread* create(const B &blocks, const C &centers,
				     const S &screen,
				     const D &density, lockable<F> &fock,
				     Q &queue) {
	    using boost::ref;
	    return new
		boost::thread(fock_thread::run<B, C, S, D, F, Q>,
			      blocks, ref(centers), ref(screen),
			      ref(density), ref(fock), ref(queue));
	}

	boost::thread thread_;
	template<class B, class C, class S, class D, class F, class Q>
	fock_thread(const B &blocks, const C &centers, const S &screen,
		    const D &density, lockable<F> &fock, Q &queue)
	    : thread_(fock_thread::run<B, C, S, D, F, Q>,
		      blocks, boost::ref(centers), boost::ref(screen),
		      boost::ref(density), boost::ref(fock), boost::ref(queue))
	{
	}
	void join() { thread_.join(); }

	template<class B, class C, class S, class D, class F, class Q>
	static void run(const B blocks, const C &centers, const S &screen,
			const D &density, lockable<F> &fock, Q &queue) {

	    timer t;

	    std::vector<boost::array<int,4> > quartets;
	    typename Q::value_type task;

	    while (true) {
	    	try { task = queue.backlog().pop(); }
	    	catch(...) {
	    	    try { task = queue.pop(); }
	    	    catch(...) { break; }
	    	}
	    	run_task(blocks, centers, screen, density, fock, task, quartets);
	    }
	    while(true) {
	    	try { task = queue.backlog().pop(); }
	    	catch(...) { break; }
	    	run_task(blocks, centers, screen, density, fock, task, quartets);
	    }

	    std::cout << "CPU thread time: " << t << std::endl;
	}
	template<class B, class C, class S, class D, class F, class T, class Q>
	static void run_task(const B blocks, const C &centers, const S &screen,
			     const D &density, lockable<F> &fock,
			     const T &task, Q &quartets) {

	    BOOST_AUTO(ib, &blocks[task[0]]);
	    BOOST_AUTO(kb, &blocks[task[1]]);
	    BOOST_AUTO(jb, &blocks[task[2]]);
	    BOOST_AUTO(lb, &blocks[task[3]]);

	    const BOOST_TYPEOF(ib) block[] = { ib, jb, kb, lb };

	    quartets.clear();
	    screen(block, quartets);
	    if (quartets.size() == 0) return;

	    typedef adapter::rysq::Shell Shell;
	    typedef rysq::Quartet<rysq::Shell> Quartet;
	    Shell a(block[0]->shell());
	    Shell b(block[1]->shell());
	    Shell c(block[2]->shell());
	    Shell d(block[3]->shell());
	    Quartet quartet(a,b,c,d);

	    // if (quartet.L()/2 + 1 < 3) return;
	    // std::cout << quartet << std::endl;

	    int base[] = { ib->firstShell(), jb->firstShell(),
			   kb->firstShell(), lb->firstShell() };

	    rysq::hf::matrix_set<adapter::rysq::density_matrix> D_(base);
	    rysq::hf::matrix_set<adapter::rysq::fock_matrix> F_(base);

	    struct {
		typename F::matrix matrix;
		int index[2];
	    } fock_tls[6];

	    int k = 0;
	    foreach (const size_t (&index)[2],  rysq::index_list()) {
	        size_t i = index[0], j = index[1];
	        size_t im = std::distance(&blocks[0], block[i]);
	        size_t jm = std::distance(&blocks[0], block[j]);

	        D_.get(i,j) = adapter::rysq::density_matrix(density.m(im,jm));
	        D_.get(j,i) = adapter::rysq::density_matrix(density.m(jm,im));

		{
		    typename F::matrix &f = fock.data().m(im,jm);
		    fock_tls[k].matrix.resize(f.size1(), f.size2(),
					      f.block1(), f.block2());
		    fock_tls[k].matrix.clear();
		    fock_tls[k].index[0] = im;
		    fock_tls[k].index[1] = jm;
		    F_.get(i,j) = adapter::rysq::fock_matrix(fock_tls[k].matrix);
		    ++k;
		}
	    }

	    rysq::Parameters parameters(0, -1.0, 4.0, 1e-12);
	    (rysq::Fock(quartet))(centers, quartets, D_, F_, parameters);

	    fock.lock();
	    for (int k = 0; k < 6; ++k) {
		int i = fock_tls[k].index[0], j = fock_tls[k].index[1];
		fock.data().m(i,j) += fock_tls[k].matrix;
	    }
	    fock.unlock();

	}
    };

    struct fock_thread_group {
	static size_t max_threads() {
	    return std::max<size_t>(boost::thread::hardware_concurrency(), 1);
	}
	boost::thread_group group_;
	template<class B, class C, class S, class D, class F, class Q>
	fock_thread_group(const B &blocks, const C &centers, const S &screen,
			  const D &density, lockable<F> &fock, Q &queue,
			  size_t threads = max_threads()) {
	    for (size_t i = 0; i < threads; ++i) {
		group_.add_thread(fock_thread::create(blocks, centers, screen,
						      density, fock, queue));
	    }
	}
	void join_all() { group_.join_all(); }
    };

}

#endif /* _HF_THREAD_HPP_ */


text:
//
// http.hpp
// ~~~~~~~~
//
// Copyright (c) 2009-2013 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef URDL_HTTP_HPP
#define URDL_HTTP_HPP

#include <string>
#include <boost/system/error_code.hpp>
#include "urdl/detail/config.hpp"

#include "urdl/detail/abi_prefix.hpp"

namespace urdl {
namespace http {

/// Gets the error category for HTTP errors.
/**
 * @returns The @c boost::system::error_category used for HTTP errors.
 *
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
URDL_DECL const boost::system::error_category& error_category();

/// Option to specify the HTTP request method.
/**
 * @par Remarks
 * The default request method is "GET".
 *
 * @par Example
 * To set the request method for an object of class @c urdl::istream:
 * @code
 * urdl::istream is;
 * is.set_option(urdl::http::request_method("HEAD"));
 * is.open("http://www.boost.org");
 * @endcode
 *
 * To set the request method for an object of class @c urdl::read_stream:
 * @code
 * urdl::read_stream stream;
 * stream.set_option(urdl::http::request_method("HEAD"));
 * stream.open("http://www.boost.org");
 * @endcode
 *
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
class request_method
{
public:
  /// Constructs an object of class @c request_method.
  /**
   * @par Remarks
   * Postcondition: <tt>value() == "GET"</tt>.
   */
  request_method()
    : value_("GET")
  {
  }

  /// Constructs an object of class @c request_method.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  explicit request_method(const std::string& v)
    : value_(v)
  {
  }

  /// Gets the value of the option.
  /**
   * @returns The value of the option.
   */
  std::string value() const
  {
    return value_;
  }

  /// Sets the value of the option.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  void value(const std::string& v)
  {
    value_ = v;
  }

private:
  std::string value_;
};

/// Option to specify content to accompany an HTTP request.
/**
 * @par Remarks
 * The default is for no content to be sent.
 *
 * @par Example
 * To add content to the HTTP request using an object of class @c urdl::istream:
 * @code
 * urdl::istream is;
 * is.set_option(urdl::http::request_method("POST"));
 * is.set_option(urdl::http::request_content("Hello, world!"));
 * is.set_option(urdl::http::request_content_type("text/plain"));
 * is.open("http://host/path");
 * @endcode
 *
 * To add content to the HTTP request using an object of class
 * @c urdl::read_stream:
 * @code
 * urdl::read_stream stream;
 * stream.set_option(urdl::http::request_method("POST"));
 * stream.set_option(urdl::http::request_content("Hello, world!"));
 * stream.set_option(urdl::http::request_content_type("text/plain"));
 * stream.open("http://host/path");
 * @endcode
 *
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
class request_content
{
public:
  /// Constructs an object of class @c request_content.
  /**
   * @par Remarks
   * Postcondition: <tt>value() == ""</tt>.
   */
  request_content()
    : value_("")
  {
  }

  /// Constructs an object of class @c request_content.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  explicit request_content(const std::string& v)
    : value_(v)
  {
  }

  /// Gets the value of the option.
  /**
   * @returns The value of the option.
   */
  std::string value() const
  {
    return value_;
  }

  /// Sets the value of the option.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  void value(const std::string& v)
  {
    value_ = v;
  }

private:
  std::string value_;
};

/// Option to specify the type of the content that accompanies an HTTP request.
/**
 * @par Remarks
 * The default is for no content type to be specified in the request.
 *
 * @par Example
 * To add content to the HTTP request using an object of class @c urdl::istream:
 * @code
 * urdl::istream is;
 * is.set_option(urdl::http::request_method("POST"));
 * is.set_option(urdl::http::request_content("Hello, world!"));
 * is.set_option(urdl::http::request_content_type("text/plain"));
 * is.open("http://host/path");
 * @endcode
 *
 * To add content to the HTTP request using an object of class
 * @c urdl::read_stream:
 * @code
 * urdl::read_stream stream;
 * stream.set_option(urdl::http::request_method("POST"));
 * stream.set_option(urdl::http::request_content("Hello, world!"));
 * stream.set_option(urdl::http::request_content_type("text/plain"));
 * stream.open("http://host/path");
 * @endcode
 *
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
class request_content_type
{
public:
  /// Constructs an object of class @c request_content_type.
  /**
   * @par Remarks
   * Postcondition: <tt>value() == ""</tt>.
   */
  request_content_type()
    : value_("")
  {
  }

  /// Constructs an object of class @c request_content_type.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  explicit request_content_type(const std::string& v)
    : value_(v)
  {
  }

  /// Gets the value of the option.
  /**
   * @returns The value of the option.
   */
  std::string value() const
  {
    return value_;
  }

  /// Sets the value of the option.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  void value(const std::string& v)
  {
    value_ = v;
  }

private:
  std::string value_;
};

/// Option to specify the maximum number of allowed HTTP redirects.
/**
 * @par Remarks
 * The default value is for there to be no limit on the number of allowed
 * redirects. Set the option to 0 to disable HTTP redirects.
 *
 * @par Example
 * To set maximum number of redirects for an object of class @c urdl::istream:
 * @code
 * urdl::istream is;
 * is.set_option(urdl::http::max_redirects(1));
 * is.open("http://www.boost.org");
 * @endcode
 *
 * To set maximum number of redirects for an object of class
 * @c urdl::read_stream:
 * @code
 * urdl::read_stream stream;
 * stream.set_option(urdl::http::max_redirects(1));
 * stream.open("http://www.boost.org");
 * @endcode
 *
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
class max_redirects
{
public:
  /// Constructs an object of class @c max_redirects.
  /**
   * @par Remarks
   * Postcondition: <tt>value() == std::numeric_limits<std::size_t>::max()</tt>.
   */
  max_redirects()
    : value_(~std::size_t(0))
  {
  }

  /// Constructs an object of class @c max_redirects.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  explicit max_redirects(std::size_t v)
    : value_(v)
  {
  }

  /// Gets the value of the option.
  /**
   * @returns The value of the option.
   */
  std::size_t value() const
  {
    return value_;
  }

  /// Sets the value of the option.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  void value(std::size_t v)
  {
    value_ = v;
  }

private:
  std::size_t value_;
};

/// Option to specify the user agent identifier.
/**
 * @par Remarks
 * The default is to not specify the user agent.
 *
 * @par Example
 * To set the user agent for an object of class @c urdl::istream:
 * @code
 * urdl::istream is;
 * is.set_option(urdl::http::user_agent("Urdl"));
 * is.open("http://www.boost.org");
 * @endcode
 *
 * To set the user agent for an object of class @c urdl::read_stream:
 * @code
 * urdl::read_stream stream;
 * stream.set_option(urdl::http::user_agent("Urdl"));
 * stream.open("http://www.boost.org");
 * @endcode
 *
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
class user_agent
{
public:
  /// Constructs an object of class @c user_agent.
  /**
   * @par Remarks
   * Postcondition: <tt>value() == ""</tt>.
   */
  user_agent()
    : value_("")
  {
  }

  /// Constructs an object of class @c user_agent.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  explicit user_agent(const std::string& v)
    : value_(v)
  {
  }

  /// Gets the value of the option.
  /**
   * @returns The value of the option.
   */
  std::string value() const
  {
    return value_;
  }

  /// Sets the value of the option.
  /**
   * @param v The desired value for the option.
   *
   * @par Remarks
   * Postcondition: <tt>value() == v</tt>
   */
  void value(const std::string& v)
  {
    value_ = v;
  }

private:
  std::string value_;
};

namespace errc {

/// HTTP error codes.
/**
 * The enumerators of type @c errc_t are implicitly convertible to objects of
 * type @c boost::system::error_code.
 *
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
enum errc_t
{
  // Client-generated errors.

  /// The response's status line was malformed.
  malformed_status_line = 1,

  /// The response's headers were malformed.
  malformed_response_headers = 2,

  // Server-generated status codes.

  /// The server-generated status code "100 Continue".
  continue_request = 100,

  /// The server-generated status code "101 Switching Protocols".
  switching_protocols = 101,

  /// The server-generated status code "200 OK".
  ok = 200,

  /// The server-generated status code "201 Created".
  created = 201,

  /// The server-generated status code "202 Accepted".
  accepted = 202,

  /// The server-generated status code "203 Non-Authoritative Information".
  non_authoritative_information = 203,

  /// The server-generated status code "204 No Content".
  no_content = 204,

  /// The server-generated status code "205 Reset Content".
  reset_content = 205,

  /// The server-generated status code "206 Partial Content".
  partial_content = 206,

  /// The server-generated status code "300 Multiple Choices".
  multiple_choices = 300,

  /// The server-generated status code "301 Moved Permanently".
  moved_permanently = 301,

  /// The server-generated status code "302 Found".
  found = 302,

  /// The server-generated status code "303 See Other".
  see_other = 303,

  /// The server-generated status code "304 Not Modified".
  not_modified = 304,

  /// The server-generated status code "305 Use Proxy".
  use_proxy = 305,

  /// The server-generated status code "307 Temporary Redirect".
  temporary_redirect = 307,

  /// The server-generated status code "400 Bad Request".
  bad_request = 400,

  /// The server-generated status code "401 Unauthorized".
  unauthorized = 401,

  /// The server-generated status code "402 Payment Required".
  payment_required = 402,

  /// The server-generated status code "403 Forbidden".
  forbidden = 403,

  /// The server-generated status code "404 Not Found".
  not_found = 404,

  /// The server-generated status code "405 Method Not Allowed".
  method_not_allowed = 405,

  /// The server-generated status code "406 Not Acceptable".
  not_acceptable = 406,

  /// The server-generated status code "407 Proxy Authentication Required".
  proxy_authentication_required = 407,

  /// The server-generated status code "408 Request Time-out".
  request_timeout = 408,

  /// The server-generated status code "409 Conflict".
  conflict = 409,

  /// The server-generated status code "410 Gone".
  gone = 410,

  /// The server-generated status code "411 Length Required".
  length_required = 411,

  /// The server-generated status code "412 Precondition Failed".
  precondition_failed = 412,

  /// The server-generated status code "413 Request Entity Too Large".
  request_entity_too_large = 413,

  /// The server-generated status code "414 Request URI Too Large".
  request_uri_too_large = 414,

  /// The server-generated status code "415 Unsupported Media Type".
  unsupported_media_type = 415,

  /// The server-generated status code "416 Requested Range Not Satisfiable".
  requested_range_not_satisfiable = 416,

  /// The server-generated status code "417 Expectation Failed".
  expectation_failed = 417,

  /// The server-generated status code "500 Internal Server Error".
  internal_server_error = 500,

  /// The server-generated status code "501 Not Implemented".
  not_implemented = 501,

  /// The server-generated status code "502 Bad Gateway".
  bad_gateway = 502,

  /// The server-generated status code "503 Service Unavailable".
  service_unavailable = 503,

  /// The server-generated status code "504 Gateway Timeout".
  gateway_timeout = 504,

  /// The server-generated status code "505 HTTP Version Not Supported".
  version_not_supported = 505
};

/// Converts a value of type @c errc_t to a corresponding object of type
/// @c boost::system::error_code.
/**
 * @par Requirements
 * @e Header: @c <urdl/http.hpp> @n
 * @e Namespace: @c urdl::http
 */
inline boost::system::error_code make_error_code(errc_t e)
{
  return boost::system::error_code(
      static_cast<int>(e), http::error_category());
}

} // namespace errc
} // namespace http
} // namespace urdl

namespace boost {
namespace system {

template <>
struct is_error_code_enum<urdl::http::errc::errc_t>
{
  static const bool value = true;
};

} // namespace system
} // namespace boost

#include "urdl/detail/abi_suffix.hpp"

#if defined(URDL_HEADER_ONLY)
# include "urdl/impl/http.ipp"
#endif

#endif // URDL_HTTP_HPP

max_stars_repo_name: marstr/winget-cli

max_issues_repo_name: marstr/winget-cli

max_forks_repo_name: marstr/winget-cli


text:
// Receives wheel speed data from vesc and yaw data from IMU
// Publishes odom and tf if requested

// Param predict_slip when set predicts wheel slip based on accel/brake
// assumes linear relation between accel and slip ratio
// requires params slip_gain and slip_offset

// x covariance increase with speed
// y covariance increase with ay
// yaw covariance increase with yaw rate

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <vesc_msgs/VescStateStamped.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <cmath>
#include <tf/transform_datatypes.h>
#include <boost/shared_ptr.hpp>

class FusedOdomPublisher
{
    public:
        FusedOdomPublisher(ros::NodeHandle nh, ros::NodeHandle pnh);

    private:
        double x_, y_, yaw_;
        double slip_tix_gain_;
        ros::Time last_imu_time_;

        // ROS params
        double speed_gain_, speed_offset_;
        double slip_gain_speed_, slip_offset_speed_;
        double slip_gain_accel_, slip_offset_accel_;
        double slip_gain_brake_, slip_offset_brake_;
        double geometric_tix_gain_;
        bool publish_tf_;
        bool predict_slip_;

        // ROS messages
        sensor_msgs::Imu::ConstPtr last_imu_;
        vesc_msgs::VescStateStamped::ConstPtr last_state_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped tf_;

        // ROS services
        ros::Subscriber vesc_state_sub_;
        ros::Subscriber imu_sub_;
        ros::Publisher odom_pub_;
        boost::shared_ptr<tf::TransformBroadcaster> tf_pub_;

        // ROS callbacks
        void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state);
        void imuCallback(const sensor_msgs::Imu::ConstPtr& servo);
};

template <typename T>
inline bool getRequiredParam(const ros::NodeHandle& n, std::string name, T& value);

FusedOdomPublisher::FusedOdomPublisher(ros::NodeHandle nh, ros::NodeHandle pnh) :
    publish_tf_(true), predict_slip_(false), x_(0.0), y_(0.0), yaw_(0.0)
{
    if (!getRequiredParam(nh, "vesc_interface/speed_gain", speed_gain_))
        return;
    if (!getRequiredParam(nh, "vesc_interface/speed_offset", speed_offset_))
        return;
    if (!getRequiredParam(nh, "vesc_interface/tix_gain", geometric_tix_gain_))
        return;
    pnh.param("publish_tf", publish_tf_, publish_tf_);
    pnh.param("predict_slip", predict_slip_, predict_slip_);
    if (predict_slip_)
    {
        if (!getRequiredParam(pnh, "slip_gain_accel", slip_gain_accel_))
            return;
        if (!getRequiredParam(pnh, "slip_offset_accel", slip_offset_accel_))
            return;
        if (!getRequiredParam(pnh, "slip_gain_brake", slip_gain_brake_))
            return;
        if (!getRequiredParam(pnh, "slip_offset_brake", slip_offset_brake_))
            return;
        if (!getRequiredParam(pnh, "slip_gain_speed", slip_gain_speed_))
            return;
        if (!getRequiredParam(pnh, "slip_offset_speed", slip_offset_speed_))
            return;
    }
    else
        slip_tix_gain_ = geometric_tix_gain_;

    // create odom publisher
    odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom_fused", 10);

    // create tf broadcaster
    if (publish_tf_) {
        tf_pub_.reset(new tf::TransformBroadcaster);
    }

    // subscribe to vesc state and servo command
    vesc_state_sub_ = nh.subscribe("sensors/core", 10, &FusedOdomPublisher::vescStateCallback, this);
    imu_sub_ = nh.subscribe("imu", 10, &FusedOdomPublisher::imuCallback, this);

    // set static values in odom_
    odom_.header.frame_id = "odom";
    odom_.child_frame_id = "base_link";
    odom_.pose.pose.position.z = 0.0;
    odom_.pose.pose.orientation.x = 0.0;
    odom_.pose.pose.orientation.y = 0.0;
    odom_.twist.twist.linear.y = 0.0;

    // set static values in tf_
    if (publish_tf_)
    {
        tf_.header.frame_id = "odom";
        tf_.child_frame_id = "base_link";
        tf_.transform.translation.z = 0.0;
    }

    last_imu_time_ = ros::Time::now();
}

void FusedOdomPublisher::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state)
{

    // use current state as last state if this is our first time here
    if (!last_state_)
        last_state_ = state;

    // wait for first imu msg
    if (!last_imu_)
        return;

    // check for imu timeout
    if ( (ros::Time::now()-last_imu_time_).toSec() > 1.0 ) {
        ROS_ERROR("IMU sensor timed out. Not publishing fused_odom.");
        return;
    }

    // get vesc state
    double wheel_speed = ( state->state.speed - speed_offset_ ) / speed_gain_;
    double current_disp = state->state.displacement;
    static double last_disp = current_disp;

    // calculate accel dependent tix gain
    double slip_ratio;
    if (predict_slip_) {
        double accel = last_imu_->linear_acceleration.x;
        accel *= (wheel_speed>0 ? 1 : -1);                       // account for when car moving backwards
        if (accel > 0)                                           // accelerating
            slip_ratio = slip_gain_accel_ * fmax(0,accel-slip_offset_accel_) + slip_gain_speed_ * fmax(0,wheel_speed-slip_offset_speed_);
        else                                                     // braking
            slip_ratio = slip_gain_brake_ * fmax(0,-1.0*accel-slip_offset_brake_) + slip_gain_speed_ * fmax(0,wheel_speed-slip_offset_speed_);
        slip_tix_gain_ = geometric_tix_gain_ * (1 + slip_ratio);
    }

    // calc elapsed time
    ros::Duration dt = state->header.stamp - last_state_->header.stamp;

    // propigate odometry
    double ds = (current_disp - last_disp) / slip_tix_gain_;
    yaw_ = tf::getYaw(last_imu_->orientation);
    x_ += ds * cos(yaw_);
    y_ += ds * sin(yaw_);

    // calculate covariance
    odom_.pose.covariance[0] = 0.1 + 5.0 * fabs(wheel_speed);
    odom_.twist.covariance[0] = 0.1 + 5.0 * fabs(wheel_speed);
    odom_.pose.covariance[6*1+1] = 0.5 + 0.5 * fmax(0,fabs(last_imu_->linear_acceleration.y)-0.2);
    odom_.twist.covariance[6*1+1] = 0.5 + 0.5 * fmax(0,fabs(last_imu_->linear_acceleration.y)-0.2);
    odom_.pose.covariance[6*5+5] = 0.1 * 0.05 * fmax(0,fabs(last_imu_->angular_velocity.z)-0.1);
    odom_.twist.covariance[6*5+5] = 0.1 * 0.1 * fmax(0,fabs(last_imu_->angular_velocity.z)-0.1);

    // save state for next time
    last_state_ = state;
    last_disp = current_disp;

    // publish odometry message
    odom_.header.stamp = state->header.stamp;
    odom_.pose.pose.position.x = x_;
    odom_.pose.pose.position.y = y_;
    odom_.pose.pose.orientation.z = sin(yaw_/2.0);
    odom_.pose.pose.orientation.w = cos(yaw_/2.0);

    // Velocity (in the coordinate frame given by the child_frame_id) accounting for wheel slip
    if (predict_slip_) {
        odom_.twist.twist.linear.x = wheel_speed / (1 + slip_ratio);
    } else odom_.twist.twist.linear.x = wheel_speed;
    odom_.twist.twist.angular.z = last_imu_->angular_velocity.z;

    if (publish_tf_) {
        tf_.header.stamp = ros::Time::now();
        tf_.transform.translation.x = x_;
        tf_.transform.translation.y = y_;
        tf_.transform.rotation = odom_.pose.pose.orientation;
        if (ros::ok()) {
            tf_pub_->sendTransform(tf_);
        }
    }

    if (ros::ok()) {
        odom_pub_.publish(odom_);
    }
}

void FusedOdomPublisher::imuCallback(const sensor_msgs::Imu::ConstPtr& imu)
{
    last_imu_time_ = imu->header.stamp;
    last_imu_ = imu;
}

template <typename T>
inline bool getRequiredParam(const ros::NodeHandle& n, std::string name, T& value)
{
  if (n.getParam(name, value))
    return true;

  ROS_FATAL("OdomPublisher: Parameter %s is required.", name.c_str());
  return false;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "fused_odom_publisher");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");

  FusedOdomPublisher fused_odom_publisher(nh, pnh);

  ros::spin();

  return 0;
}

max_stars_repo_name: jsford/FFAST

max_issues_repo_name: emilyfy/FFAST

max_forks_repo_name: emilyfy/FFAST


text:
// Copyright 2021 SiLeader and Cerussite.
//
// Licensed under the Apache License, Version 2.0 (the “License”);
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an “AS IS” BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <boost/log/trivial.hpp>
#include <random>

#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wimplicit-int-conversion"
#include <libnuraft/nuraft.hxx>
#pragma GCC diagnostic warning "-Wimplicit-int-conversion"
#pragma GCC diagnostic warning "-Wunused-parameter"

#include "storage_base.hpp"

namespace eidos::storage {

namespace detail {

inline void EncodeMessage(nuraft::buffer_serializer& bs, const BytesMessage& message) {
  const auto& mb = message.bytes();
  bs.put_bytes(static_cast<const void*>(mb.data()), mb.size());
}

class Logger : public nuraft::logger {
 public:
  void trace(const std::string& log_line) { BOOST_LOG_TRIVIAL(trace) << log_line; }
  void debug(const std::string& log_line) override { BOOST_LOG_TRIVIAL(debug) << log_line; }
  void info(const std::string& log_line) override { BOOST_LOG_TRIVIAL(info) << log_line; }
  void warn(const std::string& log_line) override { BOOST_LOG_TRIVIAL(warning) << log_line; }
  void err(const std::string& log_line) override { BOOST_LOG_TRIVIAL(error) << log_line; }
  void fatal(const std::string& log_line) { BOOST_LOG_TRIVIAL(fatal) << log_line; }

 public:
  void put_details(int level, const char*, const char*, size_t, const std::string& log_line) override {
    std::stringstream ss;
    ss << log_line;
    switch (level) {
      case 1:
        fatal(ss.str());
        break;
      case 2:
        err(ss.str());
        break;
      case 3:
        warn(ss.str());
        break;
      case 4:
        info(ss.str());
        break;
      case 5:
        debug(ss.str());
        break;
      case 6:
        trace(ss.str());
        break;
      default:
        err("unknown level");
    }
  }
};

class InMemoryLogStore : public nuraft::log_store {
 public:
  InMemoryLogStore() : start_idx_(1) {
    // Dummy entry for index 0.
    nuraft::ptr<nuraft::buffer> buf = nuraft::buffer::alloc(2);
    logs_[0] = nuraft::cs_new<nuraft::log_entry>(0, buf);
  }

  ~InMemoryLogStore() override = default;

  __nocopy__(InMemoryLogStore);

  nuraft::ulong next_slot() const override {
    std::lock_guard<std::mutex> l(logs_lock_);
    // Exclude the dummy entry.
    return start_idx_ + logs_.size() - 1;
  }

  nuraft::ulong start_index() const override { return start_idx_; }

  nuraft::ptr<nuraft::log_entry> last_entry() const override {
    nuraft::ulong next_idx = next_slot();
    std::lock_guard<std::mutex> l(logs_lock_);
    auto entry = logs_.find(next_idx - 1);
    if (entry == logs_.end()) {
      entry = logs_.find(0);
    }

    return make_clone(entry->second);
  }

  nuraft::ulong append(nuraft::ptr<nuraft::log_entry>& entry) override {
    nuraft::ptr<nuraft::log_entry> clone = make_clone(entry);

    std::lock_guard<std::mutex> l(logs_lock_);
    size_t idx = start_idx_ + logs_.size() - 1;
    logs_[idx] = clone;
    return idx;
  }

  void write_at(nuraft::ulong index, nuraft::ptr<nuraft::log_entry>& entry) override {
    nuraft::ptr<nuraft::log_entry> clone = make_clone(entry);

    // Discard all logs equal to or greater than `index.
    std::lock_guard<std::mutex> l(logs_lock_);
    auto itr = logs_.lower_bound(index);
    while (itr != logs_.end()) {
      itr = logs_.erase(itr);
    }
    logs_[index] = clone;
  }

  nuraft::ptr<std::vector<nuraft::ptr<nuraft::log_entry>>> log_entries(nuraft::ulong start,
                                                                       nuraft::ulong end) override {
    nuraft::ptr<std::vector<nuraft::ptr<nuraft::log_entry>>> ret =
        nuraft::cs_new<std::vector<nuraft::ptr<nuraft::log_entry>>>();

    ret->resize(end - start);
    nuraft::ulong cc = 0;
    for (nuraft::ulong ii = start; ii < end; ++ii) {
      nuraft::ptr<nuraft::log_entry> src;
      {
        std::lock_guard<std::mutex> l(logs_lock_);
        auto entry = logs_.find(ii);
        if (entry == logs_.end()) {
          assert(0);
        }
        src = entry->second;
      }
      (*ret)[cc++] = make_clone(src);
    }
    return ret;
  }

  nuraft::ptr<std::vector<nuraft::ptr<nuraft::log_entry>>> log_entries_ext(
      nuraft::ulong start, nuraft::ulong end, nuraft::int64 batch_size_hint_in_bytes) override {
    nuraft::ptr<std::vector<nuraft::ptr<nuraft::log_entry>>> ret =
        nuraft::cs_new<std::vector<nuraft::ptr<nuraft::log_entry>>>();

    if (batch_size_hint_in_bytes < 0) {
      return ret;
    }

    size_t accum_size = 0;
    for (nuraft::ulong ii = start; ii < end; ++ii) {
      nuraft::ptr<nuraft::log_entry> src;
      {
        std::lock_guard<std::mutex> l(logs_lock_);
        auto entry = logs_.find(ii);
        if (entry == logs_.end()) {
          assert(0);
        }
        src = entry->second;
      }
      ret->push_back(make_clone(src));
      accum_size += src->get_buf().size();
      if (batch_size_hint_in_bytes && accum_size >= (nuraft::ulong)batch_size_hint_in_bytes) break;
    }
    return ret;
  }

  nuraft::ptr<nuraft::log_entry> entry_at(nuraft::ulong index) override {
    nuraft::ptr<nuraft::log_entry> src;
    {
      std::lock_guard lg(logs_lock_);
      auto entry = logs_.find(index);
      if (entry == logs_.end()) {
        entry = logs_.find(0);
      }
      src = entry->second;
    }
    return make_clone(src);
  }

  nuraft::ulong term_at(nuraft::ulong index) override {
    nuraft::ulong term;
    {
      std::lock_guard lg(logs_lock_);
      auto entry = logs_.find(index);
      if (entry == logs_.end()) {
        entry = logs_.find(0);
      }
      term = entry->second->get_term();
    }
    return term;
  }

  nuraft::ptr<nuraft::buffer> pack(nuraft::ulong index, nuraft::int32 cnt) override {
    std::vector<nuraft::ptr<nuraft::buffer>> logs;

    const auto count = static_cast<std::size_t>(cnt);

    size_t size_total = 0;
    for (nuraft::ulong ii = index; ii < index + count; ++ii) {
      nuraft::ptr<nuraft::log_entry> le;
      {
        std::lock_guard lg(logs_lock_);
        le = logs_[ii];
      }
      assert(le.get());
      nuraft::ptr<nuraft::buffer> buf = le->serialize();
      size_total += buf->size();
      logs.push_back(buf);
    }

    auto buf_out = nuraft::buffer::alloc(sizeof(nuraft::int32) + count * sizeof(nuraft::int32) + size_total);
    buf_out->pos(0);
    buf_out->put((nuraft::int32)cnt);

    for (auto& entry : logs) {
      nuraft::ptr<nuraft::buffer>& bb = entry;
      buf_out->put((nuraft::int32)bb->size());
      buf_out->put(*bb);
    }
    return buf_out;
  }

  void apply_pack(nuraft::ulong index, nuraft::buffer& pack) override {
    pack.pos(0);
    const auto num_logs = static_cast<std::size_t>(pack.get_int());

    for (std::size_t ii = 0; ii < num_logs; ++ii) {
      const auto cur_idx = index + ii;
      const auto buf_size = static_cast<std::size_t>(pack.get_int());

      nuraft::ptr<nuraft::buffer> buf_local = nuraft::buffer::alloc(buf_size);
      pack.get(buf_local);

      nuraft::ptr<nuraft::log_entry> le = nuraft::log_entry::deserialize(*buf_local);
      {
        std::lock_guard<std::mutex> l(logs_lock_);
        logs_[cur_idx] = le;
      }
    }

    {
      std::lock_guard<std::mutex> l(logs_lock_);
      auto entry = logs_.upper_bound(0);
      if (entry != logs_.end()) {
        start_idx_ = entry->first;
      } else {
        start_idx_ = 1;
      }
    }
  }

  bool compact(nuraft::ulong last_log_index) override {
    std::lock_guard<std::mutex> l(logs_lock_);
    for (nuraft::ulong ii = start_idx_; ii <= last_log_index; ++ii) {
      auto entry = logs_.find(ii);
      if (entry != logs_.end()) {
        logs_.erase(entry);
      }
    }

    // WARNING:
    //   Even though nothing has been erased,
    //   we should set `start_idx_` to new index.
    start_idx_ = last_log_index + 1;
    return true;
  }

  bool flush() override { return true; }

  void close() {}

 private:
  static nuraft::ptr<nuraft::log_entry> make_clone(const nuraft::ptr<nuraft::log_entry>& entry) {
    nuraft::ptr<nuraft::log_entry> clone = nuraft::cs_new<nuraft::log_entry>(
        entry->get_term(), nuraft::buffer::clone(entry->get_buf()), entry->get_val_type());
    return clone;
  }

  std::map<nuraft::ulong, nuraft::ptr<nuraft::log_entry>> logs_;
  mutable std::mutex logs_lock_;
  std::atomic<nuraft::ulong> start_idx_;
};

class InMemoryStateManager : public nuraft::state_mgr {
 public:
  InMemoryStateManager(int srv_id, const std::string& endpoint)
      : my_id_(srv_id), cur_log_store_(nuraft::cs_new<InMemoryLogStore>()) {
    my_srv_config_ = nuraft::cs_new<nuraft::srv_config>(srv_id, endpoint);

    // Initial cluster config: contains only one server (myself).
    saved_config_ = nuraft::cs_new<nuraft::cluster_config>();
    saved_config_->get_servers().push_back(my_srv_config_);
  }

  ~InMemoryStateManager() override = default;

  nuraft::ptr<nuraft::cluster_config> load_config() override {
    // Just return in-memory data in this example.
    // May require reading from disk here, if it has been written to disk.
    return saved_config_;
  }

  void save_config(const nuraft::cluster_config& config) override {
    // Just keep in memory in this example.
    // Need to write to disk here, if want to make it durable.
    nuraft::ptr<nuraft::buffer> buf = config.serialize();
    saved_config_ = nuraft::cluster_config::deserialize(*buf);
  }

  void save_state(const nuraft::srv_state& state) override {
    // Just keep in memory in this example.
    // Need to write to disk here, if want to make it durable.
    nuraft::ptr<nuraft::buffer> buf = state.serialize();
    saved_state_ = nuraft::srv_state::deserialize(*buf);
  }

  nuraft::ptr<nuraft::srv_state> read_state() override {
    // Just return in-memory data in this example.
    // May require reading from disk here, if it has been written to disk.
    return saved_state_;
  }

  nuraft::ptr<nuraft::log_store> load_log_store() override { return cur_log_store_; }

  nuraft::int32 server_id() override { return my_id_; }

  void system_exit(const int) override {}

 private:
  int my_id_;
  nuraft::ptr<InMemoryLogStore> cur_log_store_;
  nuraft::ptr<nuraft::srv_config> my_srv_config_;
  nuraft::ptr<nuraft::cluster_config> saved_config_;
  nuraft::ptr<nuraft::srv_state> saved_state_;
};

class StateMachine : public nuraft::state_machine {
 private:
  struct Context {
    nuraft::ptr<nuraft::snapshot> snapshot;
    nuraft::ptr<nuraft::buffer> value;

    Context(nuraft::ptr<nuraft::snapshot> snapshot, nuraft::ptr<nuraft::buffer> value)
        : snapshot(std::move(snapshot)), value(std::move(value)) {}
    Context() = default;
  };

 private:
  std::atomic<uint64_t> last_committed_idx_;
  std::shared_ptr<StorageEngineBase> internal_engine_;

  std::mutex snapshots_mutex_;
  std::map<uint64_t, nuraft::ptr<Context>> snapshots_;

 public:
  explicit StateMachine(std::shared_ptr<StorageEngineBase> engine)
      : last_committed_idx_(0), internal_engine_(std::move(engine)) {}

 private:
  void commit(nuraft::buffer_serializer& bs) {
    const auto get_bytes = [&bs]() -> std::vector<std::byte> {
      std::size_t size = 0;
      const auto bytes = static_cast<const std::byte*>(bs.get_bytes(size));
      return std::vector<std::byte>(bytes, bytes + size);
    };
    const auto instruction = bs.get_u16();
    switch (instruction) {
      case 2: {  // SET
        BOOST_LOG_TRIVIAL(trace) << "do commit: SET";
        const auto kb = get_bytes();
        const auto digest = bs.get_u64();
        const auto vb = get_bytes();
        internal_engine_->set(Key(kb, digest), Value(vb));
        break;
      }
      case 3:  // DEL
        BOOST_LOG_TRIVIAL(trace) << "do commit: DEL";
        internal_engine_->del(Key(get_bytes(), bs.get_u64()));
        break;

      case 0:  // not assigned
      case 1:  // GET
      case 4:  // EXISTS
      case 5:  // KEYS
      default:
        BOOST_LOG_TRIVIAL(error) << "unknown commit instruction: other: " << instruction;
        break;
    }
  }

  nuraft::ptr<nuraft::buffer> dumpStorage() const {
    const auto dumped_res = internal_engine_->dump();
    if (dumped_res.is_err()) {
      return nullptr;
    }
    std::vector<nuraft::ptr<nuraft::buffer>> buffers;

    std::size_t whole_size = 0;
    const auto dumped = dumped_res.unwrap();
    for (const auto& [k, v] : dumped) {
      const auto size = 2 + 4 + k.bytes().size() + 8 + 4 + v.bytes().size();
      auto buffer = nuraft::buffer::alloc(size);
      whole_size += size;

      nuraft::buffer_serializer bs(buffer);
      bs.put_u16(2);  // SET
      EncodeMessage(bs, k);
      bs.put_u64(k.digest());
      EncodeMessage(bs, v);
      buffers.emplace_back(buffer);
    }

    auto buffer = nuraft::buffer::alloc(whole_size);
    for (const auto& b : buffers) {
      buffer->put(*b);
    }
    return buffer;
  }

 private:
  void createSnapshot(nuraft::snapshot& s) {
    const auto snapshot_buffer = s.serialize();
    const auto ss = nuraft::snapshot::deserialize(*snapshot_buffer);

    const auto dumped = dumpStorage();
    if (!dumped) {
      return;
    }
    const auto context = nuraft::cs_new<Context>(ss, dumped);
    snapshots_[s.get_last_log_idx()] = context;

    auto entry = std::begin(snapshots_);
    for (std::size_t i = 0, count = snapshots_.size(); i < (count - 3); ++i) {
      if (entry == std::end(snapshots_)) {
        break;
      }
      entry = snapshots_.erase(entry);
    }
  }

 public:
  nuraft::ptr<nuraft::buffer> pre_commit(const nuraft::ulong, nuraft::buffer&) override { return nullptr; }

  nuraft::ptr<nuraft::buffer> commit(const nuraft::ulong log_idx, nuraft::buffer& data) override {
    BOOST_LOG_TRIVIAL(trace) << "commit";
    nuraft::buffer_serializer bs(data);
    commit(bs);
    last_committed_idx_ = log_idx;

    nuraft::ptr<nuraft::buffer> ret = nuraft::buffer::alloc(sizeof(log_idx));
    nuraft::buffer_serializer rbs(ret);
    rbs.put_u64(log_idx);
    return ret;
  }

  bool apply_snapshot(nuraft::snapshot& s) override {
    BOOST_LOG_TRIVIAL(trace) << "apply snapshot";
    auto entry = snapshots_.find(s.get_last_log_idx());
    if (entry == std::end(snapshots_)) {
      return false;
    }
    auto buffer = entry->second->value;
    nuraft::buffer_serializer bs(buffer);
    while (bs.pos() < bs.size()) {
      commit(bs);
    }
    return true;
  }

  nuraft::ptr<nuraft::snapshot> last_snapshot() override {
    BOOST_LOG_TRIVIAL(trace) << "last snapshot";
    std::lock_guard<std::mutex> lg(snapshots_mutex_);
    auto entry = std::rbegin(snapshots_);
    if (entry == std::rend(snapshots_)) return nullptr;

    return entry->second->snapshot;
  }

  nuraft::ulong last_commit_index() override { return last_committed_idx_; }

  void create_snapshot(nuraft::snapshot& s, nuraft::async_result<bool>::handler_type& when_done) override {
    BOOST_LOG_TRIVIAL(trace) << "create snapshot";
    {
      std::lock_guard lg(snapshots_mutex_);
      createSnapshot(s);
    }

    bool res = true;
    nuraft::ptr<std::exception> e = nullptr;
    when_done(res, e);
  }
};

}  // namespace detail

class RaftStorageEngine : public StorageEngineBase {
 private:
  nuraft::ptr<nuraft::state_machine> state_machine_;
  nuraft::ptr<nuraft::state_mgr> state_manager_;
  nuraft::raft_launcher launcher_;
  nuraft::ptr<nuraft::raft_server> raft_server_;
  std::shared_ptr<StorageEngineBase> internal_engine_;

 public:
  explicit RaftStorageEngine(const std::shared_ptr<StorageEngineBase>& engine, std::uint16_t raft_port)
      : state_machine_(nuraft::cs_new<detail::StateMachine>(engine)),
        state_manager_(nuraft::cs_new<detail::InMemoryStateManager>(std::random_device{}(),
                                                                    "0.0.0.0:" + std::to_string(raft_port))),
        internal_engine_(engine) {
    raft_server_ = launcher_.init(state_machine_, state_manager_, nuraft::cs_new<detail::Logger>(), raft_port,
                                  nuraft::asio_service::options{}, nuraft::raft_params{});
    while (!raft_server_->is_initialized()) {
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
  }

  ~RaftStorageEngine() override { raft_server_->shutdown(); }

 public:
  Result<void> set(const Key& key, const Value& value) override {
    auto buf = nuraft::buffer::alloc(2 + 4 + key.bytes().size() + 8 + 4 + value.bytes().size());
    nuraft::buffer_serializer bs(buf);
    bs.put_u16(2);
    detail::EncodeMessage(bs, key);
    bs.put_u64(key.digest());
    detail::EncodeMessage(bs, value);

    const auto res = raft_server_->append_entries({buf});
    return Result<void>::Ok();
  }

  Result<Value> get(const Key& key) override { return internal_engine_->get(key); }

  Result<void> del(const Key& key) override {
    auto buf = nuraft::buffer::alloc(2 + 4 + key.bytes().size() + 8);
    nuraft::buffer_serializer bs(buf);
    bs.put_u16(3);
    detail::EncodeMessage(bs, key);
    bs.put_u64(key.digest());

    raft_server_->append_entries({buf});
    return Result<void>::Ok();
  }

  Result<bool> exists(const Key& key) override { return internal_engine_->exists(key); }

  Result<std::vector<Key>> keys(const std::string& pattern) override { return internal_engine_->keys(pattern); }

  Result<std::vector<std::tuple<Key, Value>>> dump() override { return internal_engine_->dump(); }
};

}  // namespace eidos::storage

max_stars_repo_name: SiLeader/eidos

max_issues_repo_name: SiLeader/eidos

max_forks_repo_name: SiLeader/eidos


text:
/*
  Test program for MatPlot

  Dag Lindbo, dag@csc.kth.se
*/

#include "matplot.h"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace ublas = boost::numeric::ublas;
typedef ublas::vector<double> Vector;
typedef ublas::matrix<double> Matrix;

using namespace std;
using namespace matplot;

// parameters for 2D plot
const int NN = 40;
const double t_low = 0;
const double t_upp = 5;
const double dt = (t_upp-t_low)/(NN-1);

// parameters for 3D stuff
const int Nx = 200;
const int Ny = 250;
const double x_low = -2.5;
const double x_upp = 1.5;
const double y_low = -2.5;
const double y_upp = 2.5;
const double dx = (x_upp-x_low)/(Nx-1);
const double dy = (y_upp-y_low)/(Ny-1);

#define G(x,y) exp(-(x*x+y*y))*sin(x)*cos(y);

// parameters for quiver plot
const int NNx = 20;
const int NNy = 37;
const double xx_low = 0;
const double xx_upp = 1;
const double yy_low = 0;
const double yy_upp = 1.5;
const double dxx = (xx_upp-xx_low)/(NNx-1);
const double dyy = (yy_upp-yy_low)/(NNy-1);

int main(void)
{
  int i, j;

  // EXAMPLE: Plot_2D =======================================
  Vector x1(NN);
  Vector y1(NN);
  Vector x2(NN);
  Vector y2(NN);

  double t = t_low;

  for (i=0; i<NN; i++)
    {
      x1(i) = cos(t)*t;
      y1(i) = sin(t);
      x2(i) = sin(t)*t+1;
      y2(i) = cos(t)*(0.1-t);
      t+=dt;
    }

  Plot2D_VTK p_2d("x(t)", "y(t)", 800, 600);

  double color1[3] =
    { 0.0, 0.0, 1.0 };
  double color2[3] =
    { 1.0, 0.0, 0.0 };

  p_2d.plot(x1, y1, color1, "-");
  p_2d.plot(x2, y2, color2, ".-");
  p_2d.show();
  //p_2d.draw_to_png("plot_2d.png");

  // EXAMPLE: Contour =======================================

  Vector x(Nx);
  Vector y(Ny);
  Matrix z(Nx, Ny);

  for (i=0; i<Nx; i++)
    x(i) = x_low+i*dx;

  for (i=0; i<Ny; i++)
    y(i) = y_low + i*dy;

  for (i=0; i<Nx; i++)
    for (j=0; j<Ny; j++)
      z(i, j) = G(x(i),y(j));

  Contour_VTK p_cont(800, 600);
  p_cont.contour(x, y, z, true, 20);
  //p_cont.contour_to_file(x,y,z,true,20,"contours.png");

  // EXAMPLE: Surf ==========================================

  double cam[3] =
    { -15.0, 10.0, 12.0 };
  double focal[3] =
    { 0, 0, 0 };
  Surf_VTK p_surf(800, 600);
  //p_surf.surf(x,y,z);
  //p_surf.surf(x,y,z,false);
  p_surf.surf(x, y, z, true, cam, focal);
  //p_surf.surf_to_file(x,y,z,true,"surf.png",cam,focal);

  // Example: Quiver ========================================
  Vector xx(NNx);
  Vector yy(NNy);
  Matrix u(NNx, NNy);
  Matrix v(NNx, NNy);

  for (i=0; i<NNx; i++)
    xx(i) = xx_low+i*dxx;

  for (i=0; i<NNy; i++)
    yy(i) = yy_low + i*dyy;

  for (i=0; i<NNx; i++)
    for (j=0; j<NNy; j++)
      {
   	u(i, j) = -yy(j);
   	v(i, j) = xx(i);
      }

  Quiver_VTK p_quiver(800, 600);
  p_quiver.quiver(xx, yy, u, v, 0.1);
  //p_quiver.quiver_to_file(xx,yy,u,v,0.1,"quiver.png");

  return 0;
}


text:
#include <iostream>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>

#include <Eigen/Core>
#include <igl/polar_dec.h>
#include <igl/polar_svd.h>

namespace py = pybind11;

template<typename float_t>
py::array_t<float_t> inv3(py::array_t<float_t, py::array::c_style> & Ts)
{
    auto Ts_buf = Ts.request();
    float_t *pT = (float_t*)Ts_buf.ptr;

    auto result = py::array_t<float_t, py::array::c_style>(Ts_buf.size);
    auto result_buf = result.request();
    float_t *pR = (float_t*)result_buf.ptr;

    for (size_t idx = 0; idx < Ts_buf.shape[0]; idx++) {
        const float_t T00 = pT[0], T01 = pT[1], T02 = pT[2];
        const float_t T10 = pT[3], T11 = pT[4], T12 = pT[5];
        const float_t T20 = pT[6], T21 = pT[7], T22 = pT[8];
        const float_t det = T00 * (T22 * T11 - T21 * T12) \
                         - T10 * (T22 * T01 - T21 * T02) \
                         + T20 * (T12 * T01 - T11 * T02);
        const float_t invDet = 1. / det;
        pR[0] =  (T11 * T22 - T21 * T12) * invDet;
        pR[1] = -(T01 * T22 - T02 * T21) * invDet;
        pR[2] =  (T01 * T12 - T02 * T11) * invDet;
        pR[3] = -(T10 * T22 - T12 * T20) * invDet;
        pR[4] =  (T00 * T22 - T02 * T20) * invDet;
        pR[5] = -(T00 * T12 - T10 * T02) * invDet;
        pR[6] =  (T10 * T21 - T20 * T11) * invDet;
        pR[7] = -(T00 * T21 - T20 * T01) * invDet;
        pR[8] =  (T00 * T11 - T10 * T01) * invDet;

        pT += 3*3;
        pR += 3*3;
    }

    return result;
}

template<typename float_t>
py::array_t<float_t> inv2(py::array_t<float_t, py::array::c_style> & Ts)
{
    auto Ts_buf = Ts.request();
    float_t *pT = (float_t*)Ts_buf.ptr;

    auto result = py::array_t<float_t, py::array::c_style>(Ts_buf.size);
    auto result_buf = result.request();
    float_t *pR = (float_t*)result_buf.ptr;

    for (size_t idx = 0; idx < Ts_buf.shape[0]; idx++) {
        const float_t T00 = pT[0], T01 = pT[1];
        const float_t T10 = pT[2], T11 = pT[3];
        const float_t det = T00 * T11 - T01 * T10;
        const float_t invDet = 1. / det;
        pR[0] =  T11 * invDet;
        pR[1] = -1 * T01 * invDet;
        pR[2] = -1 * T10 * invDet;
        pR[3] = T00 * invDet;

        pT += 2*2;
        pR += 2*2;
    }

    return result;
}

template<typename float_t>
py::array_t<float_t> matmat(
        py::array_t<float_t, py::array::c_style> & a,
        py::array_t<float_t, py::array::c_style> & b
    )
{
    auto a_buf = a.request();
    float_t *p_a = (float_t*)a_buf.ptr;
    auto b_buf = b.request();
    float_t *p_b = (float_t*)b_buf.ptr;

    auto result = py::array_t<float_t, py::array::c_style>(
            {a_buf.shape[0], a_buf.shape[1], b_buf.shape[2]});
    auto result_buf = result.request();
    float_t *p_res = (float_t*)result_buf.ptr;

    const size_t n_rows_a = a_buf.shape[1];
    const size_t n_cols_a = a_buf.shape[2];
    const size_t n_rows_b = b_buf.shape[1];
    const size_t n_cols_b = b_buf.shape[2];
    assert(n_cols_a == n_rows_b);
    for (size_t idx = 0; idx < a_buf.shape[0]; idx++) {
        for (size_t row_a = 0; row_a < n_rows_a; row_a++) {
            for (size_t col_b = 0; col_b < n_cols_b; col_b++) {
                float_t sum = 0.0;
                for (size_t k = 0; k < n_cols_a; k++) {
                    const float_t ai = p_a[row_a * n_cols_a + k];
                    const float_t bi = p_b[k * n_cols_b + col_b];
                    sum += ai * bi;
                }
                *p_res = sum;
                p_res++;
            }
        }
        p_a += n_cols_a * n_rows_a;
        p_b += n_cols_b * n_rows_b;
    }

    return result;
}

template<typename float_t>
py::array_t<float_t> matvec(
        py::array_t<float_t, py::array::c_style> & mats,
        py::array_t<float_t, py::array::c_style> & vecs
    )
{
    auto mats_buf = mats.request();
    float_t *p_mats = (float_t*)mats_buf.ptr;
    auto vecs_buf = vecs.request();
    float_t *p_vecs = (float_t*)vecs_buf.ptr;

    auto result = py::array_t<float_t, py::array::c_style>({mats_buf.shape[0], mats_buf.shape[1]});
    auto result_buf = result.request();
    float_t *p_res = (float_t*)result_buf.ptr;

    const size_t mat_stride1 = mats_buf.strides[1] / sizeof(float_t);
    for (size_t idx = 0; idx < mats_buf.shape[0]; idx++) {
        for (size_t row = 0; row < mats_buf.shape[1]; row++) {
            float_t sum = 0.0;
            for (size_t k = 0; k < mats_buf.shape[2]; k++) {
                sum += *(p_mats++) * p_vecs[k];
            }
            *p_res = sum;
            p_res++;
        }
        p_vecs += vecs_buf.shape[1];
    }

    return result;
}

template<typename float_t>
py::array_t<float_t> cross3(
        py::array_t<float_t, py::array::c_style> & a,
        py::array_t<float_t, py::array::c_style> & b
    )
{
    auto a_buf = a.request();
    float_t *p_a = (float_t*)a_buf.ptr;
    auto b_buf = b.request();
    float_t *p_b = (float_t*)b_buf.ptr;

    auto result = py::array_t<float_t, py::array::c_style>(
            {a_buf.shape[0], a_buf.shape[1]});
    auto result_buf = result.request();
    float_t *p_res = (float_t*)result_buf.ptr;

    for (size_t idx = 0; idx < a_buf.shape[0]; idx++) {
        const double ax = p_a[0];
        const double ay = p_a[1];
        const double az = p_a[2];
        const double bx = p_b[0];
        const double by = p_b[1];
        const double bz = p_b[2];
        p_res[0] = ay * bz - az * by;
        p_res[1] = az * bx - ax * bz;
        p_res[2] = ax * by - ay * bx;
        p_res += 3;
        p_a += 3;
        p_b += 3;
    }

    return result;
}

template<typename float_t>
py::array_t<float_t> multikron(
        py::array_t<float_t, py::array::c_style> & a,
        py::array_t<float_t, py::array::c_style> & b
    )
{
    auto a_buf = a.request();
    float_t *p_a = (float_t*)a_buf.ptr;
    auto b_buf = b.request();
    float_t *p_b = (float_t*)b_buf.ptr;

    const auto n_rows_a = a_buf.shape[1];
    const auto n_cols_a = a_buf.shape[2];
    const auto n_rows_b = b_buf.shape[1];
    const auto n_cols_b = b_buf.shape[2];

    auto result = py::array_t<float_t, py::array::c_style>(
            {a_buf.shape[0], n_rows_a * n_rows_b, n_cols_a * n_cols_b});
    auto result_buf = result.request();
    float_t *p_res = (float_t*)result_buf.ptr;

    for (size_t idx = 0; idx < a_buf.shape[0]; idx++) {
        // iterate over rows of a
        for (size_t row_a = 0; row_a < n_rows_a; row_a++) {
            // iterate over rows of b
            for (size_t row_b = 0; row_b < n_rows_b; row_b++) {
                // iterate over columns of a
                for (size_t col_a = 0; col_a < n_cols_a; col_a++) {
                    // iterate over columns of b
                    for (size_t col_b = 0; col_b < n_cols_b; col_b++) {
                        const float_t ai = p_a[row_a * n_cols_a + col_a];
                        const float_t bi = p_b[row_b * n_cols_b + col_b];
                        *p_res = ai * bi;
                        p_res++;
                    }
                }
            }
        }
        // next matrix
        p_a += n_cols_a * n_rows_a;
        p_b += n_cols_b * n_rows_b;
    }

    return result;
}

std::tuple<py::array_t<double>, py::array_t<double>>
polarDecompose(py::array_t<double, py::array::c_style> Ms)
{
    using RowMat3d = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>;

    auto Ms_raw = Ms.unchecked<3>();
    // allocate output matrices
    auto Rs = py::array_t<double>({Ms_raw.shape(0), 3l, 3l});
    auto Rs_raw = Rs.mutable_unchecked<3>();
    auto Ss = py::array_t<double>({Ms_raw.shape(0), 3l, 3l});
    auto Ss_raw = Ss.mutable_unchecked<3>();

    RowMat3d U;
    RowMat3d V;
    Eigen::Matrix<double,3,1> S;

    for (int i = 0; i < Ms_raw.shape(0); ++i) {
        const RowMat3d Mi = Eigen::Map<const RowMat3d>(Ms_raw.data(i, 0, 0));
        Eigen::Map<RowMat3d> Ri_map(Rs_raw.mutable_data(i, 0, 0));
        Eigen::Map<RowMat3d> Si_map(Ss_raw.mutable_data(i, 0, 0));
        RowMat3d Ri = Ri_map; // performs copy, since igl::polar_dec does not take Eigen::Map
        RowMat3d Si = Si_map;
        //igl::polar_dec(Mi, Ri, Si);
        igl::polar_svd(Mi, Ri, Si, U, S, V);
        // TODO: use igl::polar_svd3x3?
        // write back results
        Ri_map = Ri;
        Si_map = Si;
    }

    return std::make_tuple(Rs, Ss);
}

PYBIND11_MODULE(_fastmath_ext, m) {
    m.def("inv3", &inv3<float>);
    m.def("inv3", &inv3<double>);
    m.def("inv2", &inv2<float>);
    m.def("inv2", &inv2<double>);
    m.def("matmat", &matmat<double>);
    m.def("matvec", &matvec<double>);
    m.def("cross3", &cross3<double>);
    m.def("multikron", &multikron<double>);
    m.def("polar_dec", &polarDecompose);
}

max_stars_repo_name: tneumann/cgtools

max_issues_repo_name: tneumann/cgtools

max_forks_repo_name: tneumann/cgtools