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); }