std::vector<Matrix> Block::eigh(cflag tp)const{
std::vector<Matrix> outs;
try{
checkUni10TypeError(tp);
if(!(Rnum == Cnum)){
std::ostringstream err;
err<<"Cannot perform eigenvalue decomposition on a non-square matrix.";
throw std::runtime_error(exception_msg(err.str()));
}
if(diag){
std::ostringstream err;
err<<"Cannot perform eigenvalue decomposition on a diagonal matrix. Need not to do so.";
throw std::runtime_error(exception_msg(err.str()));
}
//GPU_NOT_READY
outs.push_back(Matrix(CTYPE, Rnum, Cnum, true, ongpu));
outs.push_back(Matrix(CTYPE, Rnum, Cnum, false, ongpu));
Matrix Eig(RTYPE, Rnum, Cnum, true, ongpu);
eigSyDecompose(cm_elem, Rnum, Eig.m_elem, outs[1].cm_elem, ongpu);
outs[0] = Eig;
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::eigh(uni10::cflag ):");
}
return outs;
}
void FormIntArr(MessRelaxClcPar *RelaxClc,MessRelaxInpPar &RelaxPar,
double H,double Qs,double Eta,double Is)
{
int N=8;//Mat.Dim();
CMatrCl Mat(N),TimeShiftLeft,TimeShiftRight;
CMatrCl Mat3,Mat3Eig,Mat3L,Mat3R, Mat1,Mat1Eig,Mat1L,Mat1R;
CMatrCl Uni3(4),Uni1(2);Uni3=Uni3*0+1;Uni1=Uni1*0+1;
TimeShiftLeft=Uni3;TimeShiftRight=Uni1;
CVecCl Int(N),Pos(N);
RelaxPar.Times[0]=0;
RelaxClc[0].EndTime=0;
for (int k=1;k<=RelaxPar.NumHam;k++)
{
QsHMat(Mat,RelaxPar.Teta[k],RelaxPar.Phi[k],H, -Qs, Eta, -Is,&Mat3,&Mat1);
RelaxClc[k].EndTime=RelaxPar.Times[k];
// CMatrCl AmpL,AmpR;
// AmpL=R0HQsAmpVec_();
// AmpR=TimeShiftRight*AmpL;AmpL=Conj(Transpon(Conj(Transpon(AmpL))*TimeShiftLeft));
CMatrCl Eig,Lft,Rgt;
Mat1=Transpon(Mat1);
GetVecIntTurn_(Int,Pos,Mat3,Mat1,TimeShiftLeft,TimeShiftRight,
&Mat3Eig,&Mat3L,&Mat3R,&Mat1Eig,&Mat1L,&Mat1R);
RelaxClc[k].RelInt=Int;
RelaxClc[k].Freq=Pos;
Eig=Mat3Eig;
for (int i=1;i<=Mat3.Dim();i++)
Eig(i,i)=exp(my_comp(0,-my_real(Mat3Eig(i,i))*
(RelaxPar.Times[k]-RelaxPar.Times[k-1])));
Mat3=Mat3L*Eig*Mat3R;
TimeShiftLeft= Mat3*TimeShiftLeft;
Eig=Mat1Eig;
for ( i=1;i<=Mat1.Dim();i++)
Eig(i,i)=exp(my_comp(0,my_real(Mat1Eig(i,i))*
(RelaxPar.Times[k]-RelaxPar.Times[k-1])));
Mat1=Mat1L*Eig*Mat1R;
TimeShiftRight=TimeShiftRight*Mat1;
}
};
bool SBVAR_symmetric::DrawParametersFromPrior(double *Parameters)
{
if (!flat_prior)
{
TDenseVector a0_aplus(n_vars+n_predetermined,0.0), x(n_vars+n_predetermined,0.0);
TDenseMatrix covariance(n_vars+n_predetermined, n_vars+n_predetermined,0.0);
covariance.Insert(0,0,prior_YY);
covariance.Insert(0,n_vars,-Transpose(prior_XY));
covariance.Insert(n_vars,0,-prior_XY);
covariance.Insert(n_vars,n_vars,prior_XX);
covariance = 0.5*(covariance+Transpose(covariance));
// eig analysis of covariance
TDenseVector EigValue(prior_YY.rows+prior_XX.rows,0.0);
TDenseMatrix EigVector(prior_YY.rows+prior_XX.rows, prior_YY.cols+prior_YY.cols, 0.0);
Eig(EigValue, EigVector, covariance);
for (int i=n_vars-1; i>=0; i--)
{
x.RandomNormal();
a0_aplus.Insert(0, EigVector*(DotMultiply(EigValue,x)));
A0.InsertRowMatrix(i,0,a0_aplus,0,n_vars-1);
Aplus.InsertRowMatrix(0,0,a0_aplus,n_vars,a0_aplus.dim-1);
}
GetParameters(Parameters);
return true;
}
else
{
TDenseVector a0_aplus(n_vars+n_predetermined,0.0);
for (int i=n_vars-1; i>=0; i--)
{
a0_aplus.RandomNormal();
A0.InsertRowMatrix(i,0,a0_aplus,0,n_vars-1);
Aplus.InsertRowMatrix(0,0,a0_aplus,n_vars,a0_aplus.dim-1);
}
GetParameters(Parameters);
return true;
}
}
void
opengv::absolute_pose::modules::gp3p_main(
const Eigen::Matrix3d & f,
const Eigen::Matrix3d & v,
const Eigen::Matrix3d & p,
transformations_t & solutions)
{
Eigen::Matrix<double,48,85> groebnerMatrix =
Eigen::Matrix<double,48,85>::Zero();
gp3p::init(groebnerMatrix,f,v,p);
gp3p::compute(groebnerMatrix);
Eigen::Matrix<double,8,8> M = Eigen::Matrix<double,8,8>::Zero();
M.block<6,8>(0,0) = -groebnerMatrix.block<6,8>(36,77);
M(6,0) = 1.0;
M(7,6) = 1.0;
Eigen::ComplexEigenSolver< Eigen::Matrix<double,8,8> > Eig(M,true);
Eigen::Matrix<std::complex<double>,8,1> D = Eig.eigenvalues();
Eigen::Matrix<std::complex<double>,8,8> V = Eig.eigenvectors();
for( int c = 0; c < V.cols(); c++ )
{
std::complex<double> eigValue = D[c];
if( eigValue.imag() < 0.0001 )
{
cayley_t cayley;
Eigen::Vector3d n;
for(size_t i = 0; i < 3; i++)
{
std::complex<double> cay = V(i+4,c)/V(7,c);
cayley[2-i] = cay.real();
std::complex<double> depth = V(i+1,c)/V(7,c);
n[2-i] = depth.real();
}
rotation_t rotation = math::cayley2rot(cayley);
//the groebner problem was set up to find the transpose!
rotation.transposeInPlace();
point_t center_cam = Eigen::Vector3d::Zero();
point_t center_world = Eigen::Vector3d::Zero();
for( size_t i = 0; i < (size_t) f.cols(); i++ )
{
point_t temp = rotation*(n[i]*f.col(i)+v.col(i));
center_cam = center_cam + temp;
center_world = center_world + p.col(i);
}
center_cam = center_cam/f.cols();
center_world = center_world/f.cols();
translation_t translation = center_world - center_cam;
transformation_t transformation;
transformation.block<3,3>(0,0) = rotation;
transformation.col(3) = translation;
solutions.push_back(transformation);
}
}
}