From 2e63a58cc4f6192d7c39d84e34a8c4a0e75a5df5 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 4 Mar 2021 18:20:10 +0100
Subject: [PATCH] implemented and gtests fixed
---
include/core/problem/problem.h | 12 ++--
include/core/processor/is_motion.h | 59 +++++++++++++++--
include/core/processor/processor_motion.h | 8 ++-
src/problem/problem.cpp | 64 ++++++++++++-------
src/processor/is_motion.cpp | 5 ++
src/processor/processor_motion.cpp | 2 +-
.../tree_manager_sliding_window_dual_rate.cpp | 10 ++-
test/yaml/params_problem_odom_3d.yaml | 3 +
test/yaml/params_tree_manager1.yaml | 4 ++
test/yaml/params_tree_manager2.yaml | 4 ++
.../params_tree_manager_sliding_window1.yaml | 4 ++
.../params_tree_manager_sliding_window2.yaml | 4 ++
...ree_manager_sliding_window_dual_rate3.yaml | 4 ++
...ger_sliding_window_dual_rate_baseline.yaml | 3 +
14 files changed, 147 insertions(+), 39 deletions(-)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index dc810f276..7bc9505f2 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 dc0487046..767956aa9 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 75053ab0f..207bacb8e 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 40396509f..c2460d5d9 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 a17142e80..106a60df2 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 2fb9f2bab..17aa676f4 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 54fadc44d..43c8c48a3 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 7e0b0ccb1..6c5ed47c2 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 6a69b63a6..fa43fecb3 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 879421e79..f37e31459 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 704e8530e..277810464 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 701fc6fbf..f22fdde12 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 47d81f409..8f00f6499 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 494d27be3..114e865bd 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
--
GitLab