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