diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index 92846f85b76e7b67286f35c257ce32727de6ccb8..a01aa40f1a8faee05b5f3ccfcbfe727278c5ab55 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -230,7 +230,7 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) case CovarianceBlocksToBeComputed::GAUSS: { // State blocks: - // - Last KF: PO + // - Last KF: POV // - CaptureGnss T std::vector<StateBlockPtr> gauss_state_blocks; @@ -254,6 +254,8 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) 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 sb_gnss_T or not isStateBlockRegisteredDerived(sb_gnss_T)) { @@ -262,6 +264,8 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) 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 sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found"); WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegisteredDerived(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T); @@ -271,9 +275,19 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) // Fill vector of state blocks gauss_state_blocks = std::vector<StateBlockPtr>{last_frame->getStateBlock('P'), last_frame->getStateBlock('O'), + last_frame->getStateBlock('V'), sb_gnss_T}; - // double loop all against all (without repetitions) + // only marginals + for (unsigned int i = 0; i < gauss_state_blocks.size(); i++) + { + assert(isStateBlockRegisteredDerived(gauss_state_blocks[i])); + + state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[i]); + double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]), + getAssociatedMemBlockPtr(gauss_state_blocks[i])); + } + /*// double loop all against all (without repetitions) for (unsigned int i = 0; i < gauss_state_blocks.size(); i++) { assert(isStateBlockRegisteredDerived(gauss_state_blocks[i])); @@ -286,7 +300,7 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]), getAssociatedMemBlockPtr(gauss_state_blocks[j])); } - } + }*/ break; }