Skip to content
Snippets Groups Projects
Commit 667c47d9 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

covariance enum and implementation for GAUSS

parent 9a213f94
No related branches found
No related tags found
No related merge requests found
Pipeline #6268 passed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    include/core/solver/solver_manager.h
    + 6
    − 5
    View file @ 667c47d9
    ......@@ -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
    };
    /**
    ......
    src/ceres_wrapper/solver_ceres.cpp
    + 131
    − 57
    View file @ 667c47d9
    ......@@ -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;
    }
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment