diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index 3f1ae77d736b8af1709516d5fb9c8a306b7a6b49..a6759817ae22e8b38c5f5dbce2144e2d8aee23c5 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -56,7 +56,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 + ROBOT_LANDMARKS, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks + GAUSS }; /** diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index fa21c6bbf877c6455f6ece811b043fcbb4cf2275..9037c4ddd743f72186f0b1d54714978d7accdb97 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -205,6 +205,66 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) } break; } + case CovarianceBlocksToBeComputed::GAUSS: + { + // State blocks: + // - Last KF: PO + // - CaptureGnss T + + std::vector<StateBlockPtr> gauss_state_blocks; + + // last KF + FrameBasePtr last_frame = wolf_problem_->getLastFrame(); + if (not last_frame) + return false; + + // get capture gnss + StateBlockPtr sb_gnss_T; + for (CaptureBasePtr cap : last_frame->getCaptureList()) + if (cap->getType() == "CaptureGnss" and cap->hasStateBlock('T')) + { + sb_gnss_T = cap->getStateBlock('T'); + break; + } + + // Error + if (not last_frame->hasStateBlock('P') or + not isStateBlockRegistered(last_frame->getStateBlock('P')) or + not last_frame->hasStateBlock('O') or + not isStateBlockRegistered(last_frame->getStateBlock('O')) or + not sb_gnss_T or + not isStateBlockRegistered(sb_gnss_T)) + { + WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P O 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 isStateBlockRegistered(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 isStateBlockRegistered(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P')); + WOLF_WARN_COND(not sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found"); + WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegistered(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T); + + return false; + } + + // Fill vector of state blocks + gauss_state_blocks = std::vector<StateBlockPtr>{last_frame->getStateBlock('P'), + last_frame->getStateBlock('O'), + sb_gnss_T}; + + // double loop all against all (without repetitions) + for (unsigned int i = 0; i < gauss_state_blocks.size(); i++) + for (unsigned int j = i; j < gauss_state_blocks.size(); j++) + { + state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[j]); + double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]), + getAssociatedMemBlockPtr(gauss_state_blocks[j])); + } + + break; + } + default: + throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value"); + } //std::cout << "pairs... " << double_pairs.size() << std::endl;