Skip to content
Snippets Groups Projects
Commit b9ad025c authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Fixed some issues about positive semi-definite matrices

parent 8b3c3fb2
No related branches found
No related tags found
No related merge requests found
Pipeline #2161 failed
...@@ -17,7 +17,7 @@ ENDIF() ...@@ -17,7 +17,7 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON) option(_WOLF_TRACE "Enable wolf tracing macro" ON)
option(BUILD_EXAMPLES "Build examples" OFF) option(BUILD_EXAMPLES "Build examples" OFF)
set(BUILD_TESTS true) set(BUILD_TESTS false)
# Does this has any other interest # Does this has any other interest
# but for the examples ? # but for the examples ?
......
...@@ -145,7 +145,7 @@ void FeatureBase::avoidSingularCovariance() ...@@ -145,7 +145,7 @@ void FeatureBase::avoidSingularCovariance()
{ {
// All eigenvalues must be >= 0: // All eigenvalues must be >= 0:
Scalar epsilon = Constants::EPS; Scalar epsilon = Constants::EPS;
while ((eigensolver.eigenvalues().array() < Constants::EPS).any()) while ((eigensolver.eigenvalues().array() < Constants::EPS).any() || measurement_covariance_.determinant() <= 0)
{ {
std::cout << "----- any negative eigenvalue or too close to zero\n"; std::cout << "----- any negative eigenvalue or too close to zero\n";
std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl; std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
......
...@@ -353,6 +353,8 @@ Eigen::MatrixXs factorDescent(const Eigen::MatrixXs& J, const Eigen::MatrixXs& S ...@@ -353,6 +353,8 @@ Eigen::MatrixXs factorDescent(const Eigen::MatrixXs& J, const Eigen::MatrixXs& S
t1 = clock(); t1 = clock();
// Update constraint // Update constraint
//std::cout << __FILE__ <<": " << __LINE__ << std::endl;
//WOLF_ASSERT_COVARIANCE_MATRIX(Omega_k);
ctr_pair_it->first->getFeaturePtr()->setMeasurementInformation(Omega_k); ctr_pair_it->first->getFeaturePtr()->setMeasurementInformation(Omega_k);
KLD = 0.5*((J.transpose()*Omega*J*Sigma).trace() - logdet(J.transpose()*Omega*J*Sigma, std::string("factorDescent"))-sigma_size); KLD = 0.5*((J.transpose()*Omega*J*Sigma).trace() - logdet(J.transpose()*Omega*J*Sigma, std::string("factorDescent"))-sigma_size);
...@@ -578,6 +580,8 @@ void topologyDescent(const Eigen::MatrixXs& J, const Eigen::MatrixXs& Sigma, con ...@@ -578,6 +580,8 @@ void topologyDescent(const Eigen::MatrixXs& J, const Eigen::MatrixXs& Sigma, con
assignSparseBlock(Omega_k,Omega,ctr_pair_it->second,ctr_pair_it->second); assignSparseBlock(Omega_k,Omega,ctr_pair_it->second,ctr_pair_it->second);
// Update constraint // Update constraint
//std::cout << __FILE__ <<": " << __LINE__ << std::endl;
//WOLF_ASSERT_COVARIANCE_MATRIX(Omega_k);
ctr_pair_it->first->getFeaturePtr()->setMeasurementInformation(Omega_k); ctr_pair_it->first->getFeaturePtr()->setMeasurementInformation(Omega_k);
} }
......
...@@ -415,7 +415,8 @@ Eigen::MatrixXs interiorPoint(const Eigen::MatrixXs& J, const Eigen::MatrixXs& S ...@@ -415,7 +415,8 @@ Eigen::MatrixXs interiorPoint(const Eigen::MatrixXs& J, const Eigen::MatrixXs& S
{ {
Eigen::MatrixXs Omega_k = Omega.block(pair.second,pair.second,pair.first->getSize(),pair.first->getSize()); Eigen::MatrixXs Omega_k = Omega.block(pair.second,pair.second,pair.first->getSize(),pair.first->getSize());
setSPD(Omega_k); setSPD(Omega_k);
assert(Omega_k.determinant() > 0); //std::cout << __FILE__ <<": " <<__LINE__ << std::endl;
WOLF_ASSERT_COVARIANCE_MATRIX(Omega_k);
pair.first->getFeaturePtr()->setMeasurementInformation(Omega_k); pair.first->getFeaturePtr()->setMeasurementInformation(Omega_k);
} }
......
...@@ -63,7 +63,7 @@ ProblemPruning::~ProblemPruning() ...@@ -63,7 +63,7 @@ ProblemPruning::~ProblemPruning()
std::cout << std::endl << "Failed to open the problem sizes file " << filepath << std::endl; std::cout << std::endl << "Failed to open the problem sizes file " << filepath << std::endl;
// FILL FILE // FILL FILE
//std::cout << "Writing file " << name_ << "_problem_sizes.txt" << std::endl; std::cout << "Writing problems file for " << name_ << ": " << filepath << std::endl;
// KLD // KLD
for (auto kld : KLDs_) for (auto kld : KLDs_)
...@@ -71,8 +71,16 @@ ProblemPruning::~ProblemPruning() ...@@ -71,8 +71,16 @@ ProblemPruning::~ProblemPruning()
problems_file_ << std::endl; problems_file_ << std::endl;
// RMSE // RMSE
for (auto rmse : RMSEs_) // for (auto rmse : RMSEs_)
problems_file_ << rmse << " "; // problems_file_ << rmse << " ";
// problems_file_ << std::endl;
// RMSEs
for (auto prmse : pRMSEs_)
problems_file_ << prmse << " ";
problems_file_ << std::endl;
for (auto ormse : oRMSEs_)
problems_file_ << ormse << " ";
problems_file_ << std::endl; problems_file_ << std::endl;
// MB sizes // MB sizes
...@@ -96,6 +104,8 @@ ProblemPruning::~ProblemPruning() ...@@ -96,6 +104,8 @@ ProblemPruning::~ProblemPruning()
// close original distributions file // close original distributions file
originals_file_.close(); originals_file_.close();
std::cout << name_ << " closing after writing all logfiles!\n";
} }
} }
...@@ -184,6 +194,9 @@ void computeJacobians(const ConstraintBaseList& _ctr_list, const std::map<StateB ...@@ -184,6 +194,9 @@ void computeJacobians(const ConstraintBaseList& _ctr_list, const std::map<StateB
} }
// restore info // restore info
setSPD(info);
//WOLF_ASSERT_COVARIANCE_MATRIX(info);
//std::cout << __FILE__ <<__LINE__ << std::endl;
ctr_ptr->getFeaturePtr()->setMeasurementInformation(info); ctr_ptr->getFeaturePtr()->setMeasurementInformation(info);
//std::cout << "J: \n" << Eigen::MatrixXs(J_) << std::endl; //std::cout << "J: \n" << Eigen::MatrixXs(J_) << std::endl;
...@@ -303,8 +316,16 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen: ...@@ -303,8 +316,16 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen:
assert(Lambda.rows() == _Sigma_bl.rows() && "Sigma and Lambda of different sizes!"); assert(Lambda.rows() == _Sigma_bl.rows() && "Sigma and Lambda of different sizes!");
Eigen::VectorXs error = getMean() - _mean_bl; Eigen::VectorXs error = getMean() - _mean_bl;
Scalar sqPosErrorNorm = 0;
Scalar sqOriErrorNorm = 0;
for (auto i = 2; i < error.size(); i+=3) for (auto i = 2; i < error.size(); i+=3)
{
// position
sqPosErrorNorm += error.segment(i-2,2).squaredNorm();
// orientation
error(i) = pi2pi(error(i)); error(i) = pi2pi(error(i));
sqOriErrorNorm += error(i)*error(i);
}
// KLD // KLD
Eigen::MatrixXs Lambda_Sigma = Lambda * _Sigma_bl; Eigen::MatrixXs Lambda_Sigma = Lambda * _Sigma_bl;
...@@ -320,7 +341,9 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen: ...@@ -320,7 +341,9 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen:
KLDs_.push_back(KLD); KLDs_.push_back(KLD);
// RMSE // RMSE
RMSEs_.push_back(std::sqrt( error.squaredNorm() / (_mean_bl.size() / 3))); //RMSEs_.push_back(std::sqrt( error.squaredNorm() / (_mean_bl.size() / 3)));
pRMSEs_.push_back(std::sqrt( sqPosErrorNorm / (_mean_bl.size() / 3)));
oRMSEs_.push_back(std::sqrt( sqOriErrorNorm / (_mean_bl.size() / 3)));
// N factors // N factors
ConstraintBaseList all_ctr; ConstraintBaseList all_ctr;
...@@ -332,8 +355,11 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen: ...@@ -332,8 +355,11 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen:
std::cout << std::setprecision(3); std::cout << std::setprecision(3);
std::cout << "KLD = " << KLD; std::cout << "KLD = " << KLD;
//std::cout << " (" << 0.5 * Lambda_Sigma.trace() - 0.5 * Scalar(_mean_bl.size()) << " & " << -0.5*logdet(Lambda_Sigma) << " & " << 0.5*_alpha*error.transpose() * Lambda * error << ")"; //std::cout << " (" << 0.5 * Lambda_Sigma.trace() - 0.5 * Scalar(_mean_bl.size()) << " & " << -0.5*logdet(Lambda_Sigma) << " & " << 0.5*_alpha*error.transpose() * Lambda * error << ")";
std::cout << "\tRMSE = " << RMSEs_.back(); //std::cout << "\tRMSE = " << RMSEs_.back();
std::cout << "\ttime= " << t_sparsification_; std::cout << "\tpRMSE = " << pRMSEs_.back();
std::cout << "\toRMSE = " << oRMSEs_.back();
std::cout << "\ttime sparsification = " << t_sparsification_;
std::cout << "\ttime topology = " << t_topology_;
std::cout << "\tN factors = " << all_ctr.size() << std::endl; std::cout << "\tN factors = " << all_ctr.size() << std::endl;
if (_print_level > 0) if (_print_level > 0)
{ {
...@@ -416,7 +442,13 @@ Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs, ...@@ -416,7 +442,13 @@ Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs,
break; break;
case ODB: case ODB:
for (auto ctr : new_ctrs) for (auto ctr : new_ctrs)
ctr->getFeaturePtr()->setMeasurementInformation(initODB(ctr,J,Lambda,new_ctr_2_row,fr_2_col)); {
Eigen::MatrixXs init_omega_k = initODB(ctr,J,Lambda,new_ctr_2_row,fr_2_col);
setSPD(init_omega_k);
//std::cout << __FILE__ <<__LINE__ << std::endl;
//WOLF_ASSERT_COVARIANCE_MATRIX(init_omega_k);
ctr->getFeaturePtr()->setMeasurementInformation(init_omega_k);
}
break; break;
case FFD: case FFD:
{ {
...@@ -448,6 +480,8 @@ Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs, ...@@ -448,6 +480,8 @@ Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs,
{ {
Eigen::MatrixXs Omega_k = Omega.block(new_ctr_2_row.at(ctr), new_ctr_2_row.at(ctr),ctr->getSize(), ctr->getSize()); Eigen::MatrixXs Omega_k = Omega.block(new_ctr_2_row.at(ctr), new_ctr_2_row.at(ctr),ctr->getSize(), ctr->getSize());
setSPD(Omega_k); setSPD(Omega_k);
//std::cout << __FILE__ <<__LINE__ << std::endl;
//WOLF_ASSERT_COVARIANCE_MATRIX(Omega_k);
ctr->getFeaturePtr()->setMeasurementInformation(Omega_k); ctr->getFeaturePtr()->setMeasurementInformation(Omega_k);
} }
return logs; return logs;
...@@ -787,7 +821,13 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket ...@@ -787,7 +821,13 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket
{ {
// init ODB // init ODB
for (auto ctr : topology) for (auto ctr : topology)
ctr->getFeaturePtr()->setMeasurementInformation(initODB(ctr,Jtop,Lambda,topology_ctr_2_row,fr_2_col)); {
Eigen::MatrixXs init_Omega_k = initODB(ctr,Jtop,Lambda,topology_ctr_2_row,fr_2_col);
setSPD(init_Omega_k);
//WOLF_ASSERT_COVARIANCE_MATRIX(init_Omega_k);
//std::cout << __FILE__ <<__LINE__ << std::endl;
ctr->getFeaturePtr()->setMeasurementInformation(init_Omega_k);
}
// Factor Descent // Factor Descent
Eigen::MatrixXs KLDmat = factorDescent(JtopU, iD, topology_ctr_2_row, optim_options, false); Eigen::MatrixXs KLDmat = factorDescent(JtopU, iD, topology_ctr_2_row, optim_options, false);
...@@ -989,7 +1029,9 @@ void ProblemPruning::sparsify() ...@@ -989,7 +1029,9 @@ void ProblemPruning::sparsify()
//std::cout << "det(inv_omega_k) = " << inv_omega_k.determinant() << std::endl << "inv_omega_k:\n" << inv_omega_k << std::endl; //std::cout << "det(inv_omega_k) = " << inv_omega_k.determinant() << std::endl << "inv_omega_k:\n" << inv_omega_k << std::endl;
// Set information matrix // Set information matrix
setSPD(inv_omega_k); //setSPD(inv_omega_k);
//WOLF_ASSERT_COVARIANCE_MATRIX(inv_omega_k);
//std::cout << __FILE__ <<": " << __LINE__ << std::endl;
ctr->getFeaturePtr()->setMeasurementCovariance(inv_omega_k); ctr->getFeaturePtr()->setMeasurementCovariance(inv_omega_k);
} }
} }
......
...@@ -59,32 +59,57 @@ Scalar logdet(const Eigen::MatrixXs& M, const std::string& text = std::string()) ...@@ -59,32 +59,57 @@ Scalar logdet(const Eigen::MatrixXs& M, const std::string& text = std::string())
bool setSPD(Eigen::MatrixXs& M) bool setSPD(Eigen::MatrixXs& M)
{ {
if (M.determinant() <= 0 || M.diagonal().minCoeff() <= 0) //es.eigenvalues().real().minCoeff() < 0) // if (M.determinant() <= 0 || M.diagonal().minCoeff() <= 0) //es.eigenvalues().real().minCoeff() < 0)
// {
// Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(M);
// Scalar epsilon = 1e-9;
// //std::cout << "Not PD M... " << std::endl;
// //std::cout << "eval" << std::endl << es.eigenvalues().real().transpose() << std::endl;
// //std::cout << "eval pos def" << std::endl << es.eigenvalues().array().max(1e-6).matrix().transpose() << std::endl;
// M = es.eigenvectors() * es.eigenvalues().array().max(epsilon).matrix().asDiagonal() * es.eigenvectors().transpose();
//
// while (!(M.determinant() > 0))
// {
// epsilon *= 10;
// std::cout << "setSPD ...trying again with epsilon " << epsilon << std::endl;
// M = es.eigenvectors() * es.eigenvalues().array().max(epsilon).matrix().asDiagonal() * es.eigenvectors().transpose();
// M = M.selfadjointView<Eigen::Upper>();
// //M += 1e-9*Eigen::MatrixXs::Identity(M.rows(),M.rows());
// if (epsilon > 10)
// std::cout << "impossible to convert posdef! \neigenvectors:\n" << es.eigenvectors() << "\neigenvalues: " << es.eigenvalues().transpose() << std::endl;
// }
// //std::cout << "fixed determinant " << M.determinant() << " condition: !(M.determinant() > 0) " << !(M.determinant() > 0) << std::endl;
// assert(M.determinant() > 0 && "M negative determinant after fixing eigenvalues");
//
// return true;
// }
// else
// return false;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> eigensolver(M);
if (eigensolver.info() == Eigen::Success)
{ {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(M); // All eigenvalues must be >= 0:
Scalar epsilon = 1e-9; Scalar epsilon = Constants::EPS;
//std::cout << "Not PD M... " << std::endl; while ((eigensolver.eigenvalues().array() < Constants::EPS).any() || M.determinant() <= 0)
//std::cout << "eval" << std::endl << es.eigenvalues().real().transpose() << std::endl;
//std::cout << "eval pos def" << std::endl << es.eigenvalues().array().max(1e-6).matrix().transpose() << std::endl;
M = es.eigenvectors() * es.eigenvalues().array().max(epsilon).matrix().asDiagonal() * es.eigenvectors().transpose();
while (!(M.determinant() > 0))
{ {
epsilon *= 10; //std::cout << "----- any negative eigenvalue or too close to zero\n";
std::cout << "setSPD ...trying again with epsilon " << epsilon << std::endl; //std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
M = es.eigenvectors() * es.eigenvalues().array().max(epsilon).matrix().asDiagonal() * es.eigenvectors().transpose(); //std::cout << "previous determinant: " << M.determinant() << std::endl;
M = M.selfadjointView<Eigen::Upper>(); M= eigensolver.eigenvectors() *
//M += 1e-9*Eigen::MatrixXs::Identity(M.rows(),M.rows()); eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
if (epsilon > 10) eigensolver.eigenvectors().transpose();
std::cout << "impossible to convert posdef! \neigenvectors:\n" << es.eigenvectors() << "\neigenvalues: " << es.eigenvalues().transpose() << std::endl; 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;
} }
//std::cout << "fixed determinant " << M.determinant() << " condition: !(M.determinant() > 0) " << !(M.determinant() > 0) << std::endl;
assert(M.determinant() > 0 && "M negative determinant after fixing eigenvalues");
return true;
} }
else
return false; return true;
} }
Eigen::MatrixXs thresholdZero(const Eigen::MatrixXs& M) Eigen::MatrixXs thresholdZero(const Eigen::MatrixXs& M)
...@@ -251,7 +276,9 @@ class ProblemPruning : public Problem ...@@ -251,7 +276,9 @@ class ProblemPruning : public Problem
std::ofstream originals_file_; std::ofstream originals_file_;
std::vector<Scalar> MB_sizes_; std::vector<Scalar> MB_sizes_;
std::vector<Scalar> KLDs_; std::vector<Scalar> KLDs_;
std::vector<Scalar> RMSEs_; //std::vector<Scalar> RMSEs_;
std::vector<Scalar> pRMSEs_;
std::vector<Scalar> oRMSEs_;
private: private:
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment