Skip to content
Snippets Groups Projects

Draft: Resolve "Implementation of new nodes creation"

Open Joan Vallvé Navarro requested to merge 454-implementation-of-new-nodes-creation into devel
1 file
+ 3
3
Compare changes
  • Side-by-side
  • Inline
@@ -224,7 +224,7 @@ int main()
// SOLVE with exact initial guess
WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(ReportVerbosity::FULL);
std::string report = ceres->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very low iteration number (possibly 1)
problem->print(1, 0, 1, 0);
@@ -235,13 +235,13 @@ int main()
// SOLVE again
WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(ReportVerbosity::FULL);
report = ceres->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1, 0, 1, 0);
// GET COVARIANCES of all states
WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(CovarianceBlocksToBeComputed::ALL_MARGINALS);
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
{
Eigen::MatrixXd cov;
Loading