diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
index 04d0ff166981259edc9ca51dc3e39260fd427a62..907bbfab3be6e9f66feb6c027f34fa752fd893c4 100644
--- a/demos/hello_wolf/yaml/processor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -10,3 +10,6 @@ keyframe_vote:
max_buff_length: 999
cov_det: 999
apply_loss_function: true
+
+state_getter: true
+state_priority: 1
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index dc810f2764b895923f7a0516f6e7a6e14e2ebc5d..3d0eba08b8e234a1adcd3ab4bd00a149e10e6f65 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_;
@@ -174,14 +174,12 @@ class Problem : public std::enable_shared_from_this<Problem>
* Add a new processor of type is motion to the processor is motion list.
*/
void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
- void clearProcessorIsMotion(IsMotionPtr proc);
+ void removeProcessorIsMotion(IsMotionPtr proc);
public:
IsMotionPtr getProcessorIsMotion();
- std::list<IsMotionPtr> getProcessorIsMotionList();
-
-
-
+ std::map<int,IsMotionPtr>& getProcessorIsMotionMap();
+ const std::map<int,IsMotionPtr>& getProcessorIsMotionMap() const;
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
@@ -321,6 +319,7 @@ class Problem : public std::enable_shared_from_this<Problem>
TimeStamp getTimeStamp ( ) const;
VectorComposite getState ( const StateStructure& _structure = "" ) const;
VectorComposite getState ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
+ VectorComposite getOdometry ( const StateStructure& _structure = "" ) const;
// Zero state provider
VectorComposite stateZero ( const StateStructure& _structure = "" ) const;
@@ -431,15 +430,19 @@ 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::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap()
+{
+ return processor_is_motion_map_;
+}
-inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
+inline const std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap() const
{
- 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..a17ac88e7f2d78bc83e5866f10e85173af380638 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -10,10 +10,31 @@
#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
+{
+ bool state_getter = true;
+ int state_priority = 1;
+
+ ParamsIsMotion() = default;
+ ParamsIsMotion(std::string _unique_name, const ParamsServer& _server)
+ {
+ 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 "state_getter: " + std::to_string(state_getter) + "\n"
+ + "state_priority: " + std::to_string(state_priority) + "\n";
+ }
+
+};
class TimeStamp;
WOLF_PTR_TYPEDEFS(IsMotion);
@@ -22,7 +43,7 @@ class IsMotion
{
public:
- IsMotion(const StateStructure& _structure);
+ IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params);
virtual ~IsMotion();
// Queries to the processor:
@@ -31,8 +52,11 @@ class IsMotion
virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
VectorComposite getOdometry ( ) const;
- private:
- void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;}
+ void setOdometry(const VectorComposite&);
+
+ bool isStateGetter() const;
+ int getStatePriority() const;
+ void setStatePriority(int);
public:
const StateStructure& getStateStructure ( ) { return state_structure_; };
@@ -42,16 +66,16 @@ 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)
{
//
}
-
inline wolf::VectorComposite IsMotion::getOdometry ( ) const
{
return odometry_;
@@ -60,14 +84,27 @@ inline wolf::VectorComposite IsMotion::getOdometry ( ) const
}
///// IMPLEMENTATION ///////
-#include "core/common/time_stamp.h"
-
namespace wolf{
inline IsMotion::~IsMotion()
{
}
+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/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index c7a125ac0d96b1d1b0799ab9e983bccf76bf36db..7989fd807eab08187c802e0386af02de56ef4f1b 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -69,6 +69,8 @@ class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
void setZero();
+ bool includesStructure(const StateStructure &_structure) const;
+
friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 549b1435222c8cda2c0615a54632d6c8c5b63840..4b278bf8bf8ef16eca8ea61ed4702d85358b6730 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -21,13 +21,11 @@ FrameBase::FrameBase(const TimeStamp& _ts,
frame_id_(++frame_id_count_),
time_stamp_(_ts)
{
- for (const auto& pair_key_vec : _state)
- {
- const auto& key = pair_key_vec.first;
- const auto& vec = pair_key_vec.second;
-
- StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
+ assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure");
+ for (auto key : getStructure())
+ {
+ StateBlockPtr sb = FactoryStateBlock::create(string(1,key), _state.at(key), false); // false = non fixed
addStateBlock(key, sb, getProblem());
}
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 40396509fd506ce25a65a8e510b9b4dde8818f64..0bb18fd52c7ec787c65298afcae06e19abff3230 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);
}
}
@@ -536,6 +537,49 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
return state;
}
+VectorComposite Problem::getOdometry(const StateStructure& _structure) const
+{
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
+ VectorComposite odom_state;
+
+ // 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_pair.second->getOdometry();
+
+ // transfer processor vector blocks to problem state
+ for (const 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 (const 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_;
@@ -562,23 +606,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)
+void Problem::removeProcessorIsMotion(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
@@ -962,6 +1023,7 @@ void Problem::setPriorOptions(const std::string& _mode,
if (prior_options_->mode != "nothing")
{
assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
+ assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
WOLF_TRACE("prior state: ", _state);
WOLF_TRACE("prior time tolerance: ", _time_tolerance);
@@ -970,11 +1032,13 @@ void Problem::setPriorOptions(const std::string& _mode,
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 && "sigma is not positive");
+ assert(isPositive && "prior sigma is not positive");
MatrixComposite Q;
for (const auto& pair_key_sig : _sigma)
@@ -999,8 +1063,11 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- prior_keyframe = emplaceFrame(_ts,
- prior_options_->state);
+ prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
+
+ // Update origin for odometry processors
+ for (auto proc_pair : processor_is_motion_map_)
+ proc_pair.second->setOdometry(prior_options_->state);
if (prior_options_->mode == "fix")
prior_keyframe->fix();
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
index a17142e80bf69198674b29f0dc5f3bde955412a4..b3a510472087094a38682081e7d27cd3b00a07b6 100644
--- a/src/processor/is_motion.cpp
+++ b/src/processor/is_motion.cpp
@@ -5,6 +5,20 @@ 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);
}
+
+void IsMotion::setOdometry(const VectorComposite& _odom)
+{
+ assert(_odom.includesStructure(state_structure_) && "IsMotion::setOdometry(): provided odom has not any key.");
+
+ for (auto key : state_structure_)
+ if (_odom.count(key) != 0)
+ odometry_[key] = _odom.at(key); //overwrite or insert only keys of the state_structure_
+}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index e8f891b8c51098909b5db479f7e3d3a8139457c0..794699f4533a7d2f5e6271e8f31b05b0b36cbd69 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -91,7 +91,7 @@ void ProcessorBase::remove()
ProblemPtr P = getProblem();
auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() );
if(P && this_proc_cast_attempt )
- P->clearProcessorIsMotion(this_proc_cast_attempt);
+ P->removeProcessorIsMotion(this_proc_cast_attempt);
}
// remove from upstream
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/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 0f4efab6f1f093d8f1425d0855a453c6370f1c93..169bc82ca8edd1fa751bac1e309aa2ce1eb95379 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -81,6 +81,14 @@ Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
return x;
}
+bool VectorComposite::includesStructure(const StateStructure &_structure) const
+{
+ for (auto key : _structure)
+ if (count(key) == 0)
+ return false;
+ return true;
+}
+
std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
{
for (const auto &pair_key_vec : _x)
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..c590c4b718504cfd357a15df2bdc27f379a9deea 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 ",
+ std::dynamic_pointer_cast<ProcessorBase>(is_motion_pair.second)->getName(),
+ " 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/CMakeLists.txt b/test/CMakeLists.txt
index 885c1f53a456deb4b74392d011d560cb7216bafe..8a5eb8fa3a068dca72974a86bdcdee1fd9453baf 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -84,6 +84,10 @@ target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
+# IsMotion classes test
+wolf_add_gtest(gtest_is_motion gtest_is_motion.cpp)
+target_link_libraries(gtest_is_motion ${PLUGIN_NAME})
+
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
target_link_libraries(gtest_local_param ${PLUGIN_NAME})
diff --git a/test/dummy/processor_is_motion_dummy.h b/test/dummy/processor_is_motion_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..dca4808c90602d550156a9e486514480b2c7f6b1
--- /dev/null
+++ b/test/dummy/processor_is_motion_dummy.h
@@ -0,0 +1,76 @@
+/*
+ * processor_is_motion_dummy.h
+ *
+ * Created on: Mar 8, 2021
+ * Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+
+#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorIsMotionDummy);
+
+struct ParamsProcessorIsMotionDummy : public ParamsProcessorBase, public ParamsIsMotion
+{
+ std::string state_structure = "PO";
+
+ ParamsProcessorIsMotionDummy() = default;
+ ParamsProcessorIsMotionDummy(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server),
+ ParamsIsMotion(_unique_name, _server)
+ {
+
+ };
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorIsMotionDummy);
+
+class ProcessorIsMotionDummy : public ProcessorBase, public IsMotion
+{
+ public:
+ ProcessorIsMotionDummy(ParamsProcessorIsMotionDummyPtr _params) :
+ ProcessorBase("ProcessorIsMotionDummy", 2, _params),
+ IsMotion(_params->state_structure, _params)
+ {}
+ ~ProcessorIsMotionDummy(){};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorIsMotionDummy, ParamsProcessorIsMotionDummy);
+
+ void configure(SensorBasePtr _sensor) override {};
+ void processCapture(CaptureBasePtr) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
+ bool triggerInCapture(CaptureBasePtr) const override { return false; };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override { return false; };
+ bool storeKeyFrame(FrameBasePtr) override { return false; };
+ bool storeCapture(CaptureBasePtr) override { return false; };
+ bool voteForKeyFrame() const override { return false; };
+ TimeStamp getTimeStamp() const override {return TimeStamp(0);};
+
+ VectorComposite getState(const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorIsMotionDummy);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorIsMotionDummy);
+} // namespace wolf
+
+#endif /* TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ */
diff --git a/test/gtest_is_motion.cpp b/test/gtest_is_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0c8b8fa620a1f8b5261d9a5ca13a6299c896d6d9
--- /dev/null
+++ b/test/gtest_is_motion.cpp
@@ -0,0 +1,185 @@
+//Wolf
+#include "core/utils/utils_gtest.h"
+
+#include "core/processor/is_motion.h"
+#include "dummy/processor_is_motion_dummy.h"
+#include "core/sensor/sensor_odom_2d.h"
+
+#include "core/problem/problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+class IsMotionTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sen;
+ ProcessorBasePtr prc1, prc2, prc3;
+ IsMotionPtr im1, im2, im3;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ double dt = 0.01;
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("POV", 2);
+
+ // Install odom sensor
+ sen = problem->installSensor("SensorOdom2d",
+ "odometer",
+ Vector3d(0,0,0),
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+ // Install 3 odom processors
+ ParamsProcessorIsMotionDummyPtr prc1_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc1_params->time_tolerance = dt/2;
+ prc1_params->state_structure = "PO";
+ prc1_params->state_getter = false;
+ prc1 = problem->installProcessor("ProcessorIsMotionDummy",
+ "not getter processor",
+ sen,
+ prc1_params);
+ im1 = std::dynamic_pointer_cast<IsMotion>(prc1);
+
+ ParamsProcessorIsMotionDummyPtr prc2_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc2_params->time_tolerance = dt/2;
+ prc2_params->state_structure = "PO";
+ prc2_params->state_getter = true;
+ prc2_params->state_priority = 1;
+ prc2 = problem->installProcessor("ProcessorIsMotionDummy",
+ "getter processor",
+ sen,
+ prc2_params);
+ im2 = std::dynamic_pointer_cast<IsMotion>(prc2);
+
+ ParamsProcessorIsMotionDummyPtr prc3_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc3_params->time_tolerance = dt/2;
+ prc3_params->state_structure = "POV";
+ prc3_params->state_getter = true;
+ prc3_params->state_priority = 1;
+ prc3 = problem->installProcessor("ProcessorIsMotionDummy",
+ "getter processor low priority",
+ sen,
+ prc3_params);
+ im3 = std::dynamic_pointer_cast<IsMotion>(prc3);
+ }
+};
+
+/*
+ * Things to be tested:
+ * - state_getter
+ * - state_priority
+ * - Duplicated priority (Problem handles this)
+ * - setOdometry (stateStructures)
+ * - getOdomtry
+ * - Problem::getState/getOdometry (state_getter, state_priority)
+ */
+
+TEST_F(IsMotionTest, install)
+{
+ // All isMotion() = true
+ ASSERT_TRUE (prc1->isMotion());
+ ASSERT_TRUE (prc2->isMotion());
+ ASSERT_TRUE (prc3->isMotion());
+ ASSERT_TRUE (im1 != nullptr);
+ ASSERT_TRUE (im2 != nullptr);
+ ASSERT_TRUE (im3 != nullptr);
+
+ // well configured
+ ASSERT_FALSE(im1->isStateGetter());
+ ASSERT_TRUE(im2->isStateGetter());
+ ASSERT_TRUE(im3->isStateGetter());
+ ASSERT_EQ(im2->getStatePriority(), 1);
+ ASSERT_EQ(im3->getStatePriority(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
+ ASSERT_EQ(im1->getStateStructure(), "PO");
+ ASSERT_EQ(im2->getStateStructure(), "PO");
+ ASSERT_EQ(im3->getStateStructure(), "POV");
+
+ // Only 2 and 3 in problem::processor_is_motion_map_
+ ASSERT_EQ(problem->getProcessorIsMotionMap().size(), 2);
+ ASSERT_EQ(problem->getProcessorIsMotionMap().begin()->second, im2);
+ ASSERT_EQ(std::next(problem->getProcessorIsMotionMap().begin())->second, im3);
+}
+
+TEST_F(IsMotionTest, odometry)
+{
+ VectorComposite odom_p("P"); // missing P key
+ odom_p['P'] = Vector2d::Zero();
+ VectorComposite odom_pov("POV"); // key V not needed by prc1 and prc2
+ odom_pov['P'] = Vector2d::Zero();
+ odom_pov['O'] = Vector1d::Zero();
+ odom_pov['V'] = Vector2d::Zero();
+
+ // Error: required PO keys to be added
+ ASSERT_DEATH({im1->setOdometry(odom_p);},"");
+ im1->setOdometry(odom_pov);
+ im2->setOdometry(odom_pov);
+ im3->setOdometry(odom_pov);
+
+ // im1 ->set odom = 0, 0, 0
+ VectorComposite odom1("PO");
+ odom1['P'] = Vector2d::Zero();
+ odom1['O'] = Vector1d::Zero();
+ im1->setOdometry(odom1);
+ auto odom1_get = im1->getOdometry();
+ EXPECT_TRUE(odom1_get.count('P') == 1);
+ EXPECT_TRUE(odom1_get.count('O') == 1);
+ EXPECT_MATRIX_APPROX(odom1_get.at('P'), odom1.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
+
+ // im1 ->set odom = 1, 1, 1
+ VectorComposite odom2("PO");
+ odom2['P'] = Vector2d::Ones();
+ odom2['O'] = Vector1d::Ones();
+ im2->setOdometry(odom2);
+ auto odom2_get = im2->getOdometry();
+ EXPECT_TRUE(odom2_get.count('P') == 1);
+ EXPECT_TRUE(odom2_get.count('O') == 1);
+ EXPECT_MATRIX_APPROX(odom2_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
+
+ // im1 ->set odom = 2, 2, 2, 2, 2
+ VectorComposite odom3("POV");
+ odom3['P'] = 2 * Vector2d::Ones();
+ odom3['O'] = 2 * Vector1d::Ones();
+ odom3['V'] = 2 * Vector2d::Ones();
+ im3->setOdometry(odom3);
+ auto odom3_get = im3->getOdometry();
+ EXPECT_TRUE(odom3_get.count('P') == 1);
+ EXPECT_TRUE(odom3_get.count('O') == 1);
+ EXPECT_TRUE(odom3_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(odom3_get.at('P'), odom3.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom3_get.at('O'), odom3.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom3_get.at('V'), odom3.at('V'), 1e-9);
+
+ // problem::getOdometry(): by priority P and O should come from im2 and V from im3
+ auto odom_get = problem->getOdometry();
+ EXPECT_TRUE(odom_get.count('P') == 1);
+ EXPECT_TRUE(odom_get.count('O') == 1);
+ EXPECT_TRUE(odom_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(odom_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom_get.at('O'), odom2.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom_get.at('V'), odom3.at('V'), 1e-9);
+
+ // problem::getState(): by priority P and O should come from im2 and V from im3
+ auto state_get = problem->getState();
+ EXPECT_TRUE(state_get.count('P') == 1);
+ EXPECT_TRUE(state_get.count('O') == 1);
+ EXPECT_TRUE(state_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(state_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(state_get.at('O'), odom2.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(state_get.at('V'), odom3.at('V'), 1e-9);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index c3277078df9300439a6dee3721f0084df9751522..4497954899991b26100d23af38130e8218ed4a14 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -48,10 +48,16 @@ TEST(ProcessorBase, IsMotion)
auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d",
+ "odometer",
+ Vector3d(0,0,0),
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d",
+ "odom processor",
+ sens_odo,
+ proc_odo_params);
ASSERT_FALSE(proc_trk->isMotion());
ASSERT_TRUE (proc_odo->isMotion());
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