Skip to content
Snippets Groups Projects
problem.cpp 76.74 KiB
// wolf includes
#include "core/problem/problem.h"
#include "core/hardware/hardware_base.h"
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
#include "core/sensor/sensor_base.h"
#include "core/processor/processor_motion.h"
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_pose.h"
#include "core/factor/factor_base.h"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
#include "core/utils/logging.h"
#include "core/utils/params_server.hpp"
#include "core/utils/loader.hpp"
#include "core/utils/check_log.hpp"


// IRI libs includes

// C++ includes
#include <algorithm>
#include <map>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <unordered_set>

namespace wolf
{

Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
        hardware_ptr_(std::make_shared<HardwareBase>()),
        trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
        map_ptr_(std::make_shared<MapBase>()),
        processor_motion_ptr_(),
        prior_is_set_(false),
        frame_structure_(_frame_structure)
{
    if (_frame_structure == "PO" and _dim == 2)
    {
        state_size_ = 3;
        state_cov_size_ = 3;
        dim_ = 2;
    }else if (_frame_structure == "PO" and _dim == 3)
    {
        state_size_ = 7;
        state_cov_size_ = 6;
        dim_ = 3;
    } else if (_frame_structure == "POV" and _dim == 3)
    {
        state_size_ = 10;
        state_cov_size_ = 9;
        dim_ = 3;
    }
    else std::runtime_error(
            "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");

}

void Problem::setup()
{
    hardware_ptr_  -> setProblem(shared_from_this());
    trajectory_ptr_-> setProblem(shared_from_this());
    map_ptr_       -> setProblem(shared_from_this());
}

ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
{
    ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
    p->setup();
    return p->shared_from_this();
}

ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
#if __APPLE__
    std::string lib_extension = ".dylib";
#else
    std::string lib_extension = ".so";
#endif
    // Problem structure and dimension
    std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
    int dim                     = _server.getParam<int>         ("problem/dimension");
    auto problem                = Problem::create(frame_structure, dim);
    //
    //  cout << "PRINTING SERVER MAP" << endl;
    // _server.print();
    // cout << "-----------------------------------" << endl;
    WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");

    // Load plugins
    auto loaders = std::vector<Loader *>();
    std::string plugins_path;
    try{
        plugins_path = _server.getParam<std::string>("plugins_path");
    }
    catch(MissingValueException& e){
      WOLF_WARN(e.what());
      WOLF_WARN("Setting '/usr/local/lib/iri-algorithms/' as plugins path...");
      plugins_path="/usr/local/lib/iri-algorithms/";
    }
    for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins")) {
      if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
      std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
      WOLF_TRACE("Loading plugin " + plugin);
      auto l = new LoaderRaw(plugin);
      l->load();
      loaders.push_back(l);
    }
    std::string packages_path;
    try {
        packages_path = _server.getParam<std::string>("packages_path");
    } catch (MissingValueException& e) {
        WOLF_WARN(e.what());
        WOLF_WARN("Support for subscribers disabled...");
    }
    for (auto it : _server.getParam<std::vector<std::string>>("packages")) {
        std::string subscriber = packages_path + "/libsubscriber_" + it + lib_extension;
        WOLF_TRACE("Loading subscriber " + subscriber);
        auto l = new LoaderRaw(subscriber);
        l->load();
        loaders.push_back(l);
    }
    std::vector<std::string> raw_libs;
    try {
        raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
    } catch (MissingValueException& e) {
        WOLF_TRACE("No raw libraries to load...");
        raw_libs = std::vector<std::string>();
    }
    for (auto lib : raw_libs) {
        WOLF_TRACE("Loading raw lib " + lib);
        auto l = new LoaderRaw(lib);
        l->load();
        loaders.push_back(l);
    }
    // Install sensors and processors
    auto sensorMap = std::map<std::string, SensorBasePtr>();
    auto procesorMap = std::map<std::string, ProcessorBasePtr>();
    auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
    for(auto sen : sensors){
        sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen["name"], problem->installSensor(sen["type"], sen["name"], _server)));
    }
    auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
    for(auto prc : processors){
        procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
    }

    // Prior
    Eigen::VectorXd prior_state = _server.getParam<Eigen::VectorXd>("problem/prior/state");
    Eigen::MatrixXd prior_cov   = _server.getParam<Eigen::MatrixXd>("problem/prior/cov");
    double prior_time_tolerance = _server.getParam<double>("problem/prior/time_tolerance");
    double prior_ts             = _server.getParam<double>("problem/prior/timestamp");

    WOLF_TRACE("prior timestamp:\n"     , prior_ts);
    WOLF_TRACE("prior state:\n"         , prior_state.transpose());
    WOLF_TRACE("prior covariance:\n"    , prior_cov);
    WOLF_TRACE("prior time tolerance:\n", prior_time_tolerance);

    problem->setPrior(prior_state, prior_cov, prior_ts, prior_time_tolerance);

    // Done
    return problem;
    }

Problem::~Problem()
{
    //    WOLF_DEBUG("destructed -P");
}

SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
                                     const std::string& _unique_sensor_name, //
                                     const Eigen::VectorXd& _extrinsics, //
                                     ParamsSensorBasePtr _intrinsics)
{
    SensorBasePtr sen_ptr = SensorFactory::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
    sen_ptr->link(getHardware());
    return sen_ptr;
}

SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
                                     const std::string& _unique_sensor_name, //

                                 const Eigen::VectorXd& _extrinsics, //
                                     const std::string& _intrinsics_filename)
{

    if (_intrinsics_filename != "")
    {
        assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
        ParamsSensorBasePtr intr_ptr = ParamsSensorFactory::get().create(_sen_type, _intrinsics_filename);
        return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
    }
    else
        return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());

}

SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
                                     const std::string& _unique_sensor_name, //
                                     const ParamsServer& _server)
{
    SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(_sen_type, _unique_sensor_name, _server);
    sen_ptr->link(getHardware());
    return sen_ptr;
}

ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
                                           const std::string& _unique_processor_name, //
                                           SensorBasePtr _corresponding_sensor_ptr, //
                                           ProcessorParamsBasePtr _prc_params)
{
    if (_corresponding_sensor_ptr == nullptr)
    {
      WOLF_ERROR("Cannot install processor '", _unique_processor_name,
                 "' since the associated sensor does not exist !");
      return ProcessorBasePtr();
    }

    ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params);

    //Dimension check
    int prc_dim = prc_ptr->getDim();
    auto prb = this;
    assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");

    prc_ptr->configure(_corresponding_sensor_ptr);
    prc_ptr->link(_corresponding_sensor_ptr);

    // setting the origin in all processor motion if origin already setted
    if (prc_ptr->isMotion() && prior_is_set_)
        (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());

    // setting the main processor motion
    if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
        processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);

    return prc_ptr;
}

ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
                                           const std::string& _unique_processor_name, //
                                           const std::string& _corresponding_sensor_name, //
                                           const std::string& _params_filename)
{
    SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
    if (sen_ptr == nullptr)
        throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
    if (_params_filename == "")
        return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
    else
    {
        assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
        ProcessorParamsBasePtr prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename);
        return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
    }
}

ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
                                           const std::string& _unique_processor_name, //
                                           const std::string& _corresponding_sensor_name, //
                                           const ParamsServer& _server)
{
    SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
    if (sen_ptr == nullptr)
        throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");

    ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server);

    //Dimension check
    int prc_dim = prc_ptr->getDim();
    auto prb = this;
    assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");

    prc_ptr->configure(sen_ptr);
    prc_ptr->link(sen_ptr);


    // setting the origin in all processor motion if origin already setted
    if (prc_ptr->isMotion() && prior_is_set_)
        (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());

    // setting the main processor motion
    if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
        processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);

    return prc_ptr;
}

SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
{
    auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
                               getHardware()->getSensorList().end(),
                               [&](SensorBasePtr sb)
                               {
                                   return sb->getName() == _sensor_name;
                               }); // lambda function for the find_if

    if (sen_it == getHardware()->getSensorList().end())
        return nullptr;

    return (*sen_it);
}

IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
{
    for (auto sen : getHardware()->getSensorList()) // loop all sensors
    {
        auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
                                   sen->getProcessorList().end(),
                                   [&](ProcessorBasePtr prc)
                                   {
                                       return prc->getName() == _processor_name;
                                   }); // lambda function for the find_if

        if (prc_it != sen->getProcessorList().end())  // found something!
        {
            if ((*prc_it)->isMotion()) // found, and it's motion!
            {
                std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
                processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
                return processor_motion_ptr_;
            }
            else // found, but it's not motion!
            {
                std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl;
                return nullptr;
            }
        }
    }
    // nothing found!
    std::cout << "Processor '" << _processor_name << "' not found!" << std::endl;
    return nullptr;
}

void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
{
    processor_motion_ptr_ = _processor_motion_ptr;
}

void Problem::clearProcessorMotion()
{
    processor_motion_ptr_.reset();
}

FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
                                   const SizeEigen _dim, //
                                   FrameType _frame_key_type, //
                                   const Eigen::VectorXd& _frame_state, //
                                   const TimeStamp& _time_stamp)
{
    auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
    return frm;
}

FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
                                   const SizeEigen _dim, //
                                   FrameType _frame_key_type, //
                                   const TimeStamp& _time_stamp)
{
    return emplaceFrame(_frame_structure, _dim, _frame_key_type, getState(_time_stamp), _time_stamp);
}

FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
                                   const Eigen::VectorXd& _frame_state, //
                                   const TimeStamp& _time_stamp)
{
    return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _frame_state, _time_stamp);
}

FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
                                   const TimeStamp& _time_stamp)
{
    return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
}

Eigen::VectorXd Problem::getCurrentState() const
{
    Eigen::VectorXd state(getFrameStructureSize());
    getCurrentState(state);
    return state;
}

void Problem::getCurrentState(Eigen::VectorXd& _state) const
{
    if (processor_motion_ptr_ != nullptr)
        processor_motion_ptr_->getCurrentState(_state);
    else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
        trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
    else
        _state = zeroState();
}

void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) const
{
    if (processor_motion_ptr_ != nullptr)
    {
        processor_motion_ptr_->getCurrentState(_state);
        processor_motion_ptr_->getCurrentTimeStamp(ts);
    }
    else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
    {
        trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
        trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
    }
    else
        _state = zeroState();
}

void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
{
    // try to get the state from processor_motion if any, otherwise...
    if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
    {
        FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
        if (closest_frame != nullptr)
            closest_frame->getState(_state);
        else
            _state = zeroState();
    }
}

Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
{
    Eigen::VectorXd state(getFrameStructureSize());
    getState(_ts, state);
    return state;
}

SizeEigen Problem::getFrameStructureSize() const
{
    return state_size_;
}

void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const
{
    _x_size   = state_size_;
    _cov_size = state_cov_size_;
}

SizeEigen Problem::getDim() const
{
    return dim_;
}
std::string Problem::getFrameStructure() const
{
    return frame_structure_;
}

Eigen::VectorXd Problem::zeroState() const
{
    Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());

    // Set the quaternion identity for 3d states. Set only the real part to 1:
    if(this->getDim() == 3)
        state(6) = 1.0;
    return state;
}

bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
{
    // This should implement a black list of processors that have forbidden keyframe creation
    // This decision is made at problem level, not at processor configuration level.
    // If you want to configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.

    // Currently allowing all processors to vote:
    return true;
}

void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
    WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
    WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());

    for (auto sensor : hardware_ptr_->getSensorList())
        for (auto processor : sensor->getProcessorList())
            if (processor && (processor != _processor_ptr) )
            {
#ifdef PROFILING
                auto start = std::chrono::high_resolution_clock::now();
#endif
                processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
#ifdef PROFILING
                auto stop     = std::chrono::high_resolution_clock::now();
                auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
                WOLF_INFO("keyFrameCallback Prc_id: ", processor->id(),
                          " Prc_type: ", processor->getType(),
                          " Prc_name: ", processor->getName(),
                          " keyframe_id: ", _keyframe_ptr->id(),
                          " keyframe_type: ", _keyframe_ptr->getType(),
                          " timestamp: ", _keyframe_ptr->getTimeStamp(),
                          " microseconds: ", duration.count());
#endif
            }
}

bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
{
    // This should implement a black list of processors that have forbidden auxiliary frame creation
    // This decision is made at problem level, not at processor configuration level.
    // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.

    // Currently allowing all processors to vote:
    return true;
}

void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
    // TODO
//    if (_processor_ptr)
//    {
//        WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
//    }
//    else
//    {
//        WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
//    }
//
//    processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
}

StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
    std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
    //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;

    // Check if there is already a notification for this state block
    auto notification_it = state_block_notification_map_.find(_state_ptr);

    // exsiting notification for this state block
    if (notification_it != state_block_notification_map_.end())
    {
        // duplicated notification
        if ( notification_it->second == _noti)
        {
            WOLF_WARN("This notification has been already notified");
        }
        // opposite notification -> cancell out eachother
        else
            state_block_notification_map_.erase(notification_it);
    }
    // Add notification
    else
        state_block_notification_map_[_state_ptr] = _noti;

    return _state_ptr;
}

bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
{
    std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
    if (state_block_notification_map_.find(sb_ptr) == state_block_notification_map_.end())
        return false;

    notif = state_block_notification_map_.at(sb_ptr);
    return true;
}

FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _noti)
{
    std::lock_guard<std::mutex> lock(mut_factor_notifications_);
    //std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;

    // Check if there is already the same notification for this factor
    auto notification_it = factor_notification_map_.find(_factor_ptr);
    // exsiting notification for this factor
    if (notification_it != factor_notification_map_.end())
    {
        // duplicated notification
        if (notification_it->second == _noti)
        {
            WOLF_WARN("This notification has been already notified");
        }
        // opposite notification -> cancell out eachother
        else
            factor_notification_map_.erase(notification_it);

    }
    // Add notification
    else
        factor_notification_map_[_factor_ptr] = _noti;

    return _factor_ptr;
}

bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
{
    std::lock_guard<std::mutex> lock(mut_factor_notifications_);
    if (factor_notification_map_.find(fac_ptr) == factor_notification_map_.end())
        return false;

    notif = factor_notification_map_.at(fac_ptr);
    return true;
}

void Problem::clearCovariance()
{
    std::lock_guard<std::mutex> lock(mut_covariances_);
    covariances_.clear();
}

void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov)
{
    assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
    assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");

    std::lock_guard<std::mutex> lock(mut_covariances_);
    covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
}

void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov)
{
    assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
    assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");

    std::lock_guard<std::mutex> lock(mut_covariances_);
    covariances_[std::make_pair(_state1, _state1)] = _cov;
}

bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row,
                                 const int _col) const
{
    //std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
    //std::cout << "_row " << _row << std::endl;
    //std::cout << "_col " << _col << std::endl;
    //std::cout << "_state1 tangent space size: " << _state1->getLocalSize() << std::endl;
    //std::cout << "_state2 tangent space size: " << _state2->getLocalSize() << std::endl;
    //std::cout << "part of cov to be filled:" << std::endl <<  _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) << std::endl;
    //if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
    //    std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] << std::endl;
    //else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
    //    std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose() << std::endl;

    assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");

    std::lock_guard<std::mutex> lock(mut_covariances_);

    if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
        _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
                covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2));
    else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
       _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
                covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)).transpose();
    else
    {
      WOLF_DEBUG("Could not find the requested covariance block.");
      return false;
    }

    return true;
}

bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
{
    std::lock_guard<std::mutex> lock(mut_covariances_);

    // fill covariance
    for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
        for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
        {
            StateBlockPtr sb1 = it1->first;
            StateBlockPtr sb2 = it2->first;
            std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2);
            std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1);

            // search st1 & st2
            if (covariances_.find(pair_12) != covariances_.end())
            {
                assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
                       _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
                assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
                       _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");

                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_12);
                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_12).transpose();
            }
            else if (covariances_.find(pair_21) != covariances_.end())
            {
                assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
                       _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
                assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
                       _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");

                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_21).transpose();
                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_21);
            }
            else
                return false;
        }

    return true;
}

bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
{
    return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}

bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const
{
    bool success(true);

    // resizing
     SizeEigen sz = _frame_ptr->getLocalSize();
    _covariance.resize(sz, sz);

    // filling covariance
    int i = 0, j = 0;
    for (const auto& sb_i : _frame_ptr->getStateBlockVec())
    {
        j = 0;
        for (const auto& sb_j : _frame_ptr->getStateBlockVec())
        {
            success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
            j += sb_j->getLocalSize();
        }
        i += sb_i->getLocalSize();
    }

    return success;
}

bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const
{
    FrameBasePtr frm = getLastKeyFrame();
    return getFrameCovariance(frm, cov);
}

bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const
{
    FrameBasePtr frm = getLastKeyOrAuxFrame();
    return getFrameCovariance(frm, cov);
}

bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const
{
    bool success(true);

    // resizing
     SizeEigen sz = _landmark_ptr->getLocalSize();
    _covariance.resize(sz, sz);

    // filling covariance
    int i = 0, j = 0;
    for (const auto& sb_i : _landmark_ptr->getStateBlockVec())
    {
        j = 0;
        for (const auto& sb_j : _landmark_ptr->getStateBlockVec())
        {
            success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
            j += sb_j->getLocalSize();
        }
        i += sb_i->getLocalSize();
    }

    return success;



//    bool success(true);
//    int i = 0, j = 0;
//
//    const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
//
//    // computing size
//    SizeEigen sz = 0;
//    for (const auto& sb : state_bloc_vec)
//        if (sb)
//            sz += sb->getLocalSize();
//
//    // resizing
//    _covariance = Eigen::MatrixXs(sz, sz);
//
//    // filling covariance
//
//    for (const auto& sb_i : state_bloc_vec)
//    {
//        if (sb_i)
//        {
//            j = 0;
//            for (const auto& sb_j : state_bloc_vec)
//            {
//                if (sb_j)
//                {
//                    success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
//                    j += sb_j->getLocalSize();
//                }
//            }
//            i += sb_i->getLocalSize();
//        }
//    }
//    return success;
}

MapBasePtr Problem::getMap() const
{
    return map_ptr_;
}

TrajectoryBasePtr Problem::getTrajectory() const
{
    return trajectory_ptr_;
}

HardwareBasePtr Problem::getHardware() const
{
    return hardware_ptr_;
}

FrameBasePtr Problem::getLastFrame() const
{
    return trajectory_ptr_->getLastFrame();
}

FrameBasePtr Problem::getLastKeyFrame() const
{
    return trajectory_ptr_->getLastKeyFrame();
}

FrameBasePtr Problem::getLastKeyOrAuxFrame() const
{
    return trajectory_ptr_->getLastKeyOrAuxFrame();
}

FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
{
    return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
}

FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
{
    return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
}

FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen::MatrixXd& _prior_cov, const TimeStamp& _ts, const double _time_tolerance)
{
    if ( ! prior_is_set_ )
    {
        // Create origin frame
        FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);

        // create origin capture with the given state as data
        // Capture fix only takes 3d position and Quaternion orientation
        CapturePosePtr init_capture;
        if (this->getFrameStructure() == "POV" and this->getDim() == 3)
            init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
        else
            init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);

        // emplace feature and factor
        init_capture->emplaceFeatureAndFactor();

        WOLF_DEBUG("Set prior callback: KF", origin_keyframe->id(), " Callback emitted with ts = ", origin_keyframe->getTimeStamp());

        // Notify all motion processors about the prior KF
        for (auto sensor : hardware_ptr_->getSensorList())
            for (auto processor : sensor->getProcessorList())
                if (processor->isMotion())
                    // Motion processors will set its origin at the KF
                    (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe);

        prior_is_set_ = true;

        // Notify all other processors about the origin KF --> they will join it or not depending on their received data
        for (auto sensor : hardware_ptr_->getSensorList())
            for (auto processor : sensor->getProcessorList())
                if ( !processor->isMotion() )
                    processor->keyFrameCallback(origin_keyframe, _time_tolerance);

        return origin_keyframe;
    }
    else
        throw std::runtime_error("Origin already set!");
}

void Problem::loadMap(const std::string& _filename_dot_yaml)
{
    getMap()->load(_filename_dot_yaml);
}

void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
{
    getMap()->save(_filename_dot_yaml, _map_name);
}

void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric, bool state_blocks) const
{

    stream << std::endl;
    stream << "P: wolf tree status ---------------------" << std::endl;
    stream << "Hardware" << ((depth < 1) ? ("   -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "")  << std::endl;
    if (depth >= 1)
    {
        // Sensors =======================================================================================
        for (auto S : getHardware()->getSensorList())
        {
            stream << "  Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
            if (depth < 2)
                stream << " -- " << S->getProcessorList().size() << "p";
            stream << std::endl;
            if (metric && state_blocks)
            {
                stream << "    sb: ";
                for (auto& _key : S->getStructure())
                {
                    auto key = std::string(1,_key);
                    auto sb = S->getStateBlock(key);
                    stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
                }
                stream << std::endl;
            }
            else if (metric)
            {
                stream << "    ( ";
                for (auto& _key : S->getStructure())
                {
                    auto key = std::string(1,_key);
                    auto sb = S->getStateBlock(key);
                    stream << sb->getState().transpose() << " ";
                }
                stream << ")" << std::endl;
            }
            else if (state_blocks)
            {
                stream << "    sb: ";
                for (auto& _key : S->getStructure())
                {
                    auto key = std::string(1,_key);
                    auto sb = S->getStateBlock(key);
                    stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
                }
                stream << std::endl;
            }
            if (depth >= 2)
            {
                // Processors =======================================================================================
                for (auto p : S->getProcessorList())
                {
                    if (p->isMotion())
                    {
                        stream << "    PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
                        ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
                        if (pm->getOrigin())
                            stream << "      o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? "  KFrm" : "  AFrm" ) : "  Frm")
                            << pm->getOrigin()->getFrame()->id() << std::endl;
                        if (pm->getLast())
                            stream << "      l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
                            << pm->getLast()->getFrame()->id() << std::endl;
                        if (pm->getIncoming())
                            stream << "      i: Cap" << pm->getIncoming()->id() << std::endl;
                    }
                    else
                    {
                        stream << "    PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
                        ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
                        if (pt)
                        {
                            if (pt->getOrigin())
                                stream << "      o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
                                << pt->getOrigin()->getFrame()->id() << std::endl;
                            if (pt->getLast())
                                stream << "      l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
                                << pt->getLast()->getFrame()->id() << std::endl;
                            if (pt->getIncoming())
                                stream << "      i: Cap" << pt->getIncoming()->id() << std::endl;
                        }
                    }
                } // for p
            }
        } // for S
    }
    stream << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "")  << std::endl;
    if (depth >= 1)
    {
        // Frames =======================================================================================
        for (auto F : getTrajectory()->getFrameList())
        {
            stream << (F->isKeyOrAux() ? (F->isKey() ? "  KFrm" : " AFrm") : "  Frm") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C  " : "");
            if (constr_by)
            {
                stream << "  <-- ";
                for (auto cby : F->getConstrainedByList())
                    stream << "Fac" << cby->id() << " \t";
            }
            stream << std::endl;
            if (metric)
            {
                stream << (F->isFixed() ? "    Fix" : "    Est") << ", ts=" << std::setprecision(5)
                        << F->getTimeStamp();
                stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
                stream << std::endl;
            }
            if (state_blocks)
            {
                stream << "    sb:";
                for (const auto& sb : F->getStateBlockVec())
                {
                    stream << " " << (sb->isFixed() ? "Fix" : "Est");
                }
                stream << std::endl;
            }
            if (depth >= 2)
            {
                // Captures =======================================================================================
                for (auto C : F->getCaptureList())
                {
                    stream << "    Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();

                    if(C->getSensor() != nullptr)
                    {
                        stream << " -> Sen" << C->getSensor()->id();
                    }
                    else
                        stream << " -> Sen-";
                    if (C->isMotion())
                    {
                        auto CM = std::static_pointer_cast<CaptureMotion>(C);
                        if (auto OC = CM->getOriginCapture())
                        {
                            stream << " -> OCap" << OC->id();
                            if (auto OF = OC->getFrame())
                                stream << " ; OFrm" << OF->id();
                        }
                    }

                    stream << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
                    if (constr_by)
                    {
                        stream << "  <-- ";
                        for (auto cby : C->getConstrainedByList())
                            stream << "Fac" << cby->id() << " \t";
                    }
                    stream << std::endl;

                    if (state_blocks)
                        for (const auto& sb : C->getStateBlockVec())
                        {
                            if(sb != nullptr)
                            {
                                stream << "      sb: ";
                                stream << (sb->isFixed() ? "Fix" : "Est");
                                if (metric)
                                    stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
                                stream << std::endl;
                            }
                        }

                    if (C->isMotion() )
                    {
                        CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
                        stream << "      buffer size  :  " << CM->getBuffer().get().size() << std::endl;
                        if ( metric && ! CM->getBuffer().get().empty())
                        {
                            stream << "      delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
                            if (CM->hasCalibration())
                            {
                                stream << "      calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
                                stream << "      jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
                                stream << "      calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
                                stream << "      delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
                            }
                        }
                    }

                    if (depth >= 3)
                    {
                        // Features =======================================================================================
                        for (auto f : C->getFeatureList())
                        {
                            stream << "      Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c  " : "");
                            if (constr_by)
                            {
                                stream << "  <--\t";
                                for (auto cby : f->getConstrainedByList())
                                    stream << "Fac" << cby->id() << " \t";
                            }
                            stream << std::endl;
                            if (metric)
                                stream << "        m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
                                        << " )" << std::endl;
                            if (depth >= 4)
                            {
                                // Factors =======================================================================================
                                for (auto c : f->getFactorList())
                                {
                                    stream << "        Fac" << c->id() << " " << c->getType() << " -->";
                                    if (       c->getFrameOtherList()   .empty()
                                            && c->getCaptureOtherList() .empty()
                                            && c->getFeatureOtherList() .empty()
                                            && c->getLandmarkOtherList().empty())
                                        stream << " Abs";

                                    for (const auto& Fow : c->getFrameOtherList())
                                        if (!Fow.expired())
                                            stream << " Frm" << Fow.lock()->id();
                                    for (const auto& Cow : c->getCaptureOtherList())
                                        if (!Cow.expired())
                                            stream << " Cap" << Cow.lock()->id();
                                    for (const auto& fow : c->getFeatureOtherList())
                                        if (!fow.expired())
                                            stream << " Ftr" << fow.lock()->id();
                                    for (const auto& Low : c->getLandmarkOtherList())
                                        if (!Low.expired())
                                            stream << " Lmk" << Low.lock()->id();
                                    stream << std::endl;
                                } // for c
                            }
                        } // for f
                    }
                } // for C
            }
        } // for F
    }
    stream << "Map" << ((depth < 1) ? ("        -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
    if (depth >= 1)
    {
        // Landmarks =======================================================================================
        for (auto L : getMap()->getLandmarkList())
        {
            stream << "  Lmk" << L->id() << " " << L->getType();
            if (constr_by)
            {
                stream << "\t<-- ";
                for (auto cby : L->getConstrainedByList())
                    stream << "Fac" << cby->id() << " \t";
            }
            stream << std::endl;
            if (metric)
            {
                stream << (L->isFixed() ? "    Fix" : "    Est");
                stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
                stream << std::endl;
            }
            if (state_blocks)
            {
                stream << "    sb:";
                for (const auto& sb : L->getStateBlockVec())
                {
                    if (sb != nullptr)
                        stream << (sb->isFixed() ? " Fix" : " Est");
                }
                stream << std::endl;
            }
        } // for L
    }
    stream << "-----------------------------------------" << std::endl;
    stream << std::endl;
}

bool Problem::check(bool verbose, std::ostream& stream) const
{

    CheckLog log(true, "");
    log.explanation_ = "## WOLF::problem inconsistencies list \n   ---------------------------------- \n";

    if (verbose) stream << std::endl;
    if (verbose) stream << "Wolf tree integrity ---------------------" << std::endl;
    auto P_raw = this;
    if (verbose)
    {
        stream << "Prb @ " << P_raw << std::endl;
    }
    // ------------------------
    //     HARDWARE branch
    // ------------------------
    auto H = hardware_ptr_;
    if (verbose)
    {
        stream << "Hrw @ " << H.get() << std::endl;
    }

    // check pointer to Problem
    std::stringstream inconsistency_explanation;
    inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
        << " but problem pointer is " << P_raw << "\n";
    log.addAssertion((H->getProblem().get() == P_raw), inconsistency_explanation);
    //Clear inconsistency_explanation

    // Sensors =======================================================================================
    for (auto S : H->getSensorList())
    {
        if (verbose)
        {
            stream << "  Sen" << S->id() << " @ " << S.get() << std::endl;
            stream << "    -> Prb @ " << S->getProblem().get() << std::endl;
            stream << "    -> Hrw @ " << S->getHardware().get() << std::endl;
            for (auto pair: S->getStateBlockMap())
            {
                auto sb = pair.second;
                stream <<  "    " << pair.first << " sb @ " << sb.get();
                if (sb)
                {
                    auto lp = sb->getLocalParametrization();
                    if (lp)
                        stream <<  " (lp @ " << lp.get() << ")";
                }
                stream << std::endl;
            }
        }

        // check problem and hardware pointers
        inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
            << " but problem pointer is " << P_raw << "\n";
        log.addAssertion((S->getProblem().get() == P_raw), inconsistency_explanation);
        //Clear inconsistency_explanation

        inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
            << " but hardware pointer is " << H << "\n";
        log.addAssertion((S->getHardware() == H), inconsistency_explanation);
        //Clear inconsistency_explanation

        //  Processors =======================================================================================
        for (auto p : S->getProcessorList())
        {
            if (verbose)
            {
                stream << "    Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
                stream << "      -> Prb  @ " << p->getProblem().get() << std::endl;
                stream << "      -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
            }

            // check problem and sensor pointers
            inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
                << " but problem pointer is " << P_raw << "\n";
            log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
            //Clear inconsistency_explanation

            inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
                << " but sensor pointer is " << P_raw << "\n";
            log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
            //Clear inconsistency_explanation
        }
    }
    // ------------------------
    //    TRAJECTORY branch
    // ------------------------
    auto T = trajectory_ptr_;
    if (verbose)
    {
        stream << "Trj @ " << T.get() << std::endl;
    }

    // check pointer to Problem
    inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
       << " but problem pointer is" << P_raw << "\n";
    log.addAssertion((T->getProblem().get() == P_raw), inconsistency_explanation);
    //Clear inconsistency_explanation

    // Frames =======================================================================================
    for (auto F : T->getFrameList())
    {
      if (verbose) {
        stream << (F->isKeyOrAux() ? (F->isKey() ? "  KFrm" : " EFrm") : "  Frm")
             << F->id() << " @ " << F.get() << std::endl;
        stream << "    -> Prb @ " << F->getProblem().get() << std::endl;
        stream << "    -> Trj @ " << F->getTrajectory().get() << std::endl;
      }
      for (const auto &pair: F->getStateBlockMap()) {

          auto sb = pair.second;

          // check for valid state block
          inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
                                  << " has State block pointer " << sb.get()
                                  << " = 0 \n";
          log.addAssertion((sb.get() != 0), inconsistency_explanation);

          if (verbose) {
            stream << "    "<< pair.first << " sb @ " << sb.get();
            if (sb) {
              auto lp = sb->getLocalParametrization();
              if (lp)
                stream << " (lp @ " << lp.get() << ")";
            }
            stream << std::endl;
          }
      }

        // check problem pointer
        inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
                        << " but problem pointer is" << P_raw << "\n";
        log.addAssertion((F->getProblem().get() == P_raw), inconsistency_explanation);

        // check trajectory pointer
        inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
                        << " but trajectory pointer is" << T << "\n";
        log.addAssertion((F->getTrajectory() == T), inconsistency_explanation);
        //Clear inconsistency_explanation

        // check constrained_by
        for (auto cby : F->getConstrainedByList())
        {
            if (verbose)
            {
                stream << "    <- c" << cby->id() << " -> ";
                for (const auto& Fow : cby->getFrameOtherList())
                    stream << " F" << Fow.lock()->id() << std::endl;
            }

            // check constrained_by pointer to this frame
            inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
                                << " not found among constrained-by factors\n";
            log.addAssertion((cby->hasFrameOther(F)), inconsistency_explanation);

            for (auto sb : cby->getStateBlockPtrVector())
            {

              if (verbose) {
                stream << "      sb @ " << sb.get();
                if (sb) {
                  auto lp = sb->getLocalParametrization();
                  if (lp)
                    stream << " (lp @ " << lp.get() << ")";
                }
                stream << std::endl;
              }
            }
        }

        // Captures =======================================================================================
        for (auto C : F->getCaptureList())
        {
            if (verbose)
            {
                stream << "    Cap" << C->id() << " @ " << C.get() << " -> Sen";
                if (C->getSensor()) stream << C->getSensor()->id();
                else stream << "-";
                stream << std::endl;
                stream << "      -> Prb  @ " << C->getProblem().get() << std::endl;
                stream << "      -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
            }
            for (auto pair: C->getStateBlockMap())
            {
                auto sb = pair.second;

                // check for valid state block
                inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
                                          << sb.get() << " = 0 \n";
                log.addAssertion((sb.get() != 0), inconsistency_explanation);

                if (verbose)
                {
                    stream <<  "      " << pair.first << " sb @ " << sb.get();
                    if (sb) {
                        auto lp = sb->getLocalParametrization();
                        if (lp)
                            stream << " (lp @ " << lp.get() << ")";
                    }
                    stream << std::endl;
                }
            }

            // check problem and frame pointers
            inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
            << " but problem pointer is" << P_raw << "\n";
            log.addAssertion((C->getProblem().get() == P_raw), inconsistency_explanation);
            //Clear inconsistency_explanation

            inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
            << " but frame pointer is" << F << "\n";
            log.addAssertion((C->getFrame() == F), inconsistency_explanation);
            //Clear inconsistency_explanation

            // check contrained_by
            for (const auto& cby : C->getConstrainedByList())
            {
                if (verbose)
                {
                    stream << "    <- c" << cby->id() << " -> ";
                    for (const auto& Cow : cby->getCaptureOtherList())
                        stream << " C" << Cow.lock()->id();
                    stream << std::endl;
                }

                // check constrained_by pointer to this capture
                inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
                                          << " not found among constrained-by factors\n";
                log.addAssertion((cby->hasCaptureOther(C)), inconsistency_explanation);

                for (auto sb : cby->getStateBlockPtrVector())
                {
                    if (verbose)
                    {
                        stream << "      sb @ " << sb.get();
                        if (sb)
                        {
                            auto lp = sb->getLocalParametrization();
                            if (lp)
                                stream <<  " (lp @ " << lp.get() << ")";
                        }
                        stream << std::endl;
                    }
                }

            }
            // Features =======================================================================================
            for (auto f : C->getFeatureList())
            {
                if (verbose)
                {
                    stream << "      Ftr" << f->id() << " @ " << f.get() << std::endl;
                    stream << "        -> Prb  @ " << f->getProblem().get() << std::endl;
                    stream << "        -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
                            << std::endl;
                }

                // check problem and capture pointers
                inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
                << " but problem pointer is" << P_raw << "\n";
                log.addAssertion((f->getProblem().get() == P_raw), inconsistency_explanation);
                //Clear inconsistency_explanation

                inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
                << " but capture pointer is" << C << "\n";
                log.addAssertion((f->getCapture() == C), inconsistency_explanation);
                //Clear inconsistency_explanation

                // check contrained_by
                for (auto cby : f->getConstrainedByList())
                {
                    if (verbose)
                    {
                        stream << "     <- c" << cby->id() << " -> ";
                        for (const auto& fow : cby->getFeatureOtherList())
                            stream << " f" << fow.lock()->id();
                        stream << std::endl;
                    }

                    // check constrained_by pointer to this feature
                    inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
                                                << " not found among constrained-by factors\n";
                    log.addAssertion((cby->hasFeatureOther(f)), inconsistency_explanation);
                    //Clear inconsistency_explanation
                }

                // Factors =======================================================================================
                for (auto c : f->getFactorList())
                {
                    if (verbose)
                        stream << "        Fac" << c->id() << " @ " << c.get();

                    if (       c->getFrameOtherList()   .empty()
                            && c->getCaptureOtherList() .empty()
                            && c->getFeatureOtherList() .empty()
                            && c->getLandmarkOtherList().empty() )    // case ABSOLUTE:
                    {
                        if (verbose)
                            stream << " --> Abs.";
                    }

                    // find constrained_by pointer in constrained frame
                    for (const auto& Fow : c->getFrameOtherList())
                    {
                        if (!Fow.expired())
                        {
                            const auto& Fo = Fow.lock();
                            if (verbose)
                            {
                                stream << " ( --> F" << Fo->id() << " <- ";
                                for (auto cby : Fo->getConstrainedByList())
                                    stream << " c" << cby->id();
                            }

                            // check constrained_by pointer in constrained frame
                            bool found = Fo->isConstrainedBy(c);
                            inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
                                    << " not found among constrained-by factors\n";
                            log.addAssertion((found), inconsistency_explanation);
                            //Clear inconsistency_explanation
                        }
                    }
                    if (verbose && !c->getFrameOtherList().empty())
                        stream << ")";

                    // find constrained_by pointer in constrained capture
                    for (const auto& Cow : c->getCaptureOtherList())
                    {
                        if (!Cow.expired())
                        {
                            const auto& Co = Cow.lock();

                            if (verbose)
                            {
                                stream << " ( --> C" << Co->id() << " <- ";
                                for (auto cby : Co->getConstrainedByList())
                                    stream << " c" << cby->id();
                            }

                            // check constrained_by pointer in constrained frame
                            bool found = Co->isConstrainedBy(c);
                            inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
                                    << " not found among constrained-by factors\n";
                            log.addAssertion((found), inconsistency_explanation);
                            //Clear inconsistency_explanation
                        }
                    }
                    if (verbose && !c->getCaptureOtherList().empty())
                        stream << ")";

                    // find constrained_by pointer in constrained feature
                    for (const auto& fow : c->getFeatureOtherList())
                    {
                        if (!fow.expired())
                        {
                            const auto& fo = fow.lock();
                            if (verbose)
                            {
                                stream << " ( --> f" << fo->id() << " <- ";
                                for (auto cby : fo->getConstrainedByList())
                                    stream << " c" << cby->id();
                            }

                            // check constrained_by pointer in constrained feature
                            bool found = fo->isConstrainedBy(c);
                            inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
                                                      << " not found among constrained-by factors\n";
                            log.addAssertion((found), inconsistency_explanation);
                        }
                    }
                    if (verbose && !c->getFeatureOtherList().empty())
                        stream << ")";

                    // find constrained_by pointer in constrained landmark
                    for (const auto& Low : c->getLandmarkOtherList())
                    {
                        if (Low.expired())
                        {
                            const auto& Lo = Low.lock();

                            if (verbose)
                            {
                                stream << " ( --> L" << Lo->id() << " <- ";
                                for (auto cby : Lo->getConstrainedByList())
                                    stream << " c" << cby->id();
                            }

                            // check constrained_by pointer in constrained landmark
                            bool found = Lo->isConstrainedBy(c);
                            inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
                                    << " not found among constrained-by factors\n";
                            log.addAssertion((found), inconsistency_explanation);
                            //Clear inconsistency_explanation
                        }
                    }
                    if (verbose && !c->getLandmarkOtherList().empty())
                        stream << ")";

                    if (verbose)
                        stream << std::endl;

                    if (verbose)
                    {
                        stream << "          -> Prb  @ " << c->getProblem().get() << std::endl;
                        stream << "          -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
                    }

                    // check problem and feature pointers
                    inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
                        << " but problem ptr is " << P_raw << "\n";
                    log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
                    //Clear inconsistency_explanation

                    inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
                        << " but it should have " << f << "\n";
                    log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
                    //Clear inconsistency_explanation

                    // find state block pointers in all constrained nodes
                    SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
                    for (auto sb : c->getStateBlockPtrVector())
                    {
                        bool found = false;
                        if (verbose)
                        {
                            stream <<  "          sb @ " << sb.get();
                            if (sb)
                            {
                                auto lp = sb->getLocalParametrization();
                                if (lp)
                                    stream <<  " (lp @ " << lp.get() << ")";
                            }
                        }
                        bool found_here;

                        // find in own Frame
                        found_here  = F->hasStateBlock(sb);
                        if (found_here && verbose) stream << " F" << F->id();
                        found       = found || found_here;

                        // find in own Capture
                        found_here  = C->hasStateBlock(sb);
                        if (found_here && verbose) stream << " C" << C->id();
                        found       = found || found_here;

                        // Find in other Captures of the own Frame
                        if (!found_here)
                            for (auto FC : F->getCaptureList())
                            {
                                found_here  = FC->hasStateBlock(sb);
                                if (found_here && verbose) stream << " F" << F->id() << ".C" << FC->id();
                                found       = found || found_here;
                            }

                        // find in own Sensor
                        if (S)
                        {
                            found_here  = S->hasStateBlock(sb);
                            if (found_here && verbose) stream << " S" << S->id();
                            found       = found || found_here;
                        }


                        // find in constrained Frame
                        for (const auto& Fow : c->getFrameOtherList())
                        {
                            if (!Fow.expired())
                            {
                                const auto& Fo = Fow.lock();
                                found_here  = Fo->hasStateBlock(sb);
                                if (found_here && verbose) stream << " Fo" << Fo->id();
                                found       = found || found_here;

                                // find in feature other's captures
                                for (auto FoC : Fo->getCaptureList())
                                {
                                    found_here  = FoC->hasStateBlock(sb);
                                    if (found_here && verbose) stream << " Fo" << Fo->id() << ".C" << FoC->id();
                                    found       = found || found_here;
                                }

                            }
                        }

                        // find in constrained Capture
                        for (const auto& Cow : c->getCaptureOtherList())
                        {
                            if (!Cow.expired())
                            {
                                const auto& Co = Cow.lock();
                                found_here  = Co->hasStateBlock(sb);
                                if (found_here && verbose) stream << " Co" << Co->id();
                                found       = found || found_here;
                            }
                        }

                        // find in constrained Feature
                        for (const auto& fow : c->getFeatureOtherList())
                        {
                            if (!fow.expired())
                            {
                                const auto& fo = fow.lock();
                                // find in constrained feature's Frame
                                auto foF    = fo->getFrame();
                                found_here  = foF->hasStateBlock(sb);
                                if (found_here && verbose) stream << " foF" << foF->id();
                                found       = found || found_here;

                                // find in constrained feature's Capture
                                auto foC    = fo->getCapture();
                                found_here  = foC->hasStateBlock(sb);
                                if (found_here && verbose) stream << " foC" << foC->id();
                                found       = found || found_here;

                                // find in constrained feature's Sensor
                                auto foS    = fo->getCapture()->getSensor();
                                found_here  = foS->hasStateBlock(sb);
                                if (found_here && verbose) stream << " foS" << foS->id();
                                found       = found || found_here;
                            }
                        }

                        // find in constrained landmark
                        for (const auto& Low : c->getLandmarkOtherList())
                        {
                            if (!Low.expired())
                            {
                                const auto& Lo = Low.lock();
                                found_here  = Lo->hasStateBlock(sb);
                                if (found_here && verbose) stream << " Lo" << Lo->id();
                                found       = found || found_here;
                            }
                        }

                        if (verbose)
                        {
                            if (found)
                                stream << " found";
                            else
                                stream << " NOT FOUND !";
                            stream << std::endl;
                        }

                        // check that the state block has been found somewhere
                        inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
                        log.addAssertion((found), inconsistency_explanation);

                        inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
                        log.addAssertion((sb.get() != nullptr), inconsistency_explanation);
                    }
                }
            }
        }
    }
    // ------------------------
    //       MAP branch
    // ------------------------
    auto M = map_ptr_;
    if (verbose)
        stream << "Map @ " << M.get() << std::endl;

    // check pointer to Problem
    inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
    log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);

    // Landmarks =======================================================================================
    for (auto L : M->getLandmarkList())
    {
        if (verbose)
        {
            stream << "  Lmk" << L->id() << " @ " << L.get() << std::endl;
            stream << "  -> Prb @ " << L->getProblem().get() << std::endl;
            stream << "  -> Map @ " << L->getMap().get() << std::endl;
            for (const auto& pair : L->getStateBlockMap())
            {
                auto sb = pair.second;
                stream <<  "  " << pair.first << " sb @ " << sb.get();
                if (sb)
                {
                    auto lp = sb->getLocalParametrization();
                    if (lp)
                        stream <<  " (lp @ " << lp.get() << ")";
                }
                stream << std::endl;
            }
        }

        // check problem and map pointers
        inconsistency_explanation << "Landmarks' problem ptr is "
                                  << L->getProblem().get() << " but problem is "
                                  << P_raw << "\n";

        log.addAssertion((L->getProblem().get() == P_raw), inconsistency_explanation);

        inconsistency_explanation << "The Landmarks' map ptr is "
                                  << L->getMap() << " but map is "
                                  << M << "\n";
        log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);

        for (auto cby : L->getConstrainedByList())
        {
          if (verbose)
          {
              stream << "      <- Fac" << cby->id() << " ->";

              for (const auto& Low : cby->getLandmarkOtherList())
              {
                  if (!Low.expired())
                  {
                      const auto& Lo = Low.lock();
                      stream << " Lmk" << Lo->id();
                  }
              }
              stream << std::endl;
          }

          // check constrained-by factors
          inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
                                    << " not found among constrained-by factors\n";
          log.addAssertion((cby->hasLandmarkOther(L)), inconsistency_explanation);

          for (auto sb : cby->getStateBlockPtrVector()) {
            if (verbose) {
              stream << "      sb @ " << sb.get();
              if (sb) {
                auto lp = sb->getLocalParametrization();
                if (lp)
                  stream << " (lp @ " << lp.get() << ")";
              }
              stream << std::endl;
            }
          }
        }
    }

    if (verbose) stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
    if (verbose) stream << std::endl;
    if (verbose and not log.is_consistent_) stream << log.explanation_ << std::endl;

    return log.is_consistent_;
}

bool Problem::check(int verbose_level) const
{
    return check((verbose_level > 0), std::cout);
}
void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
{
    print(depth, std::cout, constr_by, metric, state_blocks);
}
void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
{
    if (depth.compare("T") == 0)
        print(0, constr_by, metric, state_blocks);
    else if (depth.compare("F") == 0)
        print(1, constr_by, metric, state_blocks);
    else if (depth.compare("C") == 0)
        print(2, constr_by, metric, state_blocks);
    else if (depth.compare("f") == 0)
        print(3, constr_by, metric, state_blocks);
    else if (depth.compare("c") == 0)
        print(4, constr_by, metric, state_blocks);
    else
        print(0, constr_by, metric, state_blocks);
}
void Problem::perturb(double amplitude)
{
    // Sensors
    for (auto& S : getHardware()->getSensorList())
        S->perturb(amplitude);

    // Frames and Captures
    for (auto& F : getTrajectory()->getFrameList())
    {
        F->perturb(amplitude);
        for (auto& C : F->getCaptureList())
            C->perturb(amplitude);
    }

    // Landmarks
    for (auto& L : getMap()->getLandmarkList())
        L->perturb(amplitude);
}

} // namespace wolf