diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index cf176e4789df3e4c836133c119f723509536958e..58d21a5cf834cc18372a197b3bbbead9894e46f6 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -54,11 +54,12 @@ class SolverManager */ enum class CovarianceBlocksToBeComputed : std::size_t { - 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_TUCAN + ALL = 0, ///< All blocks and all cross-covariances + ALL_MARGINALS = 1, ///< All marginals + ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks + GAUSS = 3, ///< GAUSS: last frame P and V + GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T + GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T }; /** diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index dc197328b31004e230f2909d2f1e9ab686ca134b..36518dede3455597f2b6a925f40da8226e1d8209 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -231,87 +231,161 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ // - Last KF: PV // last KF - FrameBasePtr last_frame = wolf_problem_->getLastFrame(); - if (not last_frame) + FrameBasePtr frame = wolf_problem_->getLastFrame(); + if (not frame) return false; - // Error - if (not last_frame->hasStateBlock('P') or - not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or - not last_frame->hasStateBlock('V') or - not isStateBlockRegisteredDerived(last_frame->getStateBlock('V'))) + while (frame) { - WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P or V, 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('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')); - + if (frame->hasStateBlock('P') and + isStateBlockRegisteredDerived(frame->getStateBlock('P')) and + frame->hasStateBlock('V') and + isStateBlockRegisteredDerived(frame->getStateBlock('V'))) + { + break; + } + //else + WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one..."); + frame = frame->getPreviousFrame(); + } + if (not frame) + { + WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and V, returning..."); return false; } // only marginals of P and V - 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('V'), - last_frame->getStateBlock('V')); - double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('V')), - getAssociatedMemBlockPtr(last_frame->getStateBlock('V'))); + WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: " + "\nP @", frame->getStateBlock('P'), + "\nV @", frame->getStateBlock('V')); + state_block_pairs.emplace_back(frame->getStateBlock('P'), + frame->getStateBlock('P')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')), + getAssociatedMemBlockPtr(frame->getStateBlock('P'))); + state_block_pairs.emplace_back(frame->getStateBlock('V'), + frame->getStateBlock('V')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')), + getAssociatedMemBlockPtr(frame->getStateBlock('V'))); break; } case CovarianceBlocksToBeComputed::GAUSS_TUCAN: { + WOLF_INFO("SolverCeres::computeCovariances: GAUSS_TUCAN"); + // State blocks: // - Last KF: PV // last KF - FrameBasePtr last_frame = wolf_problem_->getLastFrame(); - if (not last_frame) + FrameBasePtr frame = wolf_problem_->getLastFrame(); + if (not 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'))) + StateBlockPtr sb_gnss_T; + while (frame) { - 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')); + // get capture gnss + for (CaptureBasePtr cap : frame->getCaptureList()) + if (cap->hasStateBlock('T')) + { + sb_gnss_T = cap->getStateBlock('T'); + break; + } + if (frame->hasStateBlock('P') and + isStateBlockRegisteredDerived(frame->getStateBlock('P')) and + frame->hasStateBlock('O') and + isStateBlockRegisteredDerived(frame->getStateBlock('O')) and + frame->hasStateBlock('V') and + isStateBlockRegisteredDerived(frame->getStateBlock('V')) and + sb_gnss_T and + isStateBlockRegisteredDerived(sb_gnss_T)) + { + break; + } + // else + WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one..."); + frame = frame->getPreviousFrame(); + } + if (not frame) + { + WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, O, V and CaptureGnss with T, returning..."); 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'))); + WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: " + "\nP @", frame->getStateBlock('P'), + "\nO @", frame->getStateBlock('O'), + "\nV @", frame->getStateBlock('V'), + "\nT @", sb_gnss_T); + state_block_pairs.emplace_back(frame->getStateBlock('P'), + frame->getStateBlock('P')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')), + getAssociatedMemBlockPtr(frame->getStateBlock('P'))); + state_block_pairs.emplace_back(frame->getStateBlock('O'), + frame->getStateBlock('O')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('O')), + getAssociatedMemBlockPtr(frame->getStateBlock('O'))); + state_block_pairs.emplace_back(frame->getStateBlock('V'), + frame->getStateBlock('V')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')), + getAssociatedMemBlockPtr(frame->getStateBlock('V'))); + state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T); + double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), + getAssociatedMemBlockPtr(sb_gnss_T)); + + break; + } + case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION: + { + // State blocks: + // - Last KF: PV + + // last KF + FrameBasePtr frame = wolf_problem_->getLastFrame(); + if (not frame) + return false; + + StateBlockPtr sb_gnss_T; + while (frame) + { + // get capture gnss + for (CaptureBasePtr cap : frame->getCaptureList()) + if (cap->hasStateBlock('T')) + { + sb_gnss_T = cap->getStateBlock('T'); + break; + } + + if (frame->hasStateBlock('P') and + isStateBlockRegisteredDerived(frame->getStateBlock('P')) and + sb_gnss_T and + isStateBlockRegisteredDerived(sb_gnss_T)) + { + break; + } + // else + WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one..."); + frame = frame->getPreviousFrame(); + } + if (not frame) + { + WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and CaptureGnss with T, returning..."); + return false; + } + + // only marginals of P and T + WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: " + "\nP @", frame->getStateBlock('P'), + "\nT @", sb_gnss_T); + state_block_pairs.emplace_back(frame->getStateBlock('P'), + frame->getStateBlock('P')); + double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')), + getAssociatedMemBlockPtr(frame->getStateBlock('P'))); + state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T); + double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), + getAssociatedMemBlockPtr(sb_gnss_T)); break; }