problem.cpp

//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
// wolf includes
#include "core/problem/problem.h"
#include "core/map/factory_map.h"
#include "core/sensor/factory_sensor.h"
#include "core/processor/factory_processor.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_base.h"
#include "core/utils/loader.h"


namespace wolf
{

Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
        tree_manager_(nullptr),
        hardware_ptr_(std::make_shared<HardwareBase>()),
        trajectory_ptr_(std::make_shared<TrajectoryBase>()),
        map_ptr_(_map),
        motion_provider_map_(),
        frame_structure_(_frame_structure),
        prior_options_(std::make_shared<PriorOptions>()),
        transformed_(false)
{
    dim_ = _dim;
    if (_frame_structure == "PO" and _dim == 2)
    {
        state_size_ = 3;
        state_cov_size_ = 3;
    }else if (_frame_structure == "PO" and _dim == 3)
    {
        state_size_ = 7;
        state_cov_size_ = 6;
    }else if (_frame_structure == "POV" and _dim == 3)
    {
        state_size_ = 10;
        state_cov_size_ = 9;
    }else if (_frame_structure == "POVCDL" and _dim == 3)
    {
        state_size_ = 19;
        state_cov_size_ = 18;
    }
    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());
    if (map_ptr_)
        map_ptr_   -> setProblem(shared_from_this());
}

ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
{
    ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // 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(const std::string& _input_yaml_file, std::vector<std::string> _schema_folders)
{
#if __APPLE__
    std::string lib_extension = ".dylib";
#else
    std::string lib_extension = ".so";
#endif

    // Create yaml server
    auto server = yaml-schema-cpp::YamlServer({}, _input_yaml_file);

    // 
  
    std::stringstream log;
    if (not server.validate(SensorClass, log))
    { 
        WOLF_ERROR(log.str());
        return nullptr;
    } 
    auto params = std::make_shared<ParamsSensorClass>(server.getNode())

    // 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, nullptr);

    WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");

    // Load plugins
    auto loaders = std::vector<std::shared_ptr<Loader>>();
    std::string plugins_path = _WOLF_LIB_DIR;
    if (plugins_path.back() != '/')
    {
        plugins_path += '/';  // only works for UNIX systems
    }
    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_name + " via " + plugin);
      auto l = std::make_shared<LoaderRaw>(plugin);
      l->load();
      loaders.push_back(l);
    }

    // load raw libs
    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...");
    }
    for (auto lib : raw_libs) {
        WOLF_TRACE("Loading raw lib " + lib);
        auto l = std::make_shared<LoaderRaw>(lib);
        l->load();
        loaders.push_back(l);
    }

    // Install sensors and processors -- load plugins only if sensors and processor are not registered
    auto sensors        = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
    for (auto sen : sensors)
    {
        if (not FactorySensor::isCreatorRegistered(sen["type"]))
        {
            auto        plugin_name = sen["plugin"];
            std::string plugin      = plugins_path + "libwolf" + plugin_name + lib_extension;
            WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
            auto l = std::make_shared<LoaderRaw>(plugin);
            l->load();
            loaders.push_back(l);
        }
        problem->installSensor(sen["type"], sen["name"], _server);
    }
    auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
    for (auto prc : processors)
    {
        if (not FactoryProcessor::isCreatorRegistered(prc["type"]))
        {
            auto        plugin_name = prc["plugin"];
            std::string plugin      = plugins_path + "libwolf" + plugin_name + lib_extension;
            WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
            auto l = std::make_shared<LoaderRaw>(plugin);
            l->load();
            loaders.push_back(l);
        }
        problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server);
    }

    // Map (optional)
    std::string map_type, map_plugin;
    try
    {
        map_type = _server.getParam<std::string>("map/type");
        map_plugin = _server.getParam<std::string>("map/plugin");
    } 
    catch (MissingValueException& e) 
    {
        WOLF_TRACE("No map/type and/or map/plugin specified. Emplacing the default map: MapBase.");
        map_type = "MapBase";
        map_plugin = "core";
    }
    WOLF_TRACE("Map Type: ", map_type, " in plugin ", map_plugin);

    auto map = AutoConfFactoryMap::create(map_type, _server);
    map->setProblem(problem);
    problem->setMap(map);

    // Tree manager
    std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
    WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
    if (tree_manager_type != "None" and tree_manager_type != "none")
        problem->setTreeManager(FactoryTreeManagerServer::create(tree_manager_type, _server));

    // Set problem prior -- first keyframe
    std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
    assert((prior_mode == "nothing" || prior_mode == "initial_guess" || prior_mode == "fix" || prior_mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
    WOLF_TRACE("Prior mode: ", prior_mode);
    if (prior_mode == "nothing")
    {
        problem->setPriorOptions(prior_mode);
    }
    else if (prior_mode == "factor")
    {
        problem->setPriorOptions(prior_mode,
                                 _server.getParam<VectorComposite>("problem/prior/state"),
                                 _server.getParam<VectorComposite>("problem/prior/sigma"));
    }
    else
    {
        problem->setPriorOptions(prior_mode,
                                 _server.getParam<VectorComposite>("problem/prior/state"));
    }

    // Done
    return problem;
}

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

SensorBasePtr Problem::installSensor(const std::string& _sen_type,
                                     const std::string& _unique_sensor_name,
                                     ParamsSensorBasePtr _params,
                                     const Priors& _priors)
{
    SensorBasePtr sen_ptr = FactorySensorPriors::create(_sen_type, 
                                                        _unique_sensor_name, 
                                                        dim_, 
                                                        _params, 
                                                        _priors);
    sen_ptr->link(getHardware());
    return sen_ptr;
}

SensorBasePtr Problem::installSensor(const std::string& _sen_type,
                                     const std::string& _unique_sensor_name,
                                     const std::string& _params_yaml_filename)
{
    if (_params_yaml_filename == "" or not file_exists(_params_yaml_filename))
    {
        throw std::runtime_error("Cannot install sensor: provided params YAML file: '" + _params_yaml_filename + "' does not exist.");
    }
    SensorBasePtr sen_ptr = FactorySensorYaml::create(_sen_type, 
                                                      _unique_sensor_name, 
                                                      dim_, 
                                                      _params_yaml_filename);
    sen_ptr->link(getHardware());
    return sen_ptr;
}

SensorBasePtr Problem::installSensor(const std::string& _sen_type,
                                     const std::string& _unique_sensor_name,
                                     const ParamsServer& _server)
{
    SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, dim_, _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,
                                           ParamsProcessorBasePtr _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 = FactoryProcessor::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);

    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 = findSensor(_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);
    if (not file_exists(_params_filename))
        throw std::runtime_error("Cannot install processor: parameters' YAML file does not exist: " + _params_filename);
    
    ProcessorBasePtr prc_ptr = FactoryProcessorYaml::create(_prc_type, _unique_processor_name, _params_filename);

    //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);

    return prc_ptr;
}

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 = findSensor(_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 = FactoryProcessorServer::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);

    return prc_ptr;
}

SensorBaseConstPtr Problem::findSensor(const std::string& _sensor_name) const
{
    return getHardware()->getSensor(_sensor_name);
}

SensorBasePtr Problem::findSensor(const std::string& _sensor_name)
{
    return getHardware()->getSensor(_sensor_name);
}

ProcessorBaseConstPtr Problem::findProcessor(const std::string& _processor_name) const
{
    for (const auto& sensor : getHardware()->getSensorList())
        for (const auto& processor : sensor->getProcessorList())
            if (processor->getName() == _processor_name)
                return processor;
    return nullptr;
}

ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name)
{
    for (const auto& sensor : getHardware()->getSensorList())
        for (const auto& processor : sensor->getProcessorList())
            if (processor->getName() == _processor_name)
                return processor;
    return nullptr;
}

FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
                                   const StateStructure& _frame_structure, //
                                   const SizeEigen _dim, //
                                   const Eigen::VectorXd& _frame_state)
{
    VectorComposite state;
    SizeEigen index = 0;
    SizeEigen size = 0;
    for (auto key : _frame_structure)
    {
        if (key == 'O')
        {
            if (_dim == 2) size = 1;
            else size = 4;
        }
        else
            size = _dim;

        assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");

        state.emplace(key, _frame_state.segment(index, size));

        // append new key to frame structure
        if (frame_structure_.find(key) == std::string::npos) // not found
            frame_structure_ += std::string(1,key);

        index += size;
    }

    assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");

    auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
                                             _time_stamp,
                                             _frame_structure,
                                             state);

    return frm;
}

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

FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
                                    const StateStructure& _frame_structure, //
                                    const VectorComposite& _frame_state)
{
    return FrameBase::emplace<FrameBase>(getTrajectory(),
                                         _time_stamp,
                                         _frame_structure,
                                         _frame_state);
}

FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
                                       const VectorComposite& _frame_state)
{
    // append new keys to frame structure
    for (auto pair_key_vec : _frame_state)
    {
        auto key = pair_key_vec.first;
        if (frame_structure_.find(key) == std::string::npos) // not found
            frame_structure_ += std::string(1,key);
    }

    return FrameBase::emplace<FrameBase>(getTrajectory(),
                                         _time_stamp,
                                         getFrameStructure(),
                                         _frame_state);
}

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

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

TimeStamp Problem::getTimeStamp ( ) const
{
    TimeStamp  ts = TimeStamp::Invalid();

    for (auto prc_pair : motion_provider_map_)
        if (prc_pair.second->getTimeStamp().ok())
            if ( (not ts.ok() ) or prc_pair.second->getTimeStamp() > ts)
                ts = prc_pair.second->getTimeStamp();


    if (not ts.ok())
    {
        auto last_kf = trajectory_ptr_->getLastFrame();

        if (last_kf)
            ts = last_kf->getTimeStamp(); // Use last estimated frame's state

    }

    WOLF_DEBUG_COND(not ts.ok(), "Problem has nowhere to find a valid timestamp!");

    return ts;
}


VectorComposite Problem::getState(const StateStructure& _structure) const
{
    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);

    VectorComposite state;

    // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
    for (auto prc_pair : motion_provider_map_)
    {
        auto prc_state = prc_pair.second->getState(structure);

        // transfer processor vector blocks to problem state
        for (auto pair_key_vec : prc_state)
        {
            if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
            {  
              state.insert(pair_key_vec);
            }
        }

        //If all keys are filled return
        if (state.size() == structure.size())
        {
            return state;
        }
    }

    // check for empty blocks and fill them with the last KF, with the prior, or with zeros in the worst case

    VectorComposite state_last;

    auto last_kf = trajectory_ptr_->getLastFrame();

    if (last_kf)
        state_last = last_kf->getState(structure);

    else if (prior_options_ and prior_options_->mode != "nothing")
        state_last = prior_options_->state;

    for (auto key : structure)
    {
        if (state.count(key) == 0)
        {
            auto state_last_it = state_last.find(key);

            if (state_last_it != state_last.end())
                state.emplace(key, state_last_it->second);

            else
                state.emplace(key, stateZero(string(1,key)).at(key));
        }
    }

    return state;
}

VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
{
    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);

    VectorComposite state;

    // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
    for (auto prc_pair : motion_provider_map_)
    {
        auto prc_state = prc_pair.second->getState(_ts, structure);

        // transfer processor vector blocks to problem state
        for (auto pair_key_vec : prc_state)
        {
            if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
                state.insert(pair_key_vec);
        }

        //If all keys are filled return
        if (state.size() == structure.size())
        {
            return state;
        }
    }

    // check for empty blocks and fill them with the closest KF to ts, with the prior, or with zeros in the worst case

    VectorComposite state_last;

    auto last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);

    if (last_kf)
        state_last = last_kf->getState(structure);

    else if (prior_options_ and prior_options_->mode != "nothing")
        state_last = prior_options_->state;

    for (auto key : structure)
    {
        if (state.count(key) == 0)
        {
            auto state_last_it = state_last.find(key);

            if (state_last_it != state_last.end())
                state.emplace(key, state_last_it->second);

            else
                state.emplace(key, stateZero(string(1,key)).at(key));
        }
    }

    return state;
}

VectorComposite Problem::getOdometry(const StateStructure& _structure) const
{
    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);

    VectorComposite odom_state;

    // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
    for (auto prc_pair : motion_provider_map_)
    {
        auto prc_state = prc_pair.second->getOdometry();

        // transfer processor vector blocks to problem state
        for (auto pair_key_vec : prc_state)
        {
            if (odom_state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
                odom_state.insert(pair_key_vec);
        }
    }

    // check for empty blocks and fill them with the the prior, or with zeros in the worst case

    VectorComposite state_last;

    if (prior_options_ and prior_options_->mode != "nothing")
        state_last = prior_options_->state;

    for (auto key : structure)
    {
        if (odom_state.count(key) == 0)
        {
            auto state_last_it = state_last.find(key);

            if (state_last_it != state_last.end())
                odom_state.emplace(key, state_last_it->second);

            else
                odom_state.emplace(key, stateZero(string(1,key)).at(key));
        }
    }

    return odom_state;
}

SizeEigen Problem::getDim() const
{
    return dim_;
}

const StateStructure& Problem::getFrameStructure() const
{
    return frame_structure_;
}

void Problem::appendToStructure(const StateStructure& _structure)
{
    for (auto key : _structure)
        if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
            frame_structure_ += key;
}

void Problem::setTreeManager(TreeManagerBasePtr _gm)
{
    if (tree_manager_)
        tree_manager_->setProblem(nullptr);
    tree_manager_ = _gm;
    tree_manager_->setProblem(shared_from_this());
}

void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
{
    // Check if is state getter
    if (not _motion_provider_ptr->isStateGetter())
    {
        WOLF_WARN("Problem::addMotionProvider: adding a MotionProvider processor with state_getter=false. Not adding this processor");
        return;
    }

    // check duplicated priority
    while (motion_provider_map_.count(_motion_provider_ptr->getStatePriority()) == 1)
    {
        WOLF_ERROR("Problem::addMotionProvider: adding a MotionProvider processor with priority = ",
                   _motion_provider_ptr->getStatePriority(),
                   " which is already taken. Trying to add it with priority = ",
                   _motion_provider_ptr->getStatePriority()+1);
        _motion_provider_ptr->setStatePriority(_motion_provider_ptr->getStatePriority()+1);
    }

    // add to map ordered by priority
    motion_provider_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr);
    appendToStructure(_motion_provider_ptr->getStateStructure());
}

void Problem::removeMotionProvider(MotionProviderPtr proc)
{
    WOLF_WARN_COND(motion_provider_map_.count(proc->getStatePriority()) == 0, "Problem::clearMotionProvider: missing processor");

    motion_provider_map_.erase(proc->getStatePriority());

    // rebuild frame structure with remaining motion processors
    frame_structure_.clear();
    for (auto pm : motion_provider_map_)
        appendToStructure(pm.second->getStateStructure());
}

VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
{
    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);

    VectorComposite state;
    for (auto key : structure)
    {
        VectorXd vec;
        if (key == 'O')
        {
            if (dim_ == 2) vec = VectorXd::Zero(1);
            else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
        }
        else
        {
            vec = VectorXd::Zero(dim_);
        }
        state.emplace(key, vec);
    }
    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)
{
    WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotionProvider() ? "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());

    // pause processor profiling
    if (_processor_ptr)
        _processor_ptr->stopCaptureProfiling();

    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);
#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
            }

    // notify tree manager
    if (tree_manager_)
        tree_manager_->keyFrameCallback(_keyframe_ptr);

    // resume processor profiling
    if (_processor_ptr)
        _processor_ptr->startCaptureProfiling();
}

StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
    std::lock_guard<std::mutex> lock(mut_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_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_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_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(StateBlockConstPtr _state1, 
                                 StateBlockConstPtr _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<StateBlockConstPtr, StateBlockConstPtr>(_state1, _state2)) != covariances_.end())
        _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
                covariances_.at(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state1, _state2));
    else if (covariances_.find(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state2, _state1)) != covariances_.end())
        _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
                covariances_.at(std::pair<StateBlockConstPtr, StateBlockConstPtr>(_state2, _state1)).transpose();
    else
    {
      WOLF_DEBUG("Could not find the requested covariance block.");
      return false;
    }

    return true;
}

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

    // fill _cov
    for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
        for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
        {
            auto sb1 = it1->first;
            auto sb2 = it2->first;
            std::pair<StateBlockConstPtr, StateBlockConstPtr> pair_12(sb1, sb2);
            std::pair<StateBlockConstPtr, StateBlockConstPtr> 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(StateBlockConstPtr _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 (auto sb_i : _frame_ptr->getStateBlockVec())
    {
        j = 0;
        for (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::getLastFrameCovariance(Eigen::MatrixXd& cov) const
{
    auto frm = getLastFrame();
    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 (auto sb_i : _landmark_ptr->getStateBlockVec())
    {
        j = 0;
        for (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;
}

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

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

void Problem::setMap(MapBasePtr _map_ptr)
{
    assert(map_ptr_ == nullptr && "map has already been set");

    map_ptr_ = _map_ptr;
}

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

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

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

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

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

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

FrameBaseConstPtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
{
    return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}

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

void Problem::setPriorOptions(const std::string& _mode,
                              const VectorComposite& _state ,
                              const VectorComposite& _sigma   )
{
    assert(prior_options_ != nullptr && "prior options have already been applied");
    assert(prior_options_->mode == "" && "prior options have already been set");
    assert((_mode == "nothing" || _mode == "initial_guess" || _mode == "fix" || _mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");

    // Store options (optionals depending on the mode)
    WOLF_TRACE("prior mode:           ", _mode);
    prior_options_->mode = _mode;

    if (prior_options_->mode != "nothing")
    {
        assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");

        WOLF_TRACE("prior state:          ", _state);
        prior_options_->state = _state;

        if (prior_options_->mode == "factor")
        {
            assert(_sigma.includesStructure(frame_structure_) && "any missing key in prior sigma");

            bool isPositive = true;
            for(const auto& it: _sigma)
                isPositive = isPositive and (it.second.array() > Constants::EPS).all();

            assert(isPositive && "prior sigma is not positive");

            MatrixComposite Q;
            for (const auto& pair_key_sig : _sigma)
            {
                const auto& key = pair_key_sig.first;
                const auto& sig_blk = pair_key_sig.second;

                const auto& cov_blk = (sig_blk.array() * sig_blk.array()).matrix().asDiagonal();
                Q.emplace(key,key,cov_blk);
            }
            WOLF_TRACE("prior covariance:"    , Q);
            prior_options_->cov = Q;
        }
    }
}

FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
    assert(!isPriorSet() && "applyPriorOptions can be called once!");

    FrameBasePtr prior_keyframe(nullptr);

    if (prior_options_->mode != "nothing" and prior_options_->mode != "")
    {
        prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);

        // Update origin for odometry processors
        for (auto proc_pair : motion_provider_map_)
            proc_pair.second->setOdometry(prior_options_->state);

        if (prior_options_->mode == "fix")
            prior_keyframe->fix();
        else if (prior_options_->mode == "factor")
        {
            // ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)

            // Emplace a capture
            auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);

            // Emplace a feature and a factor for each state block
            for (auto pair_key_sb : prior_keyframe->getStateBlockMap())
            {
                auto key = pair_key_sb.first;
                auto sb  = pair_key_sb.second;

                const auto& state_blk = prior_options_->state.at(key);
                const auto& covar_blk = prior_options_->cov.at(key,key);

                assert(sb->getSize()      == state_blk.size() && "prior_options state wrong size");
                assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");

                // feature
                auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), state_blk, covar_blk);

                // factor
                if (sb->hasLocalParametrization())
                {
                    if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
                        auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
                    else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
                        auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
                    else
                        throw std::runtime_error("not implemented...!");
                }
                else
                {
                    auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
                }

            }

        }
        else
            assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");

        // notify all processors
        keyFrameCallback(prior_keyframe, nullptr);
    }
    // remove prior options
    prior_options_ = nullptr;

    return prior_keyframe;
}

FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
                                     const VectorComposite &_sigma,
                                     const TimeStamp &_ts)
{
    setPriorOptions("factor", _state, _sigma);
    return applyPriorOptions(_ts);
}


FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
                                  const TimeStamp &_ts)
{
    setPriorOptions("fix", _state);
    return applyPriorOptions(_ts);
}

FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
                                           const TimeStamp &_ts)
{
    setPriorOptions("initial_guess", _state);
    return applyPriorOptions(_ts);
}

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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream) const
{

    _stream << std::endl;
    _stream << "Wolf tree status ------------------------" << std::endl;

    getHardware()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");

    getTrajectory()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");

    getMap()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");

    _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";

    std::string tabs = "";

    if (_verbose) _stream << std::endl;
    if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
    auto P = shared_from_this();
    if (_verbose)
    {
        _stream << "Prb @ " << P.get() << std::endl;
    }
    // ------------------------
    //     HARDWARE branch
    // ------------------------
    auto H = hardware_ptr_;
    H->check(log, H, _verbose, _stream, tabs);
    // ------------------------
    //    TRAJECTORY branch
    // ------------------------
    auto T = trajectory_ptr_;
    T->check(log, T, _verbose, _stream, tabs);

    // ------------------------
    //       MAP branch
    // ------------------------
    auto M = map_ptr_;
    M->check(log, M, _verbose, _stream, tabs);


    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::perturb(double amplitude)
{
    // Sensors
    for (auto S : getHardware()->getSensorList())
        S->perturb(amplitude);

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

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

void Problem::transform(const VectorComposite& _transformation)
{
    std::lock_guard<std::mutex> lock_transform (mut_transform_); // Protect especially from SolverManager::update()

    transformation_ = _transformation;
    transformed_.store(true);

    // Sensors
    for (auto S : getHardware()->getSensorList())
        S->transform(_transformation);

    // Frames and Captures
    for (auto F : getTrajectory()->getFrameMap())
    {
        F.second->transform(_transformation);
        for (auto& C : F.second->getCaptureList())
            C->transform(_transformation);
    }

    // Landmarks
    for (auto L : getMap()->getLandmarkList())
        L->transform(_transformation);
}

bool Problem::isTransformed() const
{
    return transformed_.load();
}
void Problem::resetTransformed()
{
    transformed_.store(false);
}
VectorComposite Problem::getTransformation() const
{
    std::lock_guard<std::mutex> lock(mut_transform_);
    return transformation_;
}

} // namespace wolf