diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index fb38a8be279f9ed6572c729fdebe1551bb99f4ae..cf176e4789df3e4c836133c119f723509536958e 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -57,7 +57,8 @@ class SolverManager ALL, ///< All blocks and all cross-covariances ALL_MARGINALS, ///< All marginals ROBOT_LANDMARKS, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks - GAUSS + GAUSS, + GAUSS_TUCAN }; /** diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index 7f33435f917a913eb4bf9cbf7b541634ac2a8e1c..fec58aee7d465d167d245d57db73a17d8e66dcca 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -264,6 +264,59 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ break; } + case CovarianceBlocksToBeComputed::GAUSS_TUCAN: + { + // State blocks: + // - Last KF: PV + + // last KF + FrameBasePtr last_frame = wolf_problem_->getLastFrame(); + if (not last_frame) + return false; + + // Error + if (not last_frame->hasStateBlock('P') or + not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or + not last_frame->hasStateBlock('O') or + not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')) or + not last_frame->hasStateBlock('V') or + not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')) or + not last_frame->hasStateBlock('T') or + not isStateBlockRegisteredDerived(last_frame->getStateBlock('T'))) + { + WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P, O, V or T, returning..."); + WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found"); + WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P')); + WOLF_WARN_COND(not last_frame->getStateBlock('O'), "SolverCeres::computeCovariances: KF state block 'O' not found"); + WOLF_WARN_COND(last_frame->getStateBlock('O') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'O' not registered ", last_frame->getStateBlock('O')); + WOLF_WARN_COND(not last_frame->getStateBlock('V'), "SolverCeres::computeCovariances: KF state block 'V' not found"); + WOLF_WARN_COND(last_frame->getStateBlock('V') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')), "SolverCeres::computeCovariances: KF state block 'V' not registered ", last_frame->getStateBlock('V')); + WOLF_WARN_COND(not last_frame->getStateBlock('T'), "SolverCeres::computeCovariances: KF state block 'T' not found"); + WOLF_WARN_COND(last_frame->getStateBlock('T') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('T')), "SolverCeres::computeCovariances: KF state block 'T' not registered ", last_frame->getStateBlock('T')); + + return false; + } + + // only marginals of P, O, V and T + state_block_pairs.emplace_back(last_frame->getStateBlock('P'), + last_frame->getStateBlock('P')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('P')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('P'))); + state_block_pairs.emplace_back(last_frame->getStateBlock('O'), + last_frame->getStateBlock('O')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('O')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('O'))); + state_block_pairs.emplace_back(last_frame->getStateBlock('V'), + last_frame->getStateBlock('V')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('V')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('V'))); + state_block_pairs.emplace_back(last_frame->getStateBlock('T'), + last_frame->getStateBlock('T')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('T')), + getAssociatedMemBlockPtr(last_frame->getStateBlock('T'))); + + break; + } default: throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");