Skip to content
Snippets Groups Projects
solver_manager.cpp 5.83 KiB
#include "base/solver/solver_manager.h"
#include "base/trajectory_base.h"
#include "base/map_base.h"
#include "base/landmark/landmark_base.h"

namespace wolf {

SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
          wolf_problem_(_wolf_problem)
{
    assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
}

void SolverManager::update()
{
    // REMOVE CONSTRAINTS
    auto fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
    while ( fac_notification_it != wolf_problem_->getFactorNotificationMap().end() )
    {
        if (fac_notification_it->second == REMOVE)
        {
            removeFactor(fac_notification_it->first);
            fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
        }
        else
            fac_notification_it++;
    }

    // ADD/REMOVE STATE BLOCS
    auto sb_notification_it = wolf_problem_->getStateBlockNotificationMap().begin();
    while ( sb_notification_it != wolf_problem_->getStateBlockNotificationMap().end() )
    {
        StateBlockPtr state_ptr = sb_notification_it->first;

        if (sb_notification_it->second == ADD)
        {
            // only add if not added
            if (state_blocks_.find(state_ptr) == state_blocks_.end())
            {
                state_blocks_.emplace(state_ptr, state_ptr->getState());
                addStateBlock(state_ptr);
                // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
                state_ptr->resetStateUpdated();
                state_ptr->resetFixUpdated();
                state_ptr->resetLocalParamUpdated();
            }
            else
            {
                WOLF_DEBUG("Tried adding an already registered StateBlock.");
            }
        }
        else
        {
            // only remove if it exists
            if (state_blocks_.find(state_ptr)!=state_blocks_.end())
            {
                removeStateBlock(state_ptr);
                state_blocks_.erase(state_ptr);
            }
            else
            {
                WOLF_DEBUG("Tried to remove a StateBlock that was not added !");
            }
        }
        // next notification
        sb_notification_it = wolf_problem_->getStateBlockNotificationMap().erase(sb_notification_it);
    }

    // ADD CONSTRAINTS
    fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
    while (fac_notification_it != wolf_problem_->getFactorNotificationMap().end())
    {
        assert(wolf_problem_->getFactorNotificationMap().begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");

        addFactor(wolf_problem_->getFactorNotificationMap().begin()->first);
        fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
    }

    // UPDATE STATE BLOCKS (state, fix or local parameterization)
    for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
    {
        if (state_blocks_.find(state_ptr)==state_blocks_.end())
            continue;

        assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");

        // state update
        if (state_ptr->stateUpdated())
        {
            Eigen::VectorXs new_state = state_ptr->getState();
            // We assume the same size for the states in both WOLF and the solver.
            std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
            // reset flag
            state_ptr->resetStateUpdated();
        }
        // fix update
        if (state_ptr->fixUpdated())
        {
            updateStateBlockStatus(state_ptr);
            // reset flag
            state_ptr->resetFixUpdated();
        }
        // local parameterization update
        if (state_ptr->localParamUpdated())
        {
            updateStateBlockLocalParametrization(state_ptr);
            // reset flag
            state_ptr->resetLocalParamUpdated();
        }
    }

    //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
    //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
}

wolf::ProblemPtr SolverManager::getProblem()
{
    return wolf_problem_;
}

std::string SolverManager::solve(const ReportVerbosity report_level)
{
    // update problem
    update();

    std::string report = solveImpl(report_level);

    // update StateBlocks with optimized state value.
    /// @todo whatif someone has changed the state notification during opti ??
    /// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.

    std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
            it_end = state_blocks_.end();
    for (; it != it_end; ++it)
    {
        // Avoid usuless copies
        if (!it->first->isFixed())
            it->first->setState(it->second, false); // false = do not raise the flag state_updated_
    }
    return report;
}

Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
{
    auto it = state_blocks_.find(state_ptr);

    if (it == state_blocks_.end())
        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");

    return it->second;
}

Scalar* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
{
    auto it = state_blocks_.find(state_ptr);

    if (it == state_blocks_.end())
        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");

    return it->second.data();
}

} // namespace wolf