diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index dc810f2764b895923f7a0516f6e7a6e14e2ebc5d..7bc9505f2a3316de7678d54070ea92e38440ebc9 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -58,7 +58,7 @@ class Problem : public std::enable_shared_from_this<Problem> HardwareBasePtr hardware_ptr_; TrajectoryBasePtr trajectory_ptr_; MapBasePtr map_ptr_; - std::list<IsMotionPtr> processor_is_motion_list_; + std::map<int, IsMotionPtr> processor_is_motion_map_; std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_; SizeEigen state_size_, state_cov_size_, dim_; std::map<FactorBasePtr, Notification> factor_notification_map_; @@ -178,7 +178,7 @@ class Problem : public std::enable_shared_from_this<Problem> public: IsMotionPtr getProcessorIsMotion(); - std::list<IsMotionPtr> getProcessorIsMotionList(); + std::map<int,IsMotionPtr> getProcessorIsMotionMap(); @@ -431,15 +431,15 @@ inline bool Problem::isPriorSet() const inline IsMotionPtr Problem::getProcessorIsMotion() { - if (!processor_is_motion_list_.empty()) - return processor_is_motion_list_.front(); + if (not processor_is_motion_map_.empty()) + return processor_is_motion_map_.begin()->second; return nullptr; } -inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList() +inline std::map<int,IsMotionPtr> Problem::getProcessorIsMotionMap() { - return processor_is_motion_list_; + return processor_is_motion_map_; } inline SizeStd Problem::getStateBlockNotificationMapSize() const diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h index dc0487046e5013626ebc02288a00915a7df58b24..767956aa9ec849bdd98c5dc643b1f5991d2bf1fc 100644 --- a/include/core/processor/is_motion.h +++ b/include/core/processor/is_motion.h @@ -10,10 +10,34 @@ #include "core/common/wolf.h" #include "core/state_block/state_composite.h" +#include "core/utils/params_server.h" namespace wolf { +WOLF_STRUCT_PTR_TYPEDEFS(ParamsIsMotion); + +struct ParamsIsMotion +{ + std::string name; + bool state_getter = true; + int state_priority = 1; + + ParamsIsMotion() = default; + ParamsIsMotion(std::string _unique_name, const ParamsServer& _server) + { + name = _server.getParam<std::string>("processor/" + _unique_name + "/name"); + state_getter = _server.getParam<bool>("processor/" + _unique_name + "/state_getter"); + state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority"); + } + std::string print() const + { + return "name: " + name + "\n" + + "state_getter: " + std::to_string(state_getter) + "\n" + + "state_priority: " + std::to_string(state_priority) + "\n"; + } + +}; class TimeStamp; WOLF_PTR_TYPEDEFS(IsMotion); @@ -22,7 +46,7 @@ class IsMotion { public: - IsMotion(const StateStructure& _structure); + IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params); virtual ~IsMotion(); // Queries to the processor: @@ -31,6 +55,12 @@ class IsMotion virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0; VectorComposite getOdometry ( ) const; + + std::string getProcessorName() const; + bool isStateGetter() const; + int getStatePriority() const; + void setStatePriority(int); + private: void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;} @@ -42,11 +72,12 @@ class IsMotion protected: StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) VectorComposite odometry_; - + ParamsIsMotionPtr params_is_motion_; }; -inline IsMotion::IsMotion(const StateStructure& _structure) : - state_structure_(_structure) +inline IsMotion::IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params) : + state_structure_(_structure), + params_is_motion_(_params) { // } @@ -68,6 +99,26 @@ inline IsMotion::~IsMotion() { } +inline std::string IsMotion::getProcessorName() const +{ + return params_is_motion_->name; +} + +inline bool IsMotion::isStateGetter() const +{ + return params_is_motion_->state_getter; +} + +inline int IsMotion::getStatePriority() const +{ + return params_is_motion_->state_priority; +} + +inline void IsMotion::setStatePriority(int _priority) +{ + params_is_motion_->state_priority = _priority; +} + } /* namespace wolf */ #endif /* PROCESSOR_IS_MOTION_H_ */ diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 75053ab0f35d1c338092c723573d5cfbf85755b6..207bacb8eb7c9770787e00e9a811b30543708c51 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -23,7 +23,7 @@ namespace wolf WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion); -struct ParamsProcessorMotion : public ParamsProcessorBase +struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion { double max_time_span = 0.5; unsigned int max_buff_length = 10; @@ -33,7 +33,8 @@ struct ParamsProcessorMotion : public ParamsProcessorBase ParamsProcessorMotion() = default; ParamsProcessorMotion(std::string _unique_name, const ParamsServer& _server): - ParamsProcessorBase(_unique_name, _server) + ParamsProcessorBase(_unique_name, _server), + ParamsIsMotion(_unique_name, _server) { max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span"); max_buff_length = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/max_buff_length"); @@ -43,7 +44,8 @@ struct ParamsProcessorMotion : public ParamsProcessorBase } std::string print() const override { - return ParamsProcessorBase::print() + "\n" + return ParamsProcessorBase::print() + "\n" + + ParamsIsMotion::print() + "\n" + "max_time_span: " + std::to_string(max_time_span) + "\n" + "max_buff_length: " + std::to_string(max_buff_length) + "\n" + "dist_traveled: " + std::to_string(dist_traveled) + "\n" diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 40396509fd506ce25a65a8e510b9b4dde8818f64..c2460d5d98a30f86d42bea47ede344e1076b0abd 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -45,7 +45,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) : hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>()), map_ptr_(std::make_shared<MapBase>()), - processor_is_motion_list_(std::list<IsMotionPtr>()), + processor_is_motion_map_(), frame_structure_(_frame_structure), prior_options_(std::make_shared<PriorOptions>()) { @@ -420,10 +420,10 @@ TimeStamp Problem::getTimeStamp ( ) const { TimeStamp ts = TimeStamp::Invalid(); - for (const auto& prc : processor_is_motion_list_) - if (prc->getTimeStamp().ok()) - if ( (not ts.ok() ) or prc->getTimeStamp() > ts) - ts = prc->getTimeStamp(); + for (const auto& prc_pair : processor_is_motion_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()) @@ -447,13 +447,15 @@ VectorComposite Problem::getState(const StateStructure& _structure) const VectorComposite state; - // compose the states of all processor motions into one only state - for (const auto& prc : processor_is_motion_list_) + // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state + for (const auto& prc_pair : processor_is_motion_map_) { - const auto& prc_state = prc->getState(structure); + const auto& prc_state = prc_pair.second->getState(structure); + + // transfer processor vector blocks to problem state for (const auto& pair_key_vec : prc_state) { - if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet + if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority state.insert(pair_key_vec); } } @@ -493,16 +495,15 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ VectorComposite state; - for (const auto& prc : processor_is_motion_list_) + // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state + for (const auto& prc_pair : processor_is_motion_map_) { - const auto& prc_state = prc->getState(_ts, structure); + const auto& prc_state = prc_pair.second->getState(_ts, structure); // transfer processor vector blocks to problem state for (const auto& pair_key_vec : prc_state) { - const auto& key = pair_key_vec.first; - - if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list + if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority state.insert(pair_key_vec); } } @@ -562,23 +563,40 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm) } -void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr) +void Problem::addProcessorIsMotion(IsMotionPtr _is_motion_ptr) { - processor_is_motion_list_.push_back(_processor_motion_ptr); - appendToStructure(_processor_motion_ptr->getStateStructure()); + // Check if is state getter + if (not _is_motion_ptr->isStateGetter()) + { + WOLF_WARN("Problem::addProcessorIsMotion: adding a IsMotion processor with state_getter=false. Not adding this processor"); + return; + } + + // check duplicated priority + while (processor_is_motion_map_.count(_is_motion_ptr->getStatePriority()) == 1) + { + WOLF_ERROR("Problem::addProcessorIsMotion: adding a IsMotion processor with priority = ", + _is_motion_ptr->getStatePriority(), + " which is already taken. Trying to add it with priority = ", + _is_motion_ptr->getStatePriority()+1); + _is_motion_ptr->setStatePriority(_is_motion_ptr->getStatePriority()+1); + } + + // add to map ordered by priority + processor_is_motion_map_.emplace(_is_motion_ptr->getStatePriority(), _is_motion_ptr); + appendToStructure(_is_motion_ptr->getStateStructure()); } void Problem::clearProcessorIsMotion(IsMotionPtr proc) { - auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc); - if (it != processor_is_motion_list_.end()){ - processor_is_motion_list_.erase(it); - } + WOLF_WARN_COND(processor_is_motion_map_.count(proc->getStatePriority()) == 0, "Problem::clearProcessorIsMotion: missing processor"); + + processor_is_motion_map_.erase(proc->getStatePriority()); // rebuild frame structure with remaining motion processors frame_structure_.clear(); - for (const auto& pm : processor_is_motion_list_) - appendToStructure(pm->getStateStructure()); + for (const auto& pm : processor_is_motion_map_) + appendToStructure(pm.second->getStateStructure()); } VectorComposite Problem::stateZero ( const StateStructure& _structure ) const diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp index a17142e80bf69198674b29f0dc5f3bde955412a4..106a60df2bc0077c8903208d9c851d762efb58e2 100644 --- a/src/processor/is_motion.cpp +++ b/src/processor/is_motion.cpp @@ -5,6 +5,11 @@ using namespace wolf; void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr) { + if (not isStateGetter()) + { + WOLF_WARN("IsMotion::addToProblem: IsMotion processor with state_getter=false. Not adding this processor"); + return; + } setOdometry(_prb_ptr->stateZero(state_structure_)); _prb_ptr->addProcessorIsMotion(_motion_ptr); } diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 2fb9f2bab8cbd88b5239a72168610decda30c38f..17aa676f434d049875ecd8405c842b5ec95d1653 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -23,7 +23,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, SizeEigen _calib_size, ParamsProcessorMotionPtr _params_motion) : ProcessorBase(_type, _dim, _params_motion), - IsMotion(_state_structure), + IsMotion(_state_structure, _params_motion), params_motion_(_params_motion), processing_step_(RUNNING_WITHOUT_KF), x_size_(_state_size), diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp index 54fadc44d8ac752f8d522374e9ccdb8812241585..43c8c48a317b9e6a5f261075fe170614cd8abf1c 100644 --- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp +++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp @@ -31,11 +31,17 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame) params_swdr_->n_frames_recent - 1)->second; // compose motion captures for all processors motion - for (auto is_motion : getProblem()->getProcessorIsMotionList()) + for (auto is_motion_pair : getProblem()->getProcessorIsMotionMap()) { - auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion); + auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion_pair.second); if (proc_motion == nullptr) + { + // FIXME: IsMotion::mergeCaptures pure virtual in IsMotion without need of casting + WOLF_INFO("TreeManagerSlidingWindowDualRate::keyFrameCallback: IsMotion ", + is_motion_pair.second->getProcessorName(), + " couldn't be casted to ProcessorMotion. Not merging its captures..."); continue; + } auto cap_prev = std::static_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor())); auto cap_next = std::static_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor())); diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml index 7e0b0ccb110c0545ff24a9726c035124f0d1686a..6c5ed47c2efc2afc3ba960b075e60cea73b58a73 100644 --- a/test/yaml/params_problem_odom_3d.yaml +++ b/test/yaml/params_problem_odom_3d.yaml @@ -41,3 +41,6 @@ config: angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg) unmeasured_perturbation_std: 0.00111 + + state_getter: true + state_priority: 1 diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml index 6a69b63a6918e13b0f492c7b51ef6dfe3c97ba94..fa43fecb397dff295b683cfcd6a282adac61cc44 100644 --- a/test/yaml/params_tree_manager1.yaml +++ b/test/yaml/params_tree_manager1.yaml @@ -46,3 +46,7 @@ config: angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) unmeasured_perturbation_std: 0.00111 + + state_getter: true + state_priority: 1 + diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml index 879421e7915581f52355d1874ebb78f302b768df..f37e31459d9a883aca9eb12898aa5ac285e63210 100644 --- a/test/yaml/params_tree_manager2.yaml +++ b/test/yaml/params_tree_manager2.yaml @@ -45,3 +45,7 @@ config: angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) unmeasured_perturbation_std: 0.00111 + + state_getter: true + state_priority: 1 + diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml index 704e8530ed2c8b8eb99ac35c29c36f62d01a3fb4..277810464d6f619ed342ce3706bec30d7ca8e5f9 100644 --- a/test/yaml/params_tree_manager_sliding_window1.yaml +++ b/test/yaml/params_tree_manager_sliding_window1.yaml @@ -44,3 +44,7 @@ config: angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) unmeasured_perturbation_std: 0.00111 + + state_getter: true + state_priority: 1 + diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml index 701fc6fbfb71e35c800563224cf7a86cce6d846b..f22fdde12f065d17accb122ef7f8d1728ef6fb6c 100644 --- a/test/yaml/params_tree_manager_sliding_window2.yaml +++ b/test/yaml/params_tree_manager_sliding_window2.yaml @@ -44,3 +44,7 @@ config: angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) unmeasured_perturbation_std: 0.00111 + + state_getter: true + state_priority: 1 + diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml index 47d81f409a5ed09d37ba3b307c2fb23152759791..8f00f6499df2c96c9993bd6c486554579e7fbab9 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml @@ -52,3 +52,7 @@ config: angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg) unmeasured_perturbation_std: 0.00111 + + state_getter: true + state_priority: 1 + diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml index 494d27be3f0b9e3c68ef33919451a0f7e6ba065c..114e865bdc3a86b6d0cddf42e0f5360b7b2d5928 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml @@ -46,3 +46,6 @@ config: angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg) unmeasured_perturbation_std: 0.00111 + + state_getter: true + state_priority: 1