-
cont-integration authoredcont-integration authored
problem.cpp 49.16 KiB
// WOLF - Copyright (C) 2020,2021,2022,2023,2024
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/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/>.
// wolf includes
#include "core/problem/problem.h"
#include "core/internal/config.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/factory_state_block.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"
#include "core/utils/loader_utils.h"
// yaml-schema-cpp
#include "yaml-schema-cpp/yaml_server.hpp"
#include "yaml-schema-cpp/yaml_schema.hpp"
namespace wolf
{
Problem::Problem(SizeEigen _dim, const TypeComposite& _frame_types, LoaderPtr _loader)
: hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
map_ptr_(std::make_shared<MapBase>()),
tree_manager_(nullptr),
motion_provider_map_(),
dim_(_dim),
frame_keys_(),
// reseved state types (P -> position, O -> orientation, V -> velocity)
frame_types_({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")},
{'O', (dim_ == 2 ? "StateAngle" : "StateQuaternion")},
{'V', (dim_ == 2 ? "StateVector2d" : "StateVector3d")}}),
first_frame_values_(),
first_frame_priors_(),
first_frame_status_(0),
transformed_(false),
loader_(_loader)
{
if (not loader_) loader_ = std::make_shared<Loader>();
// append provided state types
addFrameTypes(_frame_types);
}
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(SizeEigen _dim, const TypeComposite& _frame_types, LoaderPtr _loader)
{
ProblemPtr p(
new Problem(_dim, _frame_types, _loader)); // 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,
LoaderPtr _loader,
bool _skip_yaml_validation)
{
// Load YAML node: yaml schema server only used to flatten "follows", not validation
WOLF_DEBUG("Loading YAML file...");
yaml_schema_cpp::YamlServer server = yaml_schema_cpp::YamlServer({}, _input_yaml_file);
// Call autoSetup via YAML node
return Problem::autoSetup(server.getNode(), _schema_folders, _loader, _skip_yaml_validation);
}
ProblemPtr Problem::autoSetup(YAML::Node _param_node,
std::vector<std::string> _schema_folders,
LoaderPtr _loader,
bool _skip_yaml_validation)
{
// VALIDATION ===============================================================================
if (not _skip_yaml_validation)
{
// Create YAML server
yaml_schema_cpp::YamlServer server = yaml_schema_cpp::YamlServer(_schema_folders);
server.setYaml(_param_node);
// SCHEMA FOLDERS (optional _schema_folders specify folders where to search schemas before installed ones)
// Search and load plugins to get the installed schema folders registered
if (not _loader) _loader = std::make_shared<Loader>();
searchAndLoadPlugins(server.getNode(), _loader);
// Add the installed schema folders after optional input folders
server.addFolderSchema(FolderRegistry::getRegisteredFolders());
// Validate against schema
WOLF_DEBUG("Validating YAML file...");
if (not server.applySchema("Problem.schema"))
{
WOLF_ERROR(server.getLog());
return nullptr;
}
// Validate against specific schema 2D or 3D
bool is2D = server.getNode()["problem"]["dimension"].as<int>() == 2;
if (not server.applySchema(is2D ? "Problem2d.schema" : "Problem3d.schema"))
{
WOLF_ERROR(server.getLog());
return nullptr;
}
WOLF_DEBUG("YAML file validated with ",
is2D ? "Problem2d.schema" : "Problem3d.schema",
"! Node after validation:\n",
server.getNode());
// recover modified node
_param_node = server.getNode();
}
// PROBLEM =============================================================================== // dimension
WOLF_DEBUG("Loading problem parameters...");
YAML::Node problem_node = _param_node["problem"];
int dim = problem_node["dimension"].as<int>();
// Create problem
WOLF_DEBUG("Creating problem...");
auto problem = Problem::create(dim, {}, _loader);
// First frame
WOLF_DEBUG("Setting first frame parameters...");
problem->setFirstFrameOptions(problem_node["first_frame"]);
// Tree manager
WOLF_DEBUG("Creating tree manager...");
YAML::Node tree_manager_node = problem_node["tree_manager"];
std::string tree_manager_type = tree_manager_node["type"].as<std::string>();
if (tree_manager_type != "none" and tree_manager_type != "None" and tree_manager_type != "NONE")
{
if (not FactoryTreeManagerNode::isCreatorRegistered(tree_manager_type))
_loader->loadPlugin(tree_manager_node["plugin"].as<std::string>());
problem->setTreeManager(FactoryTreeManagerNode::create(tree_manager_type, tree_manager_node, {}));
}
// SENSORS ===============================================================================
// load plugin if sensor is not registered
WOLF_DEBUG("Installing sensors...");
for (auto sensor_n : _param_node["sensors"]) problem->installSensor(sensor_n);
// PROCESSORS ===============================================================================
// load plugin if processor is not registered
WOLF_DEBUG("Installing processors...");
for (auto processor_n : _param_node["processors"]) problem->installProcessor(processor_n);
// MAP (optional) ===============================================================================
// Default MapBase
WOLF_DEBUG("Creating map...");
// load plugin if map is not registered
if (not FactoryMapNode::isCreatorRegistered(_param_node["map"]["type"].as<std::string>()))
_loader->loadPlugin(_param_node["map"]["plugin"].as<std::string>());
auto map = FactoryMapNode::create(_param_node["map"]["type"].as<std::string>(), _param_node["map"], {});
map->setProblem(problem);
problem->setMap(map);
// Done
return problem;
}
Problem::~Problem()
{
// WOLF_DEBUG("destructed -P");
}
SensorBasePtr Problem::installSensor(const YAML::Node& _sensor_node, std::vector<std::string> _schema_folders)
{
// (optionally) Validate YAML node
if (not _schema_folders.empty())
{
// Search and load plugins to get the installed schema folders registered
searchAndLoadPlugins(_sensor_node, loader_);
// Add the installed schema folders after optional input _schema_folders
FolderRegistry::appendRegisteredFolders(_schema_folders);
// Validate against TypeAndPlugin (check if it has 'type' and 'plugin')
// NOTE: Validation with the correponding schema is done in the creator
auto server = yaml_schema_cpp::YamlServer(_schema_folders);
server.setYaml(_sensor_node);
if (not server.applySchema("TypeAndPlugin"))
{ WOLF_ERROR(server.getLog());
throw std::runtime_error("Sensor could not be created! Missing type or plugin");
}
}
// Load plugin if factory not registered
auto sensor_type = _sensor_node["type"].as<std::string>();
if (not FactorySensorNode::isCreatorRegistered(sensor_type))
{
loader_->loadPlugin(_sensor_node["plugin"].as<std::string>());
if (not FactorySensorNode::isCreatorRegistered(sensor_type))
throw std::runtime_error("Sensor creator not registered after loading its plugin.");
}
// Create sensor
SensorBasePtr sen_ptr = FactorySensorNode::create(sensor_type, _sensor_node, _schema_folders);
if (not sen_ptr) throw std::runtime_error("Sensor could not be created.");
// Link
sen_ptr->link(getHardware());
return sen_ptr;
}
SensorBasePtr Problem::installSensor(const std::string& _params_yaml_filename,
std::vector<std::string> _schema_folders)
{
if (not file_exists(_params_yaml_filename))
throw std::runtime_error("Cannot install sensor: provided params YAML file: '" + _params_yaml_filename +
"' does not exist.");
// Load yaml
auto server = yaml_schema_cpp::YamlServer(_schema_folders, _params_yaml_filename);
auto sensor_node = server.getNode();
// Search and load plugins to get the installed schema folders registered
searchAndLoadPlugins(sensor_node, loader_);
// Add the installed schema folders after optional input _schema_folders
server.addFolderSchema(FolderRegistry::getRegisteredFolders());
_schema_folders = server.getFolderSchema();
// Validate against TypeAndPlugin (check if it has 'type' and 'plugin')
// NOTE: Validation with the correponding schema is done in the creator
if (not server.applySchema("TypeAndPlugin"))
{
WOLF_ERROR(server.getLog());
throw std::runtime_error("Sensor could not be created! Missing type or plugin in the yaml");
}
// Factory not registered
auto sensor_type = sensor_node["type"].as<std::string>();
if (not FactorySensorFile::isCreatorRegistered(sensor_type))
throw std::runtime_error("Creator of sensor '" + sensor_type + "' not registered.");
// Create sensor (includes validation of derived schema)
SensorBasePtr sen_ptr = FactorySensorFile::create(sensor_type, _params_yaml_filename, _schema_folders);
if (not sen_ptr) throw std::runtime_error("Sensor could not be created.");
// Link
sen_ptr->link(getHardware());
return sen_ptr;
}
ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node, std::vector<std::string> _schema_folders)
{
// (optionally) Validate YAML node
if (not _schema_folders.empty()) {
// Search and load plugins to get the installed schema folders registered
searchAndLoadPlugins(_processor_node, loader_);
// Add the installed schema folders after optional input _schema_folders
FolderRegistry::appendRegisteredFolders(_schema_folders);
// Validate against ProcessorBaseWithSensor (check if it has 'type' and 'plugin')
// NOTE: Validation with the correponding schema is done in the creator
auto server = yaml_schema_cpp::YamlServer(_schema_folders);
server.setYaml(_processor_node);
if (not server.applySchema("ProcessorBaseWithSensor"))
{
WOLF_ERROR(server.getLog());
throw std::runtime_error("Processor could not be created!");
}
}
// Find sensor
auto corresponding_sensor_name = _processor_node["sensor_name"].as<std::string>();
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!");
// Load plugin if factory not registered
auto processor_type = _processor_node["type"].as<std::string>();
if (not FactoryProcessorNode::isCreatorRegistered(processor_type))
{
loader_->loadPlugin(_processor_node["plugin"].as<std::string>());
if (not FactoryProcessorNode::isCreatorRegistered(processor_type))
throw std::runtime_error("Processor creator not registered.");
}
// Create processor (with derived class validation)
ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(processor_type, _processor_node, _schema_folders);
if (not prc_ptr) throw std::runtime_error("Processor could not be created.");
// Dimension check
if (prc_ptr->getDimCompatible() != 0 and prc_ptr->getDimCompatible() != this->getDim())
throw std::runtime_error("Processor not compatible with the Problem dimension.");
// Link
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
return prc_ptr;
}
ProcessorBasePtr Problem::installProcessor(SensorBasePtr _sensor_corresponding,
const std::string& _params_yaml_filename,
std::vector<std::string> _schema_folders)
{
if (not file_exists(_params_yaml_filename))
throw std::runtime_error("Cannot install processor: parameters' YAML file does not exist: " +
_params_yaml_filename);
// Load yaml
auto server = yaml_schema_cpp::YamlServer(_schema_folders, _params_yaml_filename);
auto processor_node = server.getNode();
// Search and load plugins to get the installed schema folders registered
searchAndLoadPlugins(processor_node, loader_);
// Add the installed schema folders after optional input _schema_folders
server.addFolderSchema(FolderRegistry::getRegisteredFolders());
_schema_folders = server.getFolderSchema();
// Validate against TypeAndPlugin (check if it has 'type' and 'plugin')
// NOTE: Validation with the correponding schema is done in the creator if (not server.applySchema("TypeAndPlugin"))
{
WOLF_ERROR(server.getLog());
throw std::runtime_error("Processor could not be created.");
}
// Creator registered
auto prc_type = processor_node["type"].as<std::string>();
if (not FactoryProcessorNode::isCreatorRegistered(prc_type))
throw std::runtime_error("Processor creator not registered.");
// Find sensor
if (_sensor_corresponding == nullptr) throw std::runtime_error("Cannot bind processor. Reason: Sensor nullptr!");
// Create processor (includes validating with derived schema)
ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(prc_type, processor_node, _schema_folders);
if (not prc_ptr) throw std::runtime_error("Processor could not be created.");
// Dimension check
if (prc_ptr->getDimCompatible() != 0 and prc_ptr->getDimCompatible() != this->getDim())
throw std::runtime_error("Processor not compatible with the Problem dimension.");
// Add processor
prc_ptr->configure(_sensor_corresponding);
prc_ptr->link(_sensor_corresponding);
return prc_ptr;
}
ProcessorBasePtr Problem::installProcessor(const std::string& _params_yaml_filename,
std::vector<std::string> _schema_folders)
{
if (not file_exists(_params_yaml_filename))
throw std::runtime_error("Cannot install processor: parameters' YAML file does not exist: " +
_params_yaml_filename);
// Load yaml
auto server = yaml_schema_cpp::YamlServer(_schema_folders, _params_yaml_filename);
// Search and load plugins to get the installed schema folders registered
searchAndLoadPlugins(server.getNode(), loader_);
// Add the installed schema folders after optional input _schema_folders
server.addFolderSchema(FolderRegistry::getRegisteredFolders());
_schema_folders = server.getFolderSchema();
// Validate against ProcessorBase and derived type
// NOTE: Validation with the correponding schema is done in the creator
if (not server.applySchema("ProcessorBaseWithSensor"))
{
WOLF_ERROR(server.getLog());
throw std::runtime_error("Processor could not be created.");
}
auto processor_node = server.getNode();
// Creator not registered
auto prc_type = processor_node["type"].as<std::string>();
if (not FactoryProcessorNode::isCreatorRegistered(prc_type))
throw std::runtime_error("Processor creator not registered: " + prc_type);
// Find sensor
auto corresponding_sensor_name = processor_node["sensor_name"].as<std::string>();
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!");
// Create processor (includes validating with derived schema)
ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(prc_type, processor_node, _schema_folders);
if (not prc_ptr) throw std::runtime_error("Processor could not be created.");
// Dimension check
if (prc_ptr->getDimCompatible() != 0 and prc_ptr->getDimCompatible() != this->getDim())
throw std::runtime_error("Processor not compatible with the Problem dimension.");
// Add processor
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 TypeComposite& _frame_types,
const VectorComposite& _frame_state,
const PriorComposite& _frame_priors)
{
// check input _frame_specs
checkTypeComposite(_frame_types);
// add the types of the keys that are not in frame_types_ (also checks that are consistent)
addFrameTypes(_frame_types);
// add the keys that are not in frame_keys_
addFrameKeys(_frame_state.getKeys());
return FrameBase::emplace<FrameBase>(getTrajectory(), _time_stamp, _frame_types, _frame_state, _frame_priors);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const StateKeys& _frame_keys)
{
if (not frame_types_.has(_frame_keys))
{
throw std::runtime_error("Problem::emplaceFrame: any unknown type, asked for " + _frame_keys +
" but problem only know the types of " + frame_types_.getKeys());
}
auto state = getState(_time_stamp, _frame_keys);
return emplaceFrame(_time_stamp, getFrameTypes(state.getKeys()), state);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const VectorComposite& _frame_state)
{ if (not frame_types_.has(_frame_state.getKeys()))
{
throw std::runtime_error("Problem::emplaceFrame: any unknown type, given vectors of " +
_frame_state.getKeys() + " but problem only know the types of " +
frame_types_.getKeys());
}
return Problem::emplaceFrame(_time_stamp, getFrameTypes(_frame_state.getKeys()), _frame_state);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
const StateKeys& _frame_keys,
const Eigen::VectorXd& _frame_state)
{
if (not frame_types_.has(_frame_keys))
{
throw std::runtime_error("Problem::emplaceFrame: any unknown type, given vectors of " + _frame_keys +
" but problem only know the types of " + frame_types_.getKeys());
}
VectorComposite vec_composite;
int next_state_index = 0;
for (auto key : _frame_keys)
{
auto state_size = FactoryStateBlockIdentityVector::create(frame_types_.at(key)).size();
if (_frame_state.size() < next_state_index + state_size)
{
throw std::runtime_error("Problem::emplaceFrame: the provided vector _frame_state is too short");
}
vec_composite[key] = _frame_state.segment(next_state_index, state_size);
next_state_index += state_size;
}
if (_frame_state.size() != next_state_index)
{
throw std::runtime_error("Problem::emplaceFrame: the provided vector _frame_state is too long");
}
return Problem::emplaceFrame(_time_stamp, getFrameTypes(vec_composite.getKeys()), vec_composite);
}
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 StateKeys& _keys) const
{
StateKeys keys = (_keys == "" ? getFrameKeys() : _keys);
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(keys);
// transfer processor vector blocks to problem state
for (auto pair_key_vec : prc_state)
{
if (not state.has(
pair_key_vec.first)) // Only write once. This gives priority to processors with more priority
{
state.insert(pair_key_vec);
}
}
// If all keys are filled return
if (state.has(keys))
{
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(keys);
else if (not first_frame_values_.empty())
state_last = first_frame_values_;
for (auto key : keys)
{
if (state.has(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(std::string(1, key)).at(key));
}
}
return state;
}
VectorComposite Problem::getState(const TimeStamp& _ts, const StateKeys& _structure) const
{
StateKeys structure = (_structure == "" ? getFrameKeys() : _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 (not first_frame_values_.empty())
state_last = first_frame_values_;
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(std::string(1, key)).at(key));
}
}
return state;
}
VectorComposite Problem::getOdometry(const StateKeys& _structure) const
{
StateKeys structure = (_structure == "" ? getFrameKeys() : _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 prior, or with zeros in the worst case
for (auto key : structure)
{
if (odom_state.count(key) == 0)
{
auto state_last_it = first_frame_values_.find(key);
if (state_last_it != first_frame_values_.end())
odom_state.emplace(key, state_last_it->second);
else
odom_state.emplace(key, stateZero(std::string(1, key)).at(key));
}
}
return odom_state;}
SizeEigen Problem::getDim() const
{
return dim_;
}
StateKeys Problem::getFrameKeys() const
{
return frame_keys_;
}
TypeComposite Problem::getFrameTypes(StateKeys _keys) const
{
if (_keys == "") return frame_types_(frame_keys_);
if (not frame_types_.has(_keys))
throw std::runtime_error("Problem::getFrameTypes: frame_types_ does not have any of the required keys");
TypeComposite types;
for (auto key : _keys) types.emplace(key, frame_types_.at(key));
return types;
}
bool Problem::hasFrameKeys(StateKeys _keys) const
{
for (auto input_key : _keys)
if (frame_keys_.find(input_key) == std::string::npos) return false;
return true;
}
void Problem::addFrameKeys(StateKeys _keys)
{
for (auto input_key : _keys)
if (frame_keys_.find(input_key) == std::string::npos) frame_keys_.push_back(input_key);
}
void Problem::addFrameTypes(const TypeComposite& _types)
{
for (auto pair : _types)
if (not frame_types_.has(pair.first)) // new key not found in problem structure -> append!
frame_types_.emplace(pair.first, pair.second);
else
{
WOLF_DEBUG_COND(frame_types_.at(pair.first) == pair.second,
"Type already in 'frame_types_' doing nothing.");
if (frame_types_.at(pair.first) != pair.second)
{
WOLF_ERROR("Trying to append type '",
pair.second,
"' for key '",
pair.first,
"' but it is already set as '",
frame_types_.at(pair.first),
"'");
throw std::runtime_error("Problem::addFrameTypes: Different type already defined in this key.");
}
}
// add also keys
addFrameKeys(_types.getKeys());
}
void Problem::setTreeManager(TreeManagerBasePtr _gm)
{
if (tree_manager_) tree_manager_->setProblem(nullptr);
tree_manager_ = _gm;
if (tree_manager_) 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("adding a MotionProvider processor with state_provider=false. Not adding this processor");
return;
}
// check duplicated priority
while (motion_provider_map_.count(_motion_provider_ptr->getOrder()) == 1)
{
WOLF_ERROR("Adding a MotionProvider processor with priority = ",
_motion_provider_ptr->getOrder(),
" which is already taken. Trying to add it with priority = ",
_motion_provider_ptr->getOrder() + 1);
_motion_provider_ptr->setOrder(_motion_provider_ptr->getOrder() + 1);
}
// add to map ordered by priority
motion_provider_map_.emplace(_motion_provider_ptr->getOrder(), _motion_provider_ptr);
// check consistent types
for (auto new_key : _motion_provider_ptr->getStateTypes().getKeys())
if (frame_types_.has(new_key) and frame_types_[new_key] != _motion_provider_ptr->getStateTypes().at(new_key))
{
throw std::runtime_error("Problem::addMotionProvider: inconsistent type for key " +
std::string(1, new_key) + " previously registered as " + frame_types_[new_key] +
" and now as " + _motion_provider_ptr->getStateTypes().at(new_key));
}
// Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key
addFrameTypes(_motion_provider_ptr->getStateTypes());
}
void Problem::removeMotionProvider(MotionProviderPtr proc)
{
WOLF_WARN_COND(motion_provider_map_.count(proc->getOrder()) == 0, "Missing processor");
motion_provider_map_.erase(proc->getOrder());
// rebuild frame structure with remaining motion processors
frame_keys_.clear();
for (auto pm : motion_provider_map_) addFrameKeys(pm.second->getStateTypes().getKeys());
}
VectorComposite Problem::stateZero(TypeComposite _types) const
{
if (_types.empty()) _types = frame_types_(frame_keys_);
VectorComposite state;
for (auto&& type_pair : _types)
{
state.emplace(type_pair.first, FactoryStateBlockIdentityVector::create(type_pair.second));
}
return state;
}
VectorComposite Problem::stateZero(const StateKeys& _keys) const
{
if (not frame_types_.has(_keys))
throw std::runtime_error("Problem::stateZero any unknown type... asked for " + _keys +
" but only have types for " + frame_types_.getKeys());
StateKeys keys = (_keys == "" ? getFrameKeys() : _keys);
return stateZero(getFrameTypes(_keys));
}
bool Problem::permitKeyFrame(ProcessorBaseConstPtr _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->getType(),
" - ",
_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)) processor->keyFrameCallback(_keyframe_ptr);
// 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 << 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 -> warning
WOLF_WARN_COND(notification_it->second == _noti, "This notification has been already notified");
// opposite notification -> cancell out eachother
if (notification_it->second != _noti) 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::getCovariance(NodeStateBlocksConstPtr _has_blocks,
const StateKeys& _keys,
Eigen::MatrixXd& _covariance) const
{
if (not _has_blocks)
{
WOLF_WARN("Called with a nullptr");
return false;
}
if (not _has_blocks->has(_keys))
{
WOLF_WARN("Does not have the keys ", _keys, ". Available keys: ", _has_blocks->getKeys());
return false;
}
bool success(true);
// resizing
SizeEigen sz = _has_blocks->getLocalSize();
_covariance.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
for (auto key_i : _keys)
{
auto sb_i = _has_blocks->getStateBlock(key_i);
j = 0;
for (auto key_j : _keys)
{
auto sb_j = _has_blocks->getStateBlock(key_j);
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
}
i += sb_i->getLocalSize();
}
return success;
}
bool Problem::getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& cov) const
{
return getCovariance(getLastFrame(), _keys, cov);
}
MapBaseConstPtr Problem::getMap() const
{
return map_ptr_;
}
MapBasePtr Problem::getMap()
{ return map_ptr_;
}
void Problem::setMap(MapBasePtr _map_ptr)
{
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::setFirstFrameOptions(const YAML::Node& _first_frame_node)
{
setFirstFrameOptions(TypeComposite(_first_frame_node, "type", false),
VectorComposite(_first_frame_node, "value", true),
PriorComposite(_first_frame_node, "prior", true));
}
void Problem::setFirstFrameOptions(const TypeComposite& _first_frame_types,
const VectorComposite& _first_frame_values,
const PriorComposite& _first_frame_priors)
{
if (isFirstFrameApplied())
{
throw std::runtime_error("prior options have already been applied");
}
WOLF_WARN_COND(first_frame_status_ == 1, "Overriding prior options.");
// add the types of the keys that are not in frame_types_
addFrameTypes(_first_frame_types);
first_frame_values_ = _first_frame_values;
first_frame_priors_ = _first_frame_priors; first_frame_status_ = 1; // options set
}
FrameBasePtr Problem::applyFirstFrameOptions(const TimeStamp& _ts)
{
if (isFirstFrameApplied())
{
throw std::runtime_error("applyFirstFrameOptions can be called once!");
}
FrameBasePtr first_frame(nullptr);
// prior options set
if (first_frame_status_ == 1)
{
// Create first frame with the prior options
first_frame =
emplaceFrame(_ts, frame_types_(first_frame_values_.getKeys()), first_frame_values_, first_frame_priors_);
// notify all processors
keyFrameCallback(first_frame, nullptr);
}
// first frame status: APPLIED
first_frame_status_ = 2;
return first_frame;
}
FrameBasePtr Problem::emplaceFirstFrame(const TimeStamp& _ts, const YAML::Node& _first_frame_node)
{
setFirstFrameOptions(_first_frame_node);
return applyFirstFrameOptions(_ts);
}
FrameBasePtr Problem::emplaceFirstFrame(const TimeStamp& _ts,
const TypeComposite& _frame_types,
const VectorComposite& _frame_state,
const PriorComposite& _frame_priors)
{
setFirstFrameOptions(_frame_types, _frame_state, _frame_priors);
return applyFirstFrameOptions(_ts);
}
bool Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
{
return getMap()->save(_filename_dot_yaml, _map_name);
}
void Problem::print(int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream& _stream) const
{
_stream << std::endl;
_stream << "Wolf tree status ------------------------" << std::endl;
getHardware()->print(_depth, _factored_by, _metric, _state_blocks, _stream, "");
getTrajectory()->print(_depth, _factored_by, _metric, _state_blocks, _stream, "");
getMap()->print(_depth, _factored_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 << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
hardware_ptr_->check(log, _verbose, _stream, tabs);
// ------------------------
// TRAJECTORY branch
// ------------------------
trajectory_ptr_->check(log, _verbose, _stream, tabs);
// ------------------------
// MAP branch
// ------------------------
map_ptr_->check(log, _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 SolverBase::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