From b4d7cb6696ff55e337a0133f6a03f3032dd8ac62 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 4 Apr 2020 23:55:13 +0200
Subject: [PATCH] WIP getStateComposite in Prob, IsMot and ProcMot
---
include/core/problem/problem.h | 6 ++++--
include/core/processor/is_motion.h | 19 +++++++++----------
include/core/state_block/has_state_blocks.h | 15 +++++++++++----
src/problem/problem.cpp | 14 ++++++--------
src/processor/processor_motion.cpp | 19 +++++++++++++++++++
5 files changed, 49 insertions(+), 24 deletions(-)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index f92d39c8b..5290dd78c 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -44,8 +44,10 @@ class Problem : public std::enable_shared_from_this<Problem>
// TODO move somewhere below
public:
- StateComposite getStateComposite() const;
- StateComposite getStateComposite(const TimeStamp& _ts) const;
+ bool getStateComposite(StateComposite& _state) const;
+ bool getStateComposite(const TimeStamp& _ts, StateComposite& _state) const;
+ StateComposite getStateComposite() const;
+ StateComposite getStateComposite(const TimeStamp& _ts) const;
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 25ded4c22..e289ec298 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -23,9 +23,8 @@ class IsMotion
public:
// TODO move somewhere below
- virtual bool getStateComposite(StateComposite& _state) const = 0;
- virtual bool getStateComposite(const TimeStamp& _ts, StateComposite& _state) const = 0;
-
+ virtual bool getStateComposite(StateComposite& _state) const = 0;
+ virtual bool getStateComposite(const TimeStamp& _ts, StateComposite& _state) const = 0;
StateComposite getStateComposite() const;
StateComposite getStateComposite(const TimeStamp& _ts) const;
@@ -85,6 +84,13 @@ inline TimeStamp IsMotion::getCurrentTimeStamp() const
return ts;
}
+inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
+{
+ Eigen::VectorXd x;
+ getState(_ts, x);
+ return x;
+}
+
inline StateComposite IsMotion::getStateComposite() const
{
StateComposite state;
@@ -99,13 +105,6 @@ inline StateComposite IsMotion::getStateComposite(const TimeStamp &_ts) const
return state;
}
-inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
-{
- Eigen::VectorXd x;
- getState(_ts, x);
- return x;
-}
-
} /* namespace wolf */
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index c7809b8e0..e9e922351 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -72,7 +72,8 @@ class HasStateBlocks
void getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
Eigen::VectorXd getState(const StateStructure& _sub_structure = StateStructure()) const;
- StateComposite getStateComposite();
+ StateComposite getStateComposite() const;
+ bool getStateComposite(StateComposite& _state) const;
void setStateComposite(const StateComposite& _state);
// Perturb state
@@ -261,13 +262,19 @@ inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_struc
return state;
}
-inline StateComposite HasStateBlocks::getStateComposite()
+inline bool HasStateBlocks::getStateComposite(StateComposite& _state) const
{
- StateComposite state;
for (auto& pair_key_sb : state_block_map_)
{
- state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+ _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
}
+ return true;
+}
+
+inline StateComposite HasStateBlocks::getStateComposite() const
+{
+ StateComposite state;
+ getStateComposite(state);
return state;
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b7565f35c..5e7a4150a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -376,17 +376,15 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
_state = zeroState();
}
-inline StateComposite Problem::getStateComposite() const
+inline bool Problem::getStateComposite(StateComposite& _state) const
{
- StateComposite state;
-
if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
{
auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
if (last_kf_or_aux)
- state = last_kf_or_aux->getStateComposite();
+ _state = last_kf_or_aux->getStateComposite();
else
- state = StateComposite();
+ _state = StateComposite();
}
else // Compose from different processor motion
{
@@ -403,13 +401,13 @@ inline StateComposite Problem::getStateComposite() const
{
for (const auto& pair_key_vec : prc->getStateComposite(min_ts))
{
- if (state.count(pair_key_vec.first) == 0)
- state.insert(pair_key_vec);
+ if (_state.count(pair_key_vec.first) == 0)
+ _state.insert(pair_key_vec);
}
}
}
- return state;
+ return true;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 11db736b5..fb5661ebd 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -398,6 +398,25 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
return true;
}
+bool ProcessorMotion::getStateComposite(StateComposite& _state) const
+{
+ // TODO implement this
+ if (last_ptr_ && last_ptr_->getFrame())
+ return getLast()->getFrame()->getStateComposite(_state);
+ else
+ return false;
+}
+
+bool ProcessorMotion::getStateComposite(const TimeStamp& _ts, StateComposite& _state) const
+{
+ // TODO implement this
+ if (last_ptr_ && last_ptr_->getFrame())
+ return getLast()->getFrame()->getStateComposite(_state);
+ else
+ return false;
+}
+
+
FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin)
{
FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
--
GitLab