diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1d38ec3e25e9085bcfda78251a61cddf05730f1f..30f9879f8b17bee7ec5ecf74a615d96b812f3b1f 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -17,7 +17,7 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
option(BUILD_EXAMPLES "Build examples" OFF)
-set(BUILD_TESTS true)
+set(BUILD_TESTS false)
# Does this has any other interest
# but for the examples ?
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 09e9c0c79702bb8081c38ee3996f059192c9acec..5387956af7babd1b75b053f98d540dc5ffdca29f 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -145,7 +145,7 @@ void FeatureBase::avoidSingularCovariance()
{
// All eigenvalues must be >= 0:
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 << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
diff --git a/src/pruning/factor_descent.h b/src/pruning/factor_descent.h
index 547deec680c3a257302526adc5f98bdb1911d886..e7c0167d02abc231422ee2623f2b2c361682414e 100644
--- a/src/pruning/factor_descent.h
+++ b/src/pruning/factor_descent.h
@@ -353,6 +353,8 @@ Eigen::MatrixXs factorDescent(const Eigen::MatrixXs& J, const Eigen::MatrixXs& S
t1 = clock();
// Update constraint
+ //std::cout << __FILE__ <<": " << __LINE__ << std::endl;
+ //WOLF_ASSERT_COVARIANCE_MATRIX(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);
@@ -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);
// Update constraint
+ //std::cout << __FILE__ <<": " << __LINE__ << std::endl;
+ //WOLF_ASSERT_COVARIANCE_MATRIX(Omega_k);
ctr_pair_it->first->getFeaturePtr()->setMeasurementInformation(Omega_k);
}
diff --git a/src/pruning/interior_point.h b/src/pruning/interior_point.h
index 3e3dede531b91ec1f42d8137f131195f9717c9ab..e901bf367d9297c0a0625f4e5d27cbd8f410fe47 100644
--- a/src/pruning/interior_point.h
+++ b/src/pruning/interior_point.h
@@ -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());
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);
}
diff --git a/src/pruning/problem_pruning.cpp b/src/pruning/problem_pruning.cpp
index 2fae21909cf1a9f745c8a79554e9b15777165a8a..20b1e7c654dab32fbcd6cf60eb8887c3130e91a0 100644
--- a/src/pruning/problem_pruning.cpp
+++ b/src/pruning/problem_pruning.cpp
@@ -63,7 +63,7 @@ ProblemPruning::~ProblemPruning()
std::cout << std::endl << "Failed to open the problem sizes file " << filepath << std::endl;
// FILL FILE
- //std::cout << "Writing file " << name_ << "_problem_sizes.txt" << std::endl;
+ std::cout << "Writing problems file for " << name_ << ": " << filepath << std::endl;
// KLD
for (auto kld : KLDs_)
@@ -71,8 +71,16 @@ ProblemPruning::~ProblemPruning()
problems_file_ << std::endl;
// RMSE
- for (auto rmse : RMSEs_)
- problems_file_ << rmse << " ";
+// for (auto rmse : RMSEs_)
+// 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;
// MB sizes
@@ -96,6 +104,8 @@ ProblemPruning::~ProblemPruning()
// close original distributions file
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
}
// restore info
+ setSPD(info);
+ //WOLF_ASSERT_COVARIANCE_MATRIX(info);
+ //std::cout << __FILE__ <<__LINE__ << std::endl;
ctr_ptr->getFeaturePtr()->setMeasurementInformation(info);
//std::cout << "J: \n" << Eigen::MatrixXs(J_) << std::endl;
@@ -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!");
Eigen::VectorXs error = getMean() - _mean_bl;
+ Scalar sqPosErrorNorm = 0;
+ Scalar sqOriErrorNorm = 0;
for (auto i = 2; i < error.size(); i+=3)
+ {
+ // position
+ sqPosErrorNorm += error.segment(i-2,2).squaredNorm();
+ // orientation
error(i) = pi2pi(error(i));
+ sqOriErrorNorm += error(i)*error(i);
+ }
// KLD
Eigen::MatrixXs Lambda_Sigma = Lambda * _Sigma_bl;
@@ -320,7 +341,9 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen:
KLDs_.push_back(KLD);
// 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
ConstraintBaseList all_ctr;
@@ -332,8 +355,11 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen:
std::cout << std::setprecision(3);
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 << "\tRMSE = " << RMSEs_.back();
- std::cout << "\ttime= " << t_sparsification_;
+ //std::cout << "\tRMSE = " << RMSEs_.back();
+ 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;
if (_print_level > 0)
{
@@ -416,7 +442,13 @@ Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs,
break;
case ODB:
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;
case FFD:
{
@@ -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());
setSPD(Omega_k);
+ //std::cout << __FILE__ <<__LINE__ << std::endl;
+ //WOLF_ASSERT_COVARIANCE_MATRIX(Omega_k);
ctr->getFeaturePtr()->setMeasurementInformation(Omega_k);
}
return logs;
@@ -787,7 +821,13 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket
{
// init ODB
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
Eigen::MatrixXs KLDmat = factorDescent(JtopU, iD, topology_ctr_2_row, optim_options, false);
@@ -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;
// 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);
}
}
diff --git a/src/pruning/problem_pruning.h b/src/pruning/problem_pruning.h
index 1fe6efd3fc8eb481f6f130ad8a840a49743fb5ca..650c12b636a090d0f1ec350b2da371b3ed334108 100644
--- a/src/pruning/problem_pruning.h
+++ b/src/pruning/problem_pruning.h
@@ -59,32 +59,57 @@ Scalar logdet(const Eigen::MatrixXs& M, const std::string& text = std::string())
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);
- 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))
+ // All eigenvalues must be >= 0:
+ Scalar epsilon = Constants::EPS;
+ while ((eigensolver.eigenvalues().array() < Constants::EPS).any() || 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 << "----- 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;
}
- //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)
@@ -251,7 +276,9 @@ class ProblemPruning : public Problem
std::ofstream originals_file_;
std::vector<Scalar> MB_sizes_;
std::vector<Scalar> KLDs_;
- std::vector<Scalar> RMSEs_;
+ //std::vector<Scalar> RMSEs_;
+ std::vector<Scalar> pRMSEs_;
+ std::vector<Scalar> oRMSEs_;
private: