void computeStats(PointWithNormalStatistcsGenerator & generator, const Matrix3f& cameraMatrix){
zBuffer.resize(depthImage.rows(), depthImage.cols());
gaussians.fromDepthImage(depthImage,cameraMatrix);
gaussians.toPointWithNormalVector(points);
indexImage.resize(depthImage.rows(), depthImage.cols());
gaussians.toIndexImage(indexImage, zBuffer, cameraMatrix, Eigen::Isometry3f::Identity(), 10);
cerr << "points: " << points.size() << endl;
svds.resize(points.size());
double tNormalStart = get_time();
generator.computeNormalsAndSVD(points, svds, indexImage, cameraMatrix);
double tNormalEnd = get_time();
cerr << "Normal Extraction took " << tNormalEnd - tNormalStart << " sec." << endl;
}
int Simulation::initializeSimulation(double deltaT, int iterations, char method, MatrixXi& TT, MatrixXd& TV, MatrixXd& B, vector<int>& moveVertices, vector<int> fixVertices, double youngs, double poissons){
iters = iterations;
if (method =='e'){
integrator = new Verlet();
cout<<"Initialized Verlet"<<endl;
}else if(method == 'i'){
integrator = new ImplicitEuler();
cout<<"Initialized Implicit Euler"<<endl;
}
else if(method == 'n'){
integrator = new ImplicitNewmark();
cout<<"Initialized Implicit Newmark"<<endl;
}
else{
cout<<"Method not supported yet"<<endl;
exit(0);
}
VectorXd force;
force.resize(3*TV.rows());
force.setZero();
setInitPosition(force, fixVertices, moveVertices);
if(moveVertices.size()>0 or fixVertices.size()>0){
//cout << "DOING STUFFS" << endl;
MatrixXd newTV;
newTV.resize(TV.rows(), TV.cols());
newTV.setZero();
MatrixXi newTT;
newTT.resize(TT.rows(), TT.cols());
newTT.setZero();
//cout << "MoveVertsSize :: " << moveVertices.size() << endl;
//TODO: Make this shit more efficient
//Hash maps or something
vector<int> vertexNewIndices;
for(int i=0; i<TV.rows(); i++){
bool flag =false;
for(unsigned int j=0; j<fixVertices.size(); j++){
if(i==fixVertices[j]){
flag = true;
}
}
for(unsigned int j=0; j<moveVertices.size(); j++){
if(i==moveVertices[j]){
flag = true;
}
}
// if vertex not fixed or moved, re-index to front
//[v, v, v, v...., f, f, f...., m, m, m...,m]
if(!flag){
vertexNewIndices.push_back(i);
}
}
//re-index fixed verts
for(unsigned int j=0; j<fixVertices.size(); j++){
vertexNewIndices.push_back(fixVertices[j]);
}
//re-index move verts
for(unsigned int j=0; j<moveVertices.size(); j++){
vertexNewIndices.push_back(moveVertices[j]);
}
//these are the new indices for the fixed verts
vector<int> newfixIndices;
for(unsigned int i= vertexNewIndices.size() - (moveVertices.size() + fixVertices.size()); i<(vertexNewIndices.size()-moveVertices.size()); i++){
newfixIndices.push_back(i);
}
//new indices for the moving verts
vector<int> newMoveIndices;
for(unsigned int i= vertexNewIndices.size() - moveVertices.size(); i<vertexNewIndices.size(); i++){
newMoveIndices.push_back(i);
}
//cout << "NewMoveIndicesSize :: " << newMoveIndices.size() << endl;
VectorXd new_force;
new_force.resize(3*TV.rows());
reIndexTVandTT(vertexNewIndices, fixVertices.size(), moveVertices.size(), TV, TT, force, newTV, newTT, new_force);
igl::barycenter(newTV, newTT, B);
//Initialize Solid Mesh
M.initializeMesh(newTT, newTV, youngs, poissons);
if(moveVertices.size() != 0){
// cout<<"Move vertices "<<moveVertices.size()<<endl;
// cout<<"fix verts "<<fixVertices.size()<<endl;
binarySearchYoungs(newMoveIndices, newTV, newTT, fixVertices.size(), B);
// syntheticTests(newMoveIndices, newTV, newTT, fixVertices.size(), B);
}
integrator->initializeIntegrator(deltaT, M, newTV, newTT);
this->external_force = new_force;
integrator->fixVertices(newfixIndices);
int ignorePastIndex = newTV.rows() - newfixIndices.size();
staticSolveNewtonsForces(newTV, newTT, B, new_force, ignorePastIndex);
}else{
//cout << "Doing Other Stuffs" << endl;
igl::barycenter(TV, TT, B);
M.initializeMesh(TT, TV, youngs, poissons);
integrator->initializeIntegrator(deltaT, M, TV, TT);
this->external_force = force;
integrator->fixVertices(fixVertices);
}
return 1;
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
using namespace std;
using namespace Eigen;
using namespace igl;
igl::matlab::MexStream mout;
std::streambuf *outbuf = cout.rdbuf(&mout);
//mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);
// vertex position list
double * V;
// face list
double * Ftemp;
// origin list
double * O;
// number of dimensions (and simplex size in F)
int dim;
// number of mesh vertices
int n;
// number of mesh faces
int m;
// number of origins
int no;
winding_number_params params;
parse_rhs(nrhs,prhs,dim,V,n,Ftemp,m,O,no,params);
double * F = new double[dim*m];
// convert to 0-index
//transform(Ftemp,Ftemp+m*dim,F,bind2nd(plus<double>(),-1.0));
for(int i = 0;i<m;i++)
{
for(int d = 0;d<dim;d++)
{
F[d*m + i]=Ftemp[d*m+i]-1;
}
}
//// Set up openmp
//int nProcessors=omp_get_max_threads();
////std::cout<<nProcessors<<std::endl;
//omp_set_num_threads(min(nProcessors,2));
// List of winding numbers
double * W = NULL;
if(nlhs == 0)
{
return;
}
prepare_lhs(nlhs,plhs,no,W);
double start_sec = igl::get_seconds();
if(params.ray_cast)
{
#ifdef WITH_EMBREE
// Prepare eigen versions of input
MatrixXd MV(n,dim);
copy(V,V+n*dim,MV.data());
MatrixXi MF(m,dim);
copy(F,F+m*dim,MF.data());
// Initialize Embree
//cout<<"Flipping faces..."<<endl;
MatrixXi FF;
FF.resize(MF.rows()*2,MF.cols());
FF << MF, MF.rowwise().reverse().eval();
//cout<<"Computing normals..."<<endl;
Eigen::MatrixXd N;
per_face_normals(MV,MF,N);
//cout<<"Initializing Embree..."<<endl;
// Initialize intersector
igl::embree::EmbreeIntersector ei;
ei.init(MV.cast<float>(),FF);
// loop over origins
# pragma omp parallel for if (no>IGL_WINDING_NUMBER_OMP_MIN_VALUE)
for(int o = 0;o<no;o++)
{
Vector3d p(O[0*no+o], O[1*no+o], O[2*no+o]);
for(int r = 0;r<params.num_rays;r++)
{
int num_rays_shot = -1;
Vector3d dir = random_dir();
if(params.twod_rays)
{
dir[2] = 0;
dir.normalize();
}
double w = 0;
if(params.ray_parity)
{
winding_number_ray_parity(ei,N,p,dir,w,num_rays_shot);
}else
{
winding_number_ray(ei,N,p,dir,w,num_rays_shot);
}
W[o] += w;
}
W[o] /= (double)params.num_rays;
}
#else
igl::matlab::mexErrMsgTxt(false,"Recompile with WITH_EMBREE defined");
#endif
}else
{
if(params.hierarchical && dim == 3)
{
// Prepare eigen versions of input
MatrixXd MV(n,dim);
copy(V,V+n*dim,MV.data());
MatrixXi MF(m,dim);
copy(F,F+m*dim,MF.data());
// Initialize hierarchy
WindingNumberAABB<Vector3d> hier(MV,MF);
// Build hierarchy
hier.grow();
// loop over origins
# pragma omp parallel for if (no>IGL_WINDING_NUMBER_OMP_MIN_VALUE)
for(int o = 0;o<no;o++)
{
Vector3d p(O[0*no+o], O[1*no+o], O[2*no+o]);
W[o] = hier.winding_number(p);
}
}else
{
switch(dim)
{
case 3:
winding_number_3(V,n,F,m,O,no,W);
break;
case 2:
winding_number_2(V,n,F,m,O,no,W);
break;
}
}
}
//mexPrintf("Elapsed time is %g seconds\n",igl::get_seconds()-start_sec);
// Restore the std stream buffer Important!
delete[] F;
std::cout.rdbuf(outbuf);
}
bool MNE::read_events(QIODevice &p_IODevice, MatrixXi& eventlist)
{
//
// Open file
//
FiffStream::SPtr t_pStream(new FiffStream(&p_IODevice));
if(!t_pStream->open()) {
return false;
}
//
// Find the desired block
//
QList<FiffDirNode::SPtr> events = t_pStream->dirtree()->dir_tree_find(FIFFB_MNE_EVENTS);
if (events.size() == 0)
{
printf("Could not find event data\n");
return false;
}
qint32 k, nelem;
fiff_int_t kind, pos;
FiffTag::SPtr t_pTag;
quint32* serial_eventlist_uint = NULL;
qint32* serial_eventlist_int = NULL;
for(k = 0; k < events[0]->nent(); ++k)
{
kind = events[0]->dir[k]->kind;
pos = events[0]->dir[k]->pos;
if (kind == FIFF_MNE_EVENT_LIST)
{
t_pStream->read_tag(t_pTag,pos);
if(t_pTag->type == FIFFT_UINT)
{
serial_eventlist_uint = t_pTag->toUnsignedInt();
nelem = t_pTag->size()/4;
}
if(t_pTag->type == FIFFT_INT)
{
serial_eventlist_int = t_pTag->toInt();
nelem = t_pTag->size()/4;
}
break;
}
}
if(serial_eventlist_uint == NULL && serial_eventlist_int == NULL)
{
printf("Could not find any events\n");
return false;
}
else
{
eventlist.resize(nelem/3,3);
if(serial_eventlist_uint != NULL)
{
for(k = 0; k < nelem/3; ++k)
{
eventlist(k,0) = serial_eventlist_uint[k*3];
eventlist(k,1) = serial_eventlist_uint[k*3+1];
eventlist(k,2) = serial_eventlist_uint[k*3+2];
}
}
if(serial_eventlist_int != NULL)
{
for(k = 0; k < nelem/3; ++k)
{
eventlist(k,0) = serial_eventlist_int[k*3];
eventlist(k,1) = serial_eventlist_int[k*3+1];
eventlist(k,2) = serial_eventlist_int[k*3+2];
}
}
}
return true;
}
RcppExport SEXP BMEclustering(SEXP mat, SEXP moda, SEXP nb_cluster, SEXP partition_initiale, SEXP nb_init,SEXP stop_criterion){
srand(time(0));
MatrixXi data=convertMatrix<MatrixXi,NumericMatrix>(mat);
VectorXi modalite=convertvector<VectorXi,NumericVector>(moda);
datafile dataF(data,modalite);
NumericMatrix red=convertMatrix<NumericMatrix,MatrixXi>(dataF.Get_mat_datafile());
VectorXi partition_vbles=convertvector<VectorXi,NumericVector>(partition_initiale);
MatrixXi m;
int g=as<int>(nb_cluster);
//int borne=as<int>(nbiter);
m.resize(g,partition_vbles.rows());
for (int k=0;k<g;k++){
m.row(k)=partition_vbles;
}
NumericMatrix test=convertMatrix<NumericMatrix,MatrixXi>(m);
int nbinit=as<int>(nb_init);
int borne=as<int>(stop_criterion);
MCMCAlgo ref(dataF,m,m.rows(),2,1,2,6,borne);
for (int ini=0;ini<nbinit;ini++){
MCMCAlgo test(dataF,m,m.rows(),2,1,2,6,borne);
if (test.Get_best_bic()>ref.Get_best_bic()){
ref=test;
}
}
//sauvegarde des caractéristiques du modèle
NumericMatrix model=convertMatrix<NumericMatrix,MatrixXi>(ref.Get_best_omega());
double bic=ref.Get_best_bic();
double likelihood=ref.Get_best_like();
NumericMatrix probapost=convertMatrix<NumericMatrix,MatrixXd>(ref.Sauv_probapost());
NumericVector localise=convertvector<NumericVector,VectorXi>(dataF.Get_localise());
vector< vector< NumericVector > > tau;
vector< vector< vector< NumericVector > > > delta;
vector< vector< vector< NumericVector > > > alpha;
vector< vector< double > > rho;
tau.resize(g);
rho.resize(g);
delta.resize(g);
alpha.resize(g);
for (int k=0;k<g;k++){
tau[k].resize(ref.Get_best_omega().row(k).maxCoeff()+1);
rho[k].resize(ref.Get_best_omega().row(k).maxCoeff()+1);
delta[k].resize(ref.Get_best_omega().row(k).maxCoeff()+1);
alpha[k].resize(ref.Get_best_omega().row(k).maxCoeff()+1);
for (int b=0;b<=ref.Get_best_omega().row(k).maxCoeff();b++){
//for (int b=0;b<2;b++){
//vectblock[k][b]=ref.Get_rho(k,b);
//vector < VectorXd > passe=ref.Get_alpha(k,b);
if ((ref.Get_best_omega().row(k).array()==b).count()>0){
vector<VectorXd> passe=ref.Get_alpha(k,b);
alpha[k][b].resize((ref.Get_best_omega().row(k).array()==b).count());
for (int loc=0;loc<((ref.Get_best_omega().row(k).array()==b).count());loc++){
alpha[k][b][loc]=convertvector<NumericVector,VectorXd>(passe[loc]);
}
}
if ((ref.Get_best_omega().row(k).array()==b).count()>1){
MatrixXi passe=ref.Get_delta(k,b);
delta[k][b].resize(passe.cols());
tau[k][b]=convertvector<NumericVector,VectorXd>(ref.Get_tau(k,b));
for (int loc=0;loc<passe.cols();loc++){
delta[k][b][loc]=convertvector<NumericVector,VectorXi>(passe.col(loc));
}
//delta[k][b]=convertMatrix<NumericMatrix,MatrixXi>(ref.Get_delta(k,b));
rho[k][b]=ref.Get_rho(k,b);
}
}
}
List param=List::create(Rcpp::Named("tau")=tau,Rcpp::Named("rho")=rho,Rcpp::Named("delta")=delta,Rcpp::Named("alpha")=alpha,Rcpp::Named("proportions")=convertvector<NumericVector,VectorXd>(ref.Get_propor()));
List desc_model = List::create(Rcpp::Named("sigma")=model,Rcpp::Named("bic")=bic,Rcpp::Named("likelihood")=likelihood,Rcpp::Named("probapost")=probapost,Rcpp::Named("partition")=localise,Rcpp::Named("nbcluster")=nb_cluster,Rcpp::Named("parameters")=param);
return desc_model;
}
