void NormalModeMeanSquareFluctuationCalculator::calculate_eigenvalues_and_eigenvectors() {
LOGD << "calculating eigenvalues and eigenvectors";
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(this->hessian_matrix);
this->eigenvalues = eigen_solver.eigenvalues();
this->eigenvectors = eigen_solver.eigenvectors();
}
/**
\brief computes the spectral radius of the reservoir (NOT necessarily
the DESIRED spectral radius)
**/
double Reservoir::getActualSpectralRadius(void) {
//just compute the largest eigenvalue (absolute)
Eigen::SelfAdjointEigenSolver<MatrixXd> eigen_solver(this->reservoir_weights);
if (eigen_solver.info() != Eigen::Success) {
std::cout << "Whoops! can't seem to compute the eigenvalues" << std::endl;
return 0.0;
}
double max_value = fabs(eigen_solver.eigenvalues().maxCoeff());
double min_value = fabs(eigen_solver.eigenvalues().minCoeff());
return (max_value > min_value) ? max_value : min_value;
}
std::chrono::duration<double> cis(calculation_data &data) {
std::chrono::duration<double> elapsed_seconds(0);
auto start = std::chrono::steady_clock::now();
// build A matrix
int sz = (data.n_paired/2)*(data.n_baseset-data.n_paired/2);
hermitian_matrix epsilon(sz);
hermitian_matrix J(sz);
hermitian_matrix K(sz);
std::function<int(int,int)> idx = [&](int i,int a) {
return (data.n_baseset-data.n_paired/2)*i + (a-data.n_paired/2);
};
// build epsilon, J and K
for(int i=0; i<data.n_paired/2; i++) {
for(int a=data.n_paired/2; a<data.n_baseset; a++) {
int ia = idx(i,a);
for(int j=0; j<data.n_paired/2; j++) {
for(int b=data.n_paired/2; b<data.n_baseset; b++) {
int jb = idx(j,b);
epsilon(ia,jb) = ( i==j&&a==b ? (data.eigenvalues[a]-data.eigenvalues[i]) : 0 );
J(ia,jb) = data.mo_2eint(a,j,i,b);
K(ia,jb) = data.mo_2eint(a,j,b,i);
}
}
}
}
// build A_singlet and A_triplet
data.A_singlet = epsilon + 2*J - K;
data.A_triplet = epsilon - K;
// solve for eigenvalues
eigen_solver(data.A_singlet, data.ci_singlet_wavefunc, data.ci_singlet_excited);
eigen_solver(data.A_triplet, data.ci_triplet_wavefunc, data.ci_triplet_excited);
auto end = std::chrono::steady_clock::now();
elapsed_seconds = std::chrono::duration_cast<std::chrono::duration<double>>(end-start);
return elapsed_seconds;
}
void RigidObject::computeBoundingBox() {
_bounding_box.resize(8 * 3);
Eigen::Map<const Eigen::MatrixXf> vertices_f(getPositions().data(), 3,
getNPositions());
Eigen::MatrixXd vertices;
vertices = vertices_f.cast<double>();
// subtract vertices mean
Eigen::Vector3d mean_vertices = vertices.rowwise().mean();
vertices = vertices - mean_vertices.replicate(1, getNPositions());
// compute eigenvector covariance matrix
Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver(vertices *
vertices.transpose());
Eigen::MatrixXd real_eigenvectors = eigen_solver.eigenvectors().real();
// rotate centered vertices with inverse eigenvector matrix
vertices = real_eigenvectors.transpose() * vertices;
// compute simple bounding box
auto mn = vertices.rowwise().minCoeff();
auto mx = vertices.rowwise().maxCoeff();
Eigen::Matrix<double, 3, 8> bounding_box;
bounding_box << mn(0), mn(0), mn(0), mn(0), mx(0), mx(0), mx(0), mx(0), mn(1),
mn(1), mx(1), mx(1), mn(1), mn(1), mx(1), mx(1), mn(2), mx(2), mn(2),
mx(2), mn(2), mx(2), mn(2), mx(2);
// rotate and translate bounding box back to original position
Eigen::Matrix3d rot_back = real_eigenvectors;
Eigen::Translation<double, 3> tra_back(mean_vertices);
Eigen::Transform<double, 3, Eigen::Affine> t = tra_back * rot_back;
bounding_box = t * bounding_box;
// convert to float
Eigen::Map<Eigen::MatrixXf> bounding_box_f(_bounding_box.data(), 3, 8);
bounding_box_f = bounding_box.cast<float>();
}
//U saves the deflation space vectors. H=U^dag*A*U, invH=inv(H), def_len is the number of deflation space vectors
//set restart=0 will never restart, or else it will restart once when relative residule < restart
//return number of iterations to converge
//V returns the calculated eig vectors(length m, only use the first 2*nev), and M are the eigen values(length 2*nev). When we do deflation, we should only pick those has small eigen values to U
int DiracOp::InvEigCg(Vector *sol, Vector *src, Float *true_res, const int nev, const int m, Vector **V, const int vec_len, Float *M, float **U, Rcomplex *invH, const int def_len, const Float *restart, const int restart_len)
{
char *fname = "InvEigCg(V*,V*,F,F*)";
VRB.Func(cname,fname);
if(nev>0 && m<=2*nev)ERR.General(cname,fname,"m should larger than 2*nev\n");
int f_size_cb; // Node checkerboard size of the fermion field
// Set the node checkerboard size of the fermion field
//------------------------------------------------------------------
if(lat.Fclass() == F_CLASS_CLOVER) {
f_size_cb = GJP.VolNodeSites() * lat.FsiteSize() / 2;
} else {
f_size_cb = GJP.VolNodeSites() * lat.FsiteSize() / (lat.FchkbEvl()+1);
}
if(vec_len!=f_size_cb)ERR.General(cname,fname,"vector length V does not match the length of solution and src vectors!\n");
int iter=0; //Current number of CG iterations
int max_iter=dirac_arg->max_num_iter; //max iteration number
if (f_size_cb % GRAN != 0)ERR.General(cname,fname,"Field length %d is not a multiple of granularity %d\n", GRAN, f_size_cb);
//calculate source norm
Float src_norm_sq = src->NormSqNode(f_size_cb);
DiracOpGlbSum(&src_norm_sq);
VRB.Flow(cname,fname, "nev = %d, m= %d\n", nev, m);
VRB.Flow(cname,fname, "Deflation length = %d \n", def_len);
for(int i=0;i<restart_len;i++)VRB.Flow(cname,fname, "restart condition = %e\n", restart[i]);
Float stp_cnd = src_norm_sq * dirac_arg->stop_rsd * dirac_arg->stop_rsd;
VRB.Flow(cname,fname, "stp_cnd =%e\n", IFloat(stp_cnd));
// Allocate memory for the solution/residual field.
//------------------------------------------------------------------
IFloat *X = (IFloat *) fmalloc(cname,fname,"X",2*f_size_cb * sizeof(Float));
// Allocate memory for the direction vector dir.
//------------------------------------------------------------------
Vector *dir;
if(GJP.VolNodeSites() >4096)
dir = (Vector *) smalloc(cname,fname,"dir",f_size_cb * sizeof(Float));
else dir = (Vector *) fmalloc(cname,fname,"dir",f_size_cb * sizeof(Float));
// Allocate mem. for the result vector of matrix multiplication mmp.
//------------------------------------------------------------------
Vector *mmp;
if(GJP.VolNodeSites() >4096)
mmp = (Vector *) smalloc(cname,fname,"mmp",f_size_cb * sizeof(Float));
else mmp = (Vector *) fmalloc(cname,fname,"mmp",f_size_cb * sizeof(Float));
Vector *mmp_prev=NULL;
//eigCG part
if(nev>0)
{
mmp_prev = (Vector *) smalloc(cname,fname,"mmp",f_size_cb * sizeof(Float));
}
//eigCG end
Rcomplex *Ub=NULL,*invHUb=NULL;
if(def_len>0)
{
Ub=(Rcomplex *) fmalloc(cname,fname,"Ub",2*def_len*sizeof(Float));
invHUb=(Rcomplex *) fmalloc(cname,fname,"invHUb",2*def_len*sizeof(Float));
}
//initial solution guess
sol->VecZero(f_size_cb);
if(def_len>0)
{
//sol = U*invH*U^dag*src;
for(int ii=0;ii<def_len;ii++)
{
//in CPS, dot(a,b)=a^* * b
//Ub[ii]=U[ii]->CompDotProductGlbSum(src,f_size_cb);
//NOTICE!!! Should be improved to do a single glb_sum for all Ub
//use function CompDotProductNode, after loop, do glb_sum(Ub,2*def_len)
Float c_r, c_i;
compDotProduct<float, Float>(&c_r, &c_i, U[ii], (Float *)src,f_size_cb);
glb_sum_five(&c_r);
glb_sum_five(&c_i);
Ub[ii]=Complex(c_r,c_i);
}
int index=0;
for(int ii=0;ii<def_len;ii++)
{
invHUb[ii]=0.0;
for(int jj=0;jj<def_len;jj++)
invHUb[ii]+=invH[index++]*Ub[jj];
}
for(int ii=0;ii<def_len;ii++)
{
//sol->CTimesV1PlusV2(invHUb[ii],U[ii],sol,f_size_cb);
cTimesV1PlusV2<Float,float,Float>((Float *)sol, real(invHUb[ii]), imag(invHUb[ii]), U[ii],(Float *)sol,f_size_cb);
}
#ifdef TEST
for(int ii=0;ii<def_len;ii++)
{
Float xx=U[ii]->NormSqNode(f_size_cb);
DiracOpGlbSum(&xx);
std::cout<<"U[ii] norm = "<<xx<<std::endl;
}
std::cout<<"Ub vector"<<std::endl;
for(int i=0;i<def_len;i++)
std::cout<<Ub[i].real()<<'\t';
std::cout<<std::endl;
std::cout<<"inv(H) matrix:"<<std::endl;
for(int i=0;i<def_len;i++)
{
for(int j=0;j<def_len;j++)
std::cout<<invH[i*def_len+j].real()<<'\t';
std::cout<<std::endl;
}
Float xx=sol->NormSqNode(f_size_cb);
DiracOpGlbSum(&xx);
std::cout<<"inital guess norm = "<<xx<<std::endl;
#endif
}
//dir = res(part of X vector) = src - MatPcDagMatPc * sol
MatPcDagMatPc(mmp, sol);
dir->CopyVec(src, f_size_cb);
dir->VecMinusEquVec(mmp, f_size_cb);
//aux pointers
IFloat *Fsol = (IFloat*)sol;
IFloat *Fdir = (IFloat*)dir;
IFloat *Fmmp = (IFloat*)mmp;
// Interleave solution and residual
IFloat *Xptr = X;
for (int j=0; j<f_size_cb/GRAN;j++) {
for (int i=0; i<GRAN; i++) *Xptr++ = *(Fsol+j*GRAN+i); //initial solution
for (int i=0; i<GRAN; i++) *Xptr++ = *(Fdir+j*GRAN+i); //residule
}
Float res_norm_sq_prv,res_norm_sq_cur;
Float alpha,beta,pAp;
res_norm_sq_cur = dir->NormSqNode(f_size_cb);
DiracOpGlbSum(&res_norm_sq_cur);
VRB.Flow(cname,fname, "|res[initial]|^2 = %e\n", IFloat(res_norm_sq_cur));
sync();
//eigCG part
int i_eig=0;
alpha=1.0; //avoid 0/0 at the first eig iteration
beta=0.0;
Float alpha_old;
Float beta_old;
Float *T=NULL;
Float *Tt=NULL;
Float *Q=NULL;
Float *QZ=NULL;
Float *H=NULL;
Float *Y=NULL;
Float *EIG=NULL;
int *INDEX=NULL;
if(nev>0)
{
T=(Float *) fmalloc(cname,fname,"T", m*(m+1)/2*sizeof(Float));// mxm real symetric matrix, lower triangular only
Tt=(Float *) fmalloc(cname,fname,"T", m*(m+1)/2*sizeof(Float));// (m)x(m) real symetric matrix, lower triangular only
//T(i,j) = T[ i(i+1)/2+j]
//NOTICE!! T is actually very sparse matrix, so it can be speed up by a very large factor
for(int i=0;i<m*(m+1)/2;i++)T[i]=0.0;
Y=(Float *) fmalloc(cname,fname,"Y", m*m*sizeof(Float));//m*m real matrix, to store eigen vectors when needed
Q=(Float *) fmalloc(cname,fname,"Q", m*2*nev*sizeof(Float));//m*(2nev) real matrix
QZ=(Float *) fmalloc(cname,fname,"QZ", m*2*nev*sizeof(Float));//m*(2nev) real matrix
H=(Float *) fmalloc(cname,fname,"H", 2*nev*(2*nev+1)/2*sizeof(Float));//(2nev)*(2nev) real matrix, symetric, lower part
EIG=(Float *)fmalloc(cname,fname,"EIG",m*sizeof(Float));
INDEX=(int *)fmalloc(cname,fname,"INDEX",nev*sizeof(int));//this vector is not necessay if we have a good eig solver for nev low
}
//
//eigCG part end
int restarted=0;
Float *restartcond;
if(restart_len>0)
{
restartcond=(Float *)fmalloc(cname,fname,"restartcond",restart_len*sizeof(Float));
for(int i=0;i<restart_len;i++)restartcond[i]=restart[i]*restart[i]*src_norm_sq;
}
Float eigTotal=0.0;
Float eigProj=0.0;
Float defTime=0.0;
Float total_time=0.0;
total_time-=dclock();
Float linalg_flops = 0;
Float eigProj_flops = 0;
Float linalg_time=0;
CGflops=0;
for(iter=0;iter<max_iter;iter++)
{
//eigCG part
if(nev>0 && i_eig==m)mmp_prev->CopyVec(mmp,f_size_cb);
//eig CG end
res_norm_sq_prv = res_norm_sq_cur;
MatPcDagMatPc(mmp,dir,&pAp);
DiracOpGlbSum(&pAp);
if(pAp==0)break;
if(nev>0)
{
eigTotal-=dclock();
int nev2=2*nev;
//T(i_eig-1,i_eig-1)
if(i_eig!=0)T[(i_eig-1)*i_eig/2+i_eig-1]=1.0/alpha+beta_old/alpha_old;
if(i_eig==m)
{
//Yb need lowest nev eigen vector of T(m-1)
//Y need lowest nev eigen vector of T(m)
#ifdef TEST
std::cout<<" T matrix: "<<std::endl;
for(int i=0;i<m;i++)
{
for(int j=0;j<m;j++)
{
if(i>=j)std::cout<<T[i*(i+1)/2+j]<<'\t';
else std::cout<<T[j*(j+1)/2+i]<<'\t';
}
std::cout<<std::endl;
}
#endif
for(int i=0;i<m*(m-1)/2;i++)Tt[i]=T[i];
eigen_solver(Tt,Y,EIG,m-1);//NOTICE, this is NOT needed, only need to calculate the lowest nev, not all m >2*nev
//NOTICE, Y transpose is the eigen vectors.
min_eig_index(INDEX,nev,EIG,m-1);
//Q(nev:2nev-1)=Yb; with Yb last row zero
for(int i=nev;i<2*nev;i++)
{
//Y transpose is eigen vector
//for(int j=0;j<m-1;j++)Q[j*2*nev+i]=Y[j+INDEX[i-nev]*(m-1)];
//Y is eigen vector
for(int j=0;j<m-1;j++)Q[j*2*nev+i]=Y[j*(m-1)+INDEX[i-nev]];
Q[(m-1)*2*nev+i]=0.0;
}
for(int i=0;i<m*(m+1)/2;i++)Tt[i]=T[i];
eigen_solver(Tt,Y,EIG,m);//NOTICE, this is NOT needed, only need to calculate the lowest nev, not all m >2*nev
min_eig_index(INDEX,nev,EIG,m);
//Q(0:nev-1)=Yb;
for(int i=0;i<nev;i++)
{
//for(int j=0;j<m;j++)Q[j*2*nev+i]=Y[j+INDEX[i]*m];
//Y is eigen vector
for(int j=0;j<m;j++)Q[j*2*nev+i]=Y[j*m+INDEX[i]];
}
//Q=orth([Y,Yb]); with YB last row zero
//rank(Q) may be smaller than 2*nev. remove these
//should be optimized. maybe save row first for Q
int rank=nev;
for(int i=nev;i<2*nev;i++)
{
for(int j=0;j<rank;j++)
{
Float xy=0.0;
for(int k=0;k<m;k++)xy+=Q[k*2*nev+i]*Q[k*2*nev+j];
for(int k=0;k<m;k++)Q[k*2*nev+i]-=xy*Q[k*2*nev+j];
}
//normalize
Float xx=0.0;
for(int k=0;k<m;k++)xx+=Q[k*2*nev+i]*Q[k*2*nev+i];
if(xx>1e-16)
{
xx=1.0/sqrt(xx);
for(int k=0;k<m;k++)Q[k*2*nev+rank]=xx*Q[k*2*nev+i];
rank++;
}
}
VRB.Flow(cname,fname,"Rank of Q = %d\n",rank);
//H=Q' * T * Q
for(int i=0;i<rank;i++)
for(int j=0;j<=i;j++)
{
H[i*(i+1)/2+j]=0.0;
for(int l=0;l<m;l++)
for(int k=0;k<m;k++)
{
if(k<=l)H[i*(i+1)/2+j]+=Q[l*nev2+i]*T[l*(l+1)/2+k]*Q[k*nev2+j];
else H[i*(i+1)/2+j]+=Q[l*nev2+i]*T[k*(k+1)/2+l]*Q[k*nev2+j];
}
}
#ifdef TEST
std::cout<<"H matrix:"<<std::endl;
for(int i=0;i<rank;i++)
{
for(int j=0;j<rank;j++)
{
if(i>=j)std::cout<<H[i*(i+1)/2+j]<<'\t';
else std::cout<<H[j*(j+1)/2+i]<<'\t';
}
std::cout<<std::endl;
}
#endif
//[Z,M]=eig(H)
eigen_solver(H,Y,M,rank);
for(int i=rank;i<2*nev;i++)M[i]=0.0;//set M[i>=rank] to zero
//NOtice, Y transpose is eigenvectos.
for(int i=0;i<rank;i++)VRB.Flow(cname,fname,"eig %d : %e \n",i,M[i]);
//V=V*(Q*Z)
//1.QZ=Q*Z
//transpoze QZ here to speed up the later calculation
for(int j=0;j<rank;j++)
for(int i=0;i<m;i++)
{
QZ[i+m*j]=0.0;
//for(int k=0;k<rank;k++)QZ[i+m*j]+=Q[i*nev2+k]*Y[j*rank+k];
for(int k=0;k<rank;k++)QZ[i+m*j]+=Q[i*nev2+k]*Y[j+k*rank];
}
#ifdef TEST
std::cout<<"QZ matrix:"<<std::endl;
for(int i=0;i<m;i++)
{
for(int j=0;j<rank;j++)
{
std::cout<<QZ[i+j*m]<<'\t';
}
std::cout<<std::endl;
}
#endif
eigProj-=dclock();
//2.V=V*QZ need to be implement very efficiently
//. The way we save QZ is transpozed to column first
eigcg_vec_mult(V,m,QZ,rank,f_size_cb);
eigProj+=dclock();
eigProj_flops += 2*f_size_cb*m*rank;
//T=M
for(int i=0;i<m*(m+1)/2;i++)T[i]=0.0;
for(int i=0;i<rank;i++)T[i*(i+1)/2+i]=M[i];
i_eig=rank;
//w=mmp-beta*mmp_prev
mmp_prev->FTimesV1PlusV2(-beta,mmp_prev,mmp,f_size_cb);
//T(i_eig+1,1:i_eig)=w^dag * V/sqrt(rsq)
//T is symmetric and REAL !! TESTED
for(int ii=0;ii<i_eig;ii++)
{
//T(i_eig,ii)
T[i_eig*(i_eig+1)/2+ii]=mmp_prev->ReDotProductGlbSum(V[ii],f_size_cb); //again these global sum can be donce by once to speed up
T[i_eig*(i_eig+1)/2+ii]/=sqrt(res_norm_sq_cur);
}
}
else
{
if(i_eig!=0)
{
//T(i_eig,i_eig-1)
T[i_eig*(i_eig+1)/2+i_eig-1]=-sqrt(beta)/alpha;
}
}
//V[i_eig]=r/sqrt(rsq);
Float *vptr = (Float *)V[i_eig];
invcg_copy_rnorm(vptr, res_norm_sq_cur, X+GRAN, f_size_cb/GRAN);
i_eig++;
eigTotal+=dclock();
}
alpha_old=alpha; //eigCG part
alpha = -res_norm_sq_prv/pAp;
// res = - alpha * (MatPcDagMatPc * dir) + res;
// res_norm_sq_cur = res * res
//test
//Float test=((Vector *)X)->NormSqGlbSum(f_size_cb*2);
//VRB.Flow(cname,fname,"X norm=%e \n",test);
//test=mmp->NormSqGlbSum(f_size_cb);
//VRB.Flow(cname,fname,"mmp norm=%e \n",test);
linalg_time-=dclock();
invcg_r_norm(X+GRAN, &alpha, Fmmp, X+GRAN, f_size_cb/GRAN, &res_norm_sq_cur);
linalg_time+=dclock();
DiracOpGlbSum(&res_norm_sq_cur);
linalg_flops+=f_size_cb*4;
alpha = -alpha;
beta_old=beta; //eigCG part
beta = res_norm_sq_cur / res_norm_sq_prv;
//VRB.Flow(cname,fname,"a=%e, b=%e, pAp=%e \n",alpha,beta,pAp);
// sol = alpha * dir + sol;
// dir = beta * dir + res;
linalg_time-=dclock();
invcg_xp_update(X, Fdir, &alpha, &beta, Fdir, X, f_size_cb/GRAN);
linalg_time+=dclock();
linalg_flops+=f_size_cb*4;
//consider restarting the init-CG once
if(restarted<restart_len && def_len>0 && res_norm_sq_cur<restartcond[restarted])
{
defTime-=dclock();
restarted++;
VRB.Flow(cname,fname,"eigCG restarted at res_norm_sq_cur= %e\n",res_norm_sq_cur);
//reuse dir vector as the initial guess vector of A*e=r ,with dir=e=U*invH*U^dag*r
//reuse mmp for r in X
//use sol for x in X
Xptr = X;
for (int j=0; j<f_size_cb/GRAN; j++) {
for (int i=0; i<GRAN; i++) *(Fsol+j*GRAN+i)=*Xptr++; // solution
for (int i=0; i<GRAN; i++) *(Fmmp+j*GRAN+i)=*Xptr++; // residule
}
for(int ii=0;ii<def_len;ii++)
{
//Ub[ii]=U[ii]->CompDotProductGlbSum(mmp,f_size_cb);
//NOTICE!!! Should be improved to a single glb_sum for all Ub
Float c_r, c_i;
compDotProduct<float, Float>(&c_r, &c_i, U[ii], (Float *)mmp,f_size_cb);
glb_sum_five(&c_r);
glb_sum_five(&c_i);
Ub[ii]=Complex(c_r,c_i);
}
int index=0;
for(int ii=0;ii<def_len;ii++)
{
invHUb[ii]=0.0;
for(int jj=0;jj<def_len;jj++)
invHUb[ii]+=invH[index++]*Ub[jj];
}
dir->VecZero(f_size_cb);
for(int ii=0;ii<def_len;ii++)
{
//dir->CTimesV1PlusV2(invHUb[ii],U[ii],dir,f_size_cb);
cTimesV1PlusV2<Float,float,Float>((Float *)dir, real(invHUb[ii]), imag(invHUb[ii]), U[ii],(Float *)dir,f_size_cb);
}
sol->VecAddEquVec(dir,f_size_cb); //get new sol
//reuse the first half of X to save M^dag*M*e
Vector *PAe=(Vector *)X;
MatPcDagMatPc(PAe,dir);
mmp->VecMinusEquVec(PAe,f_size_cb); //get new res
res_norm_sq_cur = mmp->NormSqNode(f_size_cb);
DiracOpGlbSum(&res_norm_sq_cur);
dir->CopyVec(mmp,f_size_cb);
Xptr = X;
for (int j=0; j<f_size_cb/GRAN;j++) {
for (int i=0; i<GRAN; i++) *Xptr++ = *(Fsol+j*GRAN+i); //new initial solution
for (int i=0; i<GRAN; i++) *Xptr++ = *(Fmmp+j*GRAN+i); //new residule
}
defTime+=dclock();
}
VRB.Flow(cname,fname, "|res[%d]|^2 = %e\n", iter, IFloat(res_norm_sq_cur));
if(res_norm_sq_cur <= stp_cnd) break;
}
total_time+=dclock();
VRB.Result(cname,fname,"1. Time on CG : %e seconds in %e flops\n", total_time-eigTotal-defTime,((Float)CGflops+linalg_flops)/(total_time-eigTotal-defTime));
VRB.Result(cname,fname,"1.x CG linear algebra : %e flops / %e seconds = %e flops\n", linalg_flops, linalg_time, linalg_flops/linalg_time);
VRB.Result(cname,fname,"2. Total Time on eig part: %e seconds \n", eigTotal);
if(nev>0)VRB.Result(cname,fname,"2.x projection part of eig part : %e flops / %e seconds = %e flops\n", eigProj_flops, eigProj, eigProj_flops/eigProj);
if(def_len>0)VRB.Result(cname,fname,"3. deflation(restart) time : %e seconds\n", defTime);
if(iter == max_iter)
VRB.Warn(cname,fname, "CG reached max iterations = %d. |res|^2 = %e\n", iter+1, IFloat(res_norm_sq_cur) );
Xptr = X-GRAN;
for (int j=0; j<f_size_cb; j++) {
if (j%GRAN==0) Xptr += GRAN;
*(Fsol++) = *(Xptr++);
}
MatPcDagMatPc(mmp, sol);
dir->CopyVec(src, f_size_cb);
dir->VecMinusEquVec(mmp, f_size_cb);
res_norm_sq_cur = dir->NormSqNode(f_size_cb);
DiracOpGlbSum(&res_norm_sq_cur);
*true_res = res_norm_sq_cur/src_norm_sq;
*true_res = sqrt(*true_res);
VRB.Result(cname,fname, "True |res| / |src| = %e, iter = %d\n", IFloat(*true_res), iter);
// Free memory
sfree(cname,fname, "mmp", mmp);
sfree(cname,fname, "dir", dir);
sfree(cname,fname, "X", X);
if(def_len>0)
{
sfree(cname,fname, "Ub", Ub);
sfree(cname,fname, "invHUb", invHUb);
}
//eigCG part
if(nev>0)
{
sfree(cname,fname,"mmp_prev",mmp_prev);
}
//eigCG part end
if(restart_len>0)sfree(cname,fname,"restartcond",restartcond);
sync();
return iter;
}
void CaculateNorm::run()
{
const IndexType k = 36; //36;
int size = SampleSet::get_instance().size();
for ( int ii = 0 ; ii<size; ++ii){
Sample& smp = SampleSet::get_instance()[ii];
std::cout<< "caculate:"<< ii<<std::endl;
if( !smp.num_triangles()) //only have points
{
for ( IndexType i=0; i < smp.num_vertices(); i++ )
{
MatrixX3 k_nearest_verts(k, 3);
IndexType neighbours[k];
ScalarType dist[k];
smp.neighbours( i, k, neighbours, dist );
for ( IndexType j=0; j<k; j++ )
{
IndexType neighbour_idx = neighbours[j];
k_nearest_verts.row(j) << smp[neighbour_idx].x(), smp[neighbour_idx].y(), smp[neighbour_idx].z();
}
MatrixX3 vert_mean = k_nearest_verts.colwise().mean();
MatrixX3 Q(k, 3);
for ( IndexType j=0; j<k;j++)
{
Q.row(j) = k_nearest_verts.row(j) - vert_mean;
}
Matrix33 sigma = Q.transpose() * Q;
Eigen::EigenSolver<Matrix33> eigen_solver(sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
auto ev = eigen_solver.eigenvectors();
auto eval = eigen_solver.eigenvalues();
ScalarType tmp[3] = { eval(0).real(),eval(1).real(),eval(2).real() };
IndexType min_idx = std::min_element(tmp,tmp+3) - tmp;
NormalType nv;
nv(0) = (ev.col(min_idx))(0).real();
nv(1) = (ev.col(min_idx))(1).real();
nv(2) = (ev.col(min_idx))(2).real();
nv.normalize();
if ( (baseline_).dot(nv) < 0 )
{
nv = - nv;
}
smp[i].set_normal( nv );
}
}else
{ //has face
//auto& m_triangles = smp.triangle_array;
for ( IndexType i=0; i < smp.num_triangles(); ++i )
{
IndexType i_vetex1 = smp.getTriangle(i).get_i_vertex(0);
IndexType i_vetex2 = smp.getTriangle(i).get_i_vertex(1);
IndexType i_vetex3 = smp.getTriangle(i).get_i_vertex(2);
PointType vtx1(smp[i_vetex1].x(),smp[i_vetex1].y(),smp[i_vetex1].z());
PointType vtx2(smp[i_vetex2].x(),smp[i_vetex2].y(),smp[i_vetex2].z());
PointType vtx3(smp[i_vetex3].x(),smp[i_vetex3].y(),smp[i_vetex3].z());
PointType vector1 = vtx2 - vtx1;
PointType vector2 = vtx3 - vtx1;
vector1.normalize();
vector2.normalize();
PointType vector3 = vector1.cross(vector2); //get the normal of the triangle
vector3.normalize();
//Logger<<"vector1: "<<vector1(0)<<" "<<vector1(1)<<" "<<vector1(2)<<std::endl;
//Logger<<"vector2: "<<vector2(0)<<" "<<vector2(1)<<" "<<vector2(2)<<std::endl;
//Logger<<"vector3: "<<vector3(0)<<" "<<vector3(1)<<" "<<vector3(2)<<std::endl;
//assign the normal to all the vertex of the triangle
for(int x = 0 ; x<3;++x)
{
IndexType i_normal = smp.getTriangle(i).get_i_normal(x);
//Logger<<"norm: "<<smp[i_normal].nx()<<" "<<smp[i_normal].ny()<<" "<<smp[i_normal].nz()<<std::endl;
smp[i_normal].set_normal(
NormalType( smp[i_normal].nx() + vector3(0),smp[i_normal].ny() +vector3(1),smp[i_normal].nz()+vector3(2) ));
}
}
for ( IndexType i=0; i < smp.num_vertices(); i++ )
{
NormalType norm(smp[i].nx(),smp[i].ny(),smp[i].nz());
norm.normalize();
// Logger<<"norm: "<<norm(0)<<" "<<norm(1)<<" "<<norm(2)<<std::endl;
smp[i].set_normal(norm);
}
}
}
}
void GraphNodeCtr::pca_box_ctr()
{
IndexType boxid = 0;
for ( unordered_map<IndexType,set<IndexType>>::iterator iter=label_bucket.begin(); iter!=label_bucket.end();iter++ )
{
IndexType frame_label = iter->first;
set<IndexType>& members = iter->second;
IndexType frame = get_frame_from_key(frame_label);
IndexType k=members.size();
MatrixX3 coords(k,3);
Sample& smp = SampleSet::get_instance()[frame];
int j=0;
for ( set<IndexType>::iterator m_iter=members.begin(); m_iter!=members.end(); m_iter++,j++ )
{
coords.row(j)<<smp[*m_iter].x(),smp[*m_iter].y(),smp[*m_iter].z();
}
MatrixX3 vert_mean = coords.colwise().mean();
MatrixX3 Q(k, 3);
for ( IndexType j=0; j<k;j++)
{
Q.row(j) = coords.row(j) - vert_mean;
}
Matrix33 sigma = Q.transpose() * Q;
Eigen::EigenSolver<Matrix33> eigen_solver(sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
auto evec = eigen_solver.eigenvectors();
auto eval = eigen_solver.eigenvalues();
PCABox *new_space = allocator_.allocate<PCABox>();
box_bucket[frame_label] = new(new_space) PCABox;
Matrix33 evecMat;
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
evecMat(j,i) = (evec.col(i))(j).real();
}
}
MatrixX3 newCoor = Q * evecMat;
Matrix23 minmax;
minmax.setZero();
for (int i = 0;i<3;i++)
{
minmax(0,i) = newCoor.col(i).minCoeff();
minmax(1,i) = newCoor.col(i).maxCoeff();
}
PCABox *pbox = box_bucket[frame_label];
for (int i=0;i<3;i++)
{
pbox->center(i) = (vert_mean.row(0))(i);
}
pbox->minPoint = minmax.row(0);
pbox->maxPoint = minmax.row(1);
PointType dis = pbox->maxPoint - pbox->minPoint;
pbox->volume = dis(0,0) *dis(1,0)*dis(2.0);
pbox->diagLen = sqrt(dis(0,0)*dis(0,0) + dis(1,0)*dis(1,0) + dis(2,0)*dis(2,0));
pbox->vtxSize = k;
//Logger<<"boxes"<<boxid++<<endl;
//Logger<<"box volume"<<pbox->volume<<endl;
//Logger<<"box length"<<pbox->diagLen<<endl;
//Logger<<"size"<<members.size()<<endl;
}
}
int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
//Removes part_of_cloud but retain the original full_cloud
//extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest
extract.filter(*cloud_plane);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud(cloud_plane);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
//cloud.swap(cloud_plane);
/*
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(cloud_plane);
while (!viewer.wasStopped())
{
}
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis;
fitted_plane_norm[0] = coefficients->values[0];
fitted_plane_norm[1] = coefficients->values[1];
fitted_plane_norm[2] = coefficients->values[2];
xyaxis_plane_norm[0] = 0.0;
xyaxis_plane_norm[1] = 0.0;
xyaxis_plane_norm[2] = 1.0;
rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm);
float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm));
//float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm));
transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud_recentered, *cloud_xyz);
///////////////////////////////////////////////////////////////////
Eigen::Vector4f pcaCentroid;
pcl::compute3DCentroid(*cloud_xyz, pcaCentroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
std::cout << eigenVectorsPCA.size() << std::endl;
if(eigenVectorsPCA.size()!=9)
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform);
// Get the minimum and maximum points of the transformed cloud.
pcl::PointXYZ minPoint, maxPoint;
pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(cloud);
// viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud");
// viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2");
viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
/*
pcl::PCA< pcl::PointXYZ > pca;
pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud;
pcl::PointCloud< pcl::PointXYZ > proj;
pca.setInputCloud(cloud);
pca.project(*cloud, proj);
Eigen::Vector4f proj_min;
Eigen::Vector4f proj_max;
pcl::getMinMax3D(proj, proj_min, proj_max);
pcl::PointXYZ min;
pcl::PointXYZ max;
pca.reconstruct(proj_min, min);
pca.reconstruct(proj_max, max);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z);
*/
return (0);
}
