diff --git a/src/pruning/init_odb.h b/src/pruning/init_odb.h index 439fd0fa3337accca71f6bbb16626f411dd2af57..5fe3321499ffdafb126a58b6f242a379beec1d91 100644 --- a/src/pruning/init_odb.h +++ b/src/pruning/init_odb.h @@ -8,65 +8,113 @@ #ifndef SRC_PRUNING_INIT_ODB_H_ #define SRC_PRUNING_INIT_ODB_H_ +void makePSD(Eigen::MatrixXs& M) +{ + // impose symmetry + Eigen::MatrixXs m = M.selfadjointView<Eigen::Upper>(); + + // NaN + Scalar epsilon = Constants::EPS; + while (m.hasNaN() && epsilon < 1e6) + { + WOLF_WARN("matrix containing NaN, trying to fix it adding Identity matrix"); + std::cout << m << std::endl; + m = m + Eigen::MatrixXs::Identity(m.rows(),m.cols()) * epsilon; + epsilon *=10; + } + assert(!m.hasNaN() && "couldn't fix it..."); + while (!m.allFinite()) + { + WOLF_WARN("matrix containing Inf, trying to fix it"); + m = m.cwiseMin(1e12); + } + assert(m.allFinite() && "couldn't fix it..."); + + // eigen decomposition + Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> eigensolver(m); + + if (eigensolver.info() == Eigen::Success) + { + // All eigenvalues must be >= 0: + epsilon = Constants::EPS; + while ((eigensolver.eigenvalues().array() < Constants::EPS).any() || m.determinant() <= 0) + { + std::cout << "----- any negative eigenvalue or too close to zero\n"; + std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl; + std::cout << "previous determinant: " << m.determinant() << std::endl; + m = eigensolver.eigenvectors() * + eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() * + eigensolver.eigenvectors().transpose(); + eigensolver.compute(m); + std::cout << "epsilon used: " << epsilon << std::endl; + std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl; + std::cout << "posterior determinant: " << m.determinant() << std::endl; + epsilon *=10; + } + } + else + { + std::string info; + if (eigensolver.info() == Eigen::NumericalIssue) + info = "numerical issue"; + else if (eigensolver.info() == Eigen::NoConvergence) + info = "No Convergence"; + else if (eigensolver.info() == Eigen::InvalidInput) + info = "Invalid Input"; + else + info = "unknown"; + + WOLF_ERROR("Couldn't compute covariance eigen decomposition: ", info); + } + + WOLF_ASSERT_COVARIANCE_MATRIX(m); + + // impose symmetry + M = m.selfadjointView<Eigen::Upper>(); +} + Eigen::MatrixXs computeClosestSymPSD(const Eigen::MatrixXs& A) { assert(A.rows() == A.cols() && "matrix must be square!"); + if (A.hasNaN()) + { + WOLF_WARN("matrix A containing NaN"); + std::cout << A << std::endl; + } + // symmetrize A into B Eigen::MatrixXs B = (A + A.transpose()) / 2; + if (B.hasNaN()) + { + WOLF_WARN("matrix B containing NaN"); + std::cout << B << std::endl; + } // Compute the symmetric polar factor of B. Call it H (Clearly H is itself SPD) //[U,Sigma,V] = svd(B); Eigen::JacobiSVD<Eigen::MatrixXs> svd(B, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::MatrixXs Sigma = svd.singularValues().asDiagonal(); Eigen::MatrixXs V = svd.matrixV(); Eigen::MatrixXs H = V*Sigma*V.transpose(); - + if (H.hasNaN()) + { + WOLF_WARN("matrix H containing NaN"); + std::cout << H << std::endl; + } //get Ahat in the above formula Eigen::MatrixXs Ahat = (B+H)/2; + if (Ahat.hasNaN()) + { + WOLF_WARN("matrix Ahat containing NaN"); + std::cout << Ahat << std::endl; + } // ensure symmetry - Eigen::MatrixXs Ahat_sym = (Ahat + Ahat.transpose())/2; + Eigen::MatrixXs Ahat_sym = Ahat.selfadjointView<Eigen::Upper>(); // Ensure pos semi def - Scalar epsilon = 1e-12; - if(!(Ahat_sym.determinant() > 0)) - { - Eigen::MatrixXs Ahat_sym_fixed(Ahat_sym); - Eigen::EigenSolver<Eigen::MatrixXs> es(Ahat_sym_fixed); - Eigen::VectorXs eval = es.eigenvalues().real(); - - //std::cout << "!! Closest sym PSD not PD det = " << Ahat_sym_fixed.determinant() << std::endl; - //std::cout << "Matrix:" << std::endl << Ahat_sym_fixed << std::endl; - //std::cout << "eval" << std::endl << eval.transpose() << std::endl; - - eval = eval.array().max(epsilon).matrix(); - Ahat_sym_fixed = es.eigenvectors().real() * eval.asDiagonal() * es.eigenvectors().real().transpose(); - - //std::cout << "eval pos def" << std::endl << eval.transpose() << std::endl; - //std::cout << "Fixed Matrix:" << std::endl << Ahat_sym << std::endl; - - if (Ahat_sym_fixed.hasNaN()) - Ahat_sym_fixed = Ahat_sym + Eigen::MatrixXs::Identity(Ahat_sym.rows(),Ahat_sym.cols()) * epsilon; - - while (!(Ahat_sym.determinant() > 0)) - { - //std::cout << "Still not PD Ahat_sym.determinant() = " << Ahat_sym.determinant() << std::endl; - epsilon *=2; - //eval = eval.array().max(epsilon).matrix(); - //Ahat_sym = es.eigenvectors().real() * eval.asDiagonal() * es.eigenvectors().real().inverse() + Eigen::MatrixXs::Identity(Ahat_sym.rows(),Ahat_sym.cols()) * epsilon; - Ahat_sym += Eigen::MatrixXs::Identity(Ahat_sym.rows(),Ahat_sym.cols()) * epsilon; - //std::cout << "FIXED Ahat_sym.determinant() = " << Ahat_sym.determinant() << std::endl; - - if (epsilon > 1) - { - std::cout << "FAILED TO find closes sym PSD\n"; - break; - } - } - - } - assert(Ahat_sym.determinant() >= 0); + makePSD(Ahat_sym); return Ahat_sym; diff --git a/src/pruning/problem_pruning.cpp b/src/pruning/problem_pruning.cpp index 20b1e7c654dab32fbcd6cf60eb8887c3130e91a0..871675fece542fa13b465a0976a69727612839da 100644 --- a/src/pruning/problem_pruning.cpp +++ b/src/pruning/problem_pruning.cpp @@ -281,16 +281,15 @@ unsigned int computeStateBlockMap(const FrameBaseList& _fr_list, std::map<StateB //std::cout << "computeStateBlockMap:\n"; unsigned int size = 0; for (auto fr_ptr : _fr_list) - if (!fr_ptr->isFixed()) - { - fr_2_col_[fr_ptr] = size; - for (auto sb_ptr : fr_ptr->getStateBlockVec()) - if (sb_ptr) - { - sb_2_col_[sb_ptr] = size; - size += sb_ptr->getSize(); - } - } + { + fr_2_col_[fr_ptr] = size; + for (auto sb_ptr : fr_ptr->getStateBlockVec()) + if (sb_ptr) + { + sb_2_col_[sb_ptr] = size; + size += sb_ptr->getSize(); + } + } return size; }