// determinant computed using LU-decomposition
// runtime: O(n^3)
static matrix_entry eff_det(matrix a) {
// make a copy of a so that the entries are not affected by lu_decomp
matrix *a_cpy = copy_matrix(a);
int row_exchanges = 0;
lu_decomp(a_cpy, &row_exchanges);
// a is now upper triangular with same determinant up to sign
matrix_entry result = alt(row_exchanges); // takes care of sign
for (int i=0; i<a_cpy->m; i++) {
result *= a_cpy->entries[i][i];
}
free_matrix(a_cpy);
return result;
}
bool qbd_compute_pi0(const MatrixXd & R,
const MatrixXd & B0,
const MatrixXd & A0,
RowVectorXd & pi0,
const qbd_parms & parms) throw (Exc) {
if(R.rows()!=R.cols())
EXC_PRINT("R had to be square");
if (!check_sizes(A0,R) || !check_sizes(A0,B0))
EXC_PRINT("A0, A1, A2 matrixes have to be square and equal size");
if ((R.minCoeff()<0)&&parms.verbose)
cerr<<"QBD_COMPUTE_PI0: Warning: R has negative coeeficients"<<endl;
SelfAdjointEigenSolver<MatrixXd> eigensolver(R);
if (eigensolver.info() != Success) {
if (parms.verbose)
cerr<<"QBD_COMPUTE_PI0: cannot compute eigenvalues of R"<<endl;
return false;
}
if ((ArrayXd(eigensolver.eigenvalues()).abs().maxCoeff()>1)&&parms.verbose)
cerr<<"QBD_COMPUTE_PI0: Warning: R has spectral radius greater than 1"<<endl;
int n = R.rows();
MatrixXd Id = MatrixXd::Identity(n,n);
VectorXd u(n);
u.setOnes();
MatrixXd M(n,n+1);
M.block(0,0,n,n)= B0+R*A0-Id;
M.block(0,n,n,1)= (Id-R).inverse()*u;
FullPivLU<MatrixXd> lu_decomp(M);
if(lu_decomp.rank()<n) {
if (parms.verbose)
cerr<<"QBD_COMPUTE_PI0: No unique solution"<<endl;
return false;
}
RowVectorXd work(n+1);
work.setZero();
work(n)=1;
MatrixXd W1;
pseudoInverse<MatrixXd>(M,W1);
pi0 = work*W1;
if ((pi0.minCoeff()<0)&&parms.verbose)
cerr<<"QBD_COMPUTE_PI0: Warning: x0 has negative elements"<<endl;
return true;
}
LocalMatrix chemReductionGIA::orthcomp( LocalMatrix & inMat )
{
// initialize it so that they have the same dimension
LocalMatrix outMat;
Eigen::FullPivLU<LocalMatrix> lu_decomp(inMat.transpose());
outMat = lu_decomp.kernel();
// notice that if the kernel returns a matrix with dimension zero,
// then the returned matrix will be a column-vector filled with zeros
// therefore we do a safety check here, and set the number of columns to zero
if ( outMat.cols() == 1 && outMat.col(0).norm() == 0.0 )
outMat = LocalMatrix::Zero(inMat.rows(), 0);
return outMat;
}
// runtime: O(n^3)
matrix *invert_matrix(matrix a) {
assert(a.m == a.n); // only defined for square matrices
// make a copy of a so as not to modify it
matrix *u = copy_matrix(a);
// perform lu decomposition
matrix **pl = lu_decomp(u, NULL);
matrix *u_inv = invert_upper_tri_matrix(*u);
matrix *l_inv = invert_lower_tri_matrix(*pl[1]);
matrix *lu_inv = matrix_mult(*u_inv, *l_inv);
matrix *result = matrix_mult(*lu_inv, *pl[0]);
free_matrix(u);
free_matrix(pl[0]);
free_matrix(pl[1]);
free_matrix(u_inv);
free_matrix(l_inv);
free_matrix(lu_inv);
free(pl);
return result;
}
/**
* @function testPPP
*/
void Tensor3D::testPPP() {
Eigen::MatrixXd stacked = Eigen::MatrixXd::Zero( 9*mPPP.size(), 27);
for( int i = 0; i < mPPP.size(); ++i ) {
Eigen::MatrixXd block;
block = getEq_PPP( mPPP[i] );
// Check rank
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(block);
int rank = lu_decomp.rank();
printf("Rank of block %d is %d \n", i, rank);
stacked.block( 9*i,0, 9,27 ) = block;
// Stack
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp2(stacked);
int rank2 = lu_decomp2.rank();
printf("Rank of blocks stacked so far (until %d) is %d \n", i, rank2 );
}
}
bool Line::GeometryLineInter(Vector3f x1, Vector3f x2, Vector3f y1, Vector3f y2)
{
int i = 0;
Matrix3f k;
if ((x1 - y1).norm() < MINFLOATDIF)
{
return false;
}
if ((x1 - y2).norm() < MINFLOATDIF)
{
return false;
}
if ((x2 - y1).norm() < MINFLOATDIF)
{
return false;
}
if ((x2 - y2).norm() < MINFLOATDIF)
{
return false;
}
for (i = 0; i < 3; i++)
{
k(i, 0) = x1(i) - x2(i);
k(i, 1) = -(y1(i) - y2(i));
k(i, 2) = x2(i) - y2(i);
}
FullPivLU<Matrix3f> lu_decomp(k);
auto rank = lu_decomp.rank();
if (rank == 3)
{
return false;
}
else
{
return true;
}
}
void DIIS::update_diis_coefficients(){
/**
General case - holds true for both N_diis<N_diis_max and for N_diis==N_diis_max (after preliminary corrections )
Starting at this point we compute the extrapolation coefficients:
Solving Ax = b, where b - are the errors, x - are the changes of the parameter space, A contain the extrapolation coefficients
*/
int N_diis_curr;
int rank;
int i,j;
int debug_flag = 0;
double diis_damp = 0.0; // see [Hamilton,Pulay, JCP 84, 5728 (1986) ] - scale diagonal element of B matrix by (1+diis_damp) to
// avoid numerical problems associated with large diis coefficents
MatrixXd* A; // will be of size N_diis+2
VectorXd* b; //(N_diis+2);
VectorXd* x; //(N_diis+2);
if(debug_flag){ cout<<"*diis_err[N_diis]= "<<*diis_err[N_diis]<<endl; }
// Actual size of DIIS matrices at given call
// We save this number, because N_diis can reduce in the following run, to give full-rank matrices
N_diis_curr = N_diis;
// Initial guess
A = new MatrixXd(N_diis+2,N_diis+2);
for(i=0;i<=N_diis;i++){
for(j=0;j<=N_diis;j++){
(*A)(i,j) = ((*diis_err[i]).T() * (*diis_err[j])).tr();
if(i==j){ (*A)(i,j) *= (1.0+diis_damp); }
}
(*A)(i,N_diis+1) = -1.0;
(*A)(N_diis+1,i) = -1.0;
}
(*A)(N_diis+1,N_diis+1) = 0.0;
// Determine the rank of the DIIS matrix A
FullPivLU<MatrixXd> lu_decomp(*A);
rank = lu_decomp.rank();
if(rank==(N_diis+2)){ if(debug_flag){ cout<<"Matrix is well-conditioned\n"<<*A<<endl; } }
else{ if(debug_flag){ cout<<"Matrix is ill-conditioned\n"<<*A<<endl; }
int min_indx = 0;
// Reduce DIIS matrix by removing older iterates untill it is well-conditioned
while(rank!=(N_diis+2)){
N_diis--;
min_indx++; // this will hide first min_indx diis_err matrices from consideration
MatrixXd tempA(N_diis+2,N_diis+2);
for(i=0;i<=N_diis;i++){
for(j=0;j<=N_diis;j++){
tempA(i,j) = ((*diis_err[min_indx+i]).T() * (*diis_err[min_indx+j])).tr();
if(i==j){ tempA(i,j) *= (1.0+diis_damp); }
}
tempA(i,N_diis+1) = -1.0;
tempA(N_diis+1,i) = -1.0;
}
tempA(N_diis+1,N_diis+1) = 0.0;
FullPivLU<MatrixXd> tmp_lu_decomp(tempA);
rank = tmp_lu_decomp.rank();
if(debug_flag){
cout<<"Reduction iteration "<<min_indx<<endl;
cout<<"Reduced matrix = \n"<<tempA<<endl;
cout<<"Reduced matrix rank = "<<rank<<endl;
}
if(rank==N_diis+2){ A = new MatrixXd(N_diis+2,N_diis+2); *A = tempA; }
}// while
}// else rank!=Ndiis+2
b = new VectorXd(N_diis+2); for(i=0;i<=N_diis;i++){ (*b)(i) = 0.0; } (*b)(N_diis+1) = -1.0;
x = new VectorXd(N_diis+2);
// Solve linear algebra to get coefficients
*x = A->lu().solve(*b); for(i=0;i<=N_diis;i++){ diis_c[i] = (*x)[i]; }
if(debug_flag){
cout<<"A*x = "<<(*A)*(*x)<<endl;
cout<<"b = "<<*b<<endl;
cout<<"x = "<<*x<<endl;
}
// Free memory
delete A;
delete b;
delete x;
// Compute effective size of the DIIS matrix
N_diis_eff = (N_diis_curr - N_diis); // keep in mind that N_diis is a current one (updated)
N_diis = N_diis_curr;
}//void DIIS::update_diis_coefficients()
/* Return 1 if the matrix is singular, 0 if OK */
int
solve_se(
double **a, /* A[][] input matrix, returns LU decomposition of A */
double *b, /* B[] input array, returns solution X[] */
int n /* Dimensionality */
) {
double rip; /* Row interchange parity */
int *pivx, PIVX[10];
#if defined(DO_POLISH) || defined(DO_CHECK)
double **sa; /* save input matrix values */
double *sb; /* save input vector values */
#endif
if (n <= 10)
pivx = PIVX;
else
pivx = ivector(0, n-1);
#if defined(DO_POLISH) || defined(DO_CHECK)
sa = dmatrix(0, n-1, 0, n-1);
sb = dvector(0, n-1);
/* Copy input matrix and vector values */
for (i = 0; i < n; i++) {
sb[i] = b[i];
for (j = 0; j < n; j++)
sa[i][j] = a[i][j];
}
#endif
if (lu_decomp(a, n, pivx, &rip)) {
#if defined(DO_POLISH) || defined(DO_CHECK)
free_dvector(sb, 0, n-1);
free_dmatrix(sa, 0, n-1, 0, n-1);
if (pivx != PIVX)
free_ivector(pivx, 0, n-1);
#endif
return 1;
}
lu_backsub(a, n, pivx, b);
#ifdef DO_POLISH
lu_polish(n, sa, a, pivx, sb, b); /* Improve the solution */
#endif
#ifdef DO_CHECK
/* Check that the solution is correct */
for (i = 0; i < n; i++) {
double sum, temp;
sum = 0.0;
for (j = 0; j < n; j++)
sum += sa[i][j] * b[j];
temp = fabs(sum - sb[i]);
if (temp > 1e-6) {
free_dvector(sb, 0, n-1);
free_dmatrix(sa, 0, n-1, 0, n-1);
if (pivx != PIVX)
free_ivector(pivx, 0, n-1);
return 2;
}
}
#endif
#if defined(DO_POLISH) || defined(DO_CHECK)
free_dvector(sb, 0, n-1);
free_dmatrix(sa, 0, n-1, 0, n-1);
#endif
if (pivx != PIVX)
free_ivector(pivx, 0, n-1);
return 0;
}
void marginalize(
std::vector<aslam::backend::DesignVariable*>& inDesignVariables,
std::vector<aslam::backend::ErrorTerm*>& inErrorTerms,
int numberOfInputDesignVariablesToRemove,
bool useMEstimator,
boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm>& outPriorErrorTermPtr,
Eigen::MatrixXd& outCov,
std::vector<aslam::backend::DesignVariable*>& outDesignVariablesInRTop,
size_t numTopRowsInCov,
size_t /* numThreads */)
{
SM_WARN_STREAM_COND(inDesignVariables.size() == 0, "Zero input design variables in the marginalizer!");
// check for duplicates!
std::unordered_set<aslam::backend::DesignVariable*> inDvSetHT;
for(auto it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
{
auto ret = inDvSetHT.insert(*it);
SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate design variables in input list!");
}
std::unordered_set<aslam::backend::ErrorTerm*> inEtSetHT;
for(auto it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
auto ret = inEtSetHT.insert(*it);
SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate error term in input list!");
}
SM_DEBUG_STREAM("NO duplicates in input design variables or input error terms found.");
// Partition the design varibles into removed/remaining.
int dimOfDesignVariablesToRemove = 0;
std::vector<aslam::backend::DesignVariable*> remainingDesignVariables;
int k = 0;
size_t dimOfDvsInTopBlock = 0;
for(std::vector<aslam::backend::DesignVariable*>::const_iterator it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
{
if (k < numberOfInputDesignVariablesToRemove)
{
dimOfDesignVariablesToRemove += (*it)->minimalDimensions();
} else
{
remainingDesignVariables.push_back(*it);
}
if(dimOfDvsInTopBlock < numTopRowsInCov)
{
outDesignVariablesInRTop.push_back(*it);
}
dimOfDvsInTopBlock += (*it)->minimalDimensions();
k++;
}
// store original block indices to prevent side effects
std::vector<int> originalBlockIndices;
std::vector<int> originalColumnBase;
// assign block indices
int columnBase = 0;
for (size_t i = 0; i < inDesignVariables.size(); ++i) {
originalBlockIndices.push_back(inDesignVariables[i]->blockIndex());
originalColumnBase.push_back(inDesignVariables[i]->columnBase());
inDesignVariables[i]->setBlockIndex(i);
inDesignVariables[i]->setColumnBase(columnBase);
columnBase += inDesignVariables[i]->minimalDimensions();
}
int dim = 0;
std::vector<size_t> originalRowBase;
for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
originalRowBase.push_back((*it)->rowBase());
(*it)->setRowBase(dim);
dim += (*it)->dimension();
}
aslam::backend::DenseQrLinearSystemSolver qrSolver;
qrSolver.initMatrixStructure(inDesignVariables, inErrorTerms, false);
SM_INFO_STREAM("Marginalization optimization problem initialized with " << inDesignVariables.size() << " design variables and " << inErrorTerms.size() << " error terrms");
SM_INFO_STREAM("The Jacobian matrix is " << dim << " x " << columnBase);
qrSolver.evaluateError(1, useMEstimator);
qrSolver.buildSystem(1, useMEstimator);
const Eigen::MatrixXd& jacobian = qrSolver.getJacobian();
const Eigen::VectorXd& b = qrSolver.e();
// check dimension of jacobian
int jrows = jacobian.rows();
int jcols = jacobian.cols();
int dimOfRemainingDesignVariables = jcols - dimOfDesignVariablesToRemove;
//int dimOfPriorErrorTerm = jrows;
// check the rank
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(jacobian);
//lu_decomp.setThreshold(1e-20);
double threshold = lu_decomp.threshold();
int rank = lu_decomp.rank();
int fullRank = std::min(jacobian.rows(), jacobian.cols());
SM_DEBUG_STREAM("Rank of jacobian: " << rank << " (full rank: " << fullRank << ", threshold: " << threshold << ")");
bool rankDeficient = rank < fullRank;
if(rankDeficient)
{
SM_WARN("Marginalization jacobian is rank deficient!");
}
//SM_ASSERT_FALSE(aslam::Exception, rankDeficient, "Right now, we don't want the jacobian to be rank deficient - ever...");
Eigen::MatrixXd R_reduced;
Eigen::VectorXd d_reduced;
if (jrows < jcols)
{
SM_THROW(aslam::Exception, "underdetermined LSE!");
// underdetermined LSE, don't do QR
R_reduced = jacobian.block(0, dimOfDesignVariablesToRemove, jrows, jcols - dimOfDesignVariablesToRemove);
d_reduced = b;
} else {
// PTF: Do we know what will happen when the jacobian matrix is rank deficient?
// MB: yes, bad things!
// do QR decomposition
sm::timing::Timer myTimer("QR Decomposition");
Eigen::HouseholderQR<Eigen::MatrixXd> qr(jacobian);
Eigen::MatrixXd Q = qr.householderQ();
Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
Eigen::VectorXd d = Q.transpose()*b;
myTimer.stop();
if(numTopRowsInCov > 0)
{
sm::timing::Timer myTimer("Covariance computation");
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(R);
Eigen::MatrixXd Rinv = lu_decomp.inverse();
Eigen::MatrixXd covariance = Rinv * Rinv.transpose();
outCov = covariance.block(0, 0, numTopRowsInCov, numTopRowsInCov);
myTimer.stop();
}
// size_t numRowsToKeep = rank - dimOfDesignVariablesToRemove;
// SM_ASSERT_TRUE_DBG(aslam::Exception, rankDeficient || (numRowsToKeep == dimOfRemainingDesignVariables), "must be the same if full rank!");
// get the top left block
SM_ASSERT_GE(aslam::Exception, R.rows(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " rows of R because it only has " << R.rows() << " rows.");
SM_ASSERT_GE(aslam::Exception, R.cols(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " cols of R because it only has " << R.cols() << " cols.");
//outRtop = R.block(0, 0, numTopRowsInRtop, numTopRowsInRtop);
// cut off the zero rows at the bottom
R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables, dimOfRemainingDesignVariables);
//R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, numRowsToKeep, dimOfRemainingDesignVariables);
d_reduced = d.segment(dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables);
//d_reduced = d.segment(dimOfDesignVariablesToRemove, numRowsToKeep);
//dimOfPriorErrorTerm = dimOfRemainingDesignVariables;
}
// now create the new error term
boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm> err(new aslam::backend::MarginalizationPriorErrorTerm(remainingDesignVariables, d_reduced, R_reduced));
outPriorErrorTermPtr.swap(err);
// restore initial block indices to prevent side effects
for (size_t i = 0; i < inDesignVariables.size(); ++i) {
inDesignVariables[i]->setBlockIndex(originalBlockIndices[i]);
inDesignVariables[i]->setColumnBase(originalColumnBase[i]);
}
int index = 0;
for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
(*it)->setRowBase(originalRowBase[index++]);
}
}
