diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c4c325aac18b0435357c73af53e44f7a208186ae..b40ef049b4303fa7c58a1a47d1e06918b1b925c9 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -116,13 +116,18 @@ public:
*
* elements are ordered from most recent to oldest
*/
- std::map<TimeStamp,T> getContainer();
+ const std::map<TimeStamp,T>& getContainer();
/**\brief Remove all packs in the buffer with a time stamp older than the specified
*
*/
void removeUpTo(const TimeStamp& _time_stamp);
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpToLower(const TimeStamp& _time_stamp);
+
/**\brief Clear the buffer
*
*/
@@ -495,7 +500,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
}
template <typename T>
-std::map<TimeStamp,T> Buffer<T>::getContainer()
+const std::map<TimeStamp,T>& Buffer<T>::getContainer()
{
return container_;
}
@@ -525,6 +530,13 @@ inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
container_.erase(container_.begin(), post); // erasing by range
}
+template <typename T>
+inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.lower_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
template <typename T>
inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
const TimeStamp& _time_stamp2, const double& _time_tolerance2)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 88ee84a24470cc715adc8bf512db0d8004e827b0..bc42138ff00bd09aa8dbc3e127edac3a986570c3 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -147,6 +147,7 @@ class ProcessorMotion : public ProcessorBase
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
@@ -465,6 +466,7 @@ class ProcessorMotion : public ProcessorBase
protected:
// Attributes
+ std::string state_structure_;///< The structure of the state vector (to retrieve state blocks from frames)
SizeEigen x_size_; ///< The size of the state vector
SizeEigen data_size_; ///< the size of the incoming data
SizeEigen delta_size_; ///< the size of the deltas
@@ -520,7 +522,7 @@ inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const
inline Eigen::VectorXd ProcessorMotion::getCurrentState() const
{
- Eigen::VectorXd x(getProblem()->getFrameStructureSize());
+ Eigen::VectorXd x;
getCurrentState(x);
return x;
}
@@ -531,11 +533,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
// ensure proper size of the provided reference
- _x.resize( getProblem()->getFrameStructureSize() );
+ Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
+ _x.resize( curr_x.size() );
// do get timestamp and state corrected by possible self-calibrations
double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+ statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ade367fb88e7e0e987b087d31130dee0e78736ff..a93cf68a1cbdfa95c7fffe4f58a04d2fbdda04a7 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -65,10 +65,14 @@ class HasStateBlocks
// States
virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
+ void getState(std::string structure, Eigen::VectorXd& _state) const;
+ Eigen::VectorXd getState(std::string structure) const;
Eigen::VectorXd getState() const;
void getState(Eigen::VectorXd& _state) const;
unsigned int getSize() const;
+ unsigned int getSize(std::string _sub_structure) const;
unsigned int getLocalSize() const;
+ unsigned int getLocalSize(std::string _sub_structure) const;
private:
std::string structure_;
@@ -232,21 +236,41 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _
}
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const
+// _sub_structure can be either stateblock structure of the node or a subset of this structure
+inline void HasStateBlocks::getState(std::string _sub_structure, Eigen::VectorXd& _state) const
{
- _state.resize(getSize());
+
+ _state.resize(getSize(_sub_structure));
unsigned int index = 0;
- for (const char key : getStructure())
+ for (const char key : _sub_structure)
{
const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+
_state.segment(index,sb->getSize()) = sb->getState();
index += sb->getSize();
}
}
+inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
+{
+ Eigen::VectorXd state;
+
+ getState(_sub_structure, state);
+
+ return state;
+}
+
+inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const
+{
+ getState(getStructure(), _state);
+}
+
inline Eigen::VectorXd HasStateBlocks::getState() const
{
Eigen::VectorXd state;
@@ -256,19 +280,44 @@ inline Eigen::VectorXd HasStateBlocks::getState() const
return state;
}
+
inline unsigned int HasStateBlocks::getSize() const
+{
+ return getSize(structure_);
+}
+
+inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
{
unsigned int size = 0;
- for (const auto& pair_key_sb : getStateBlockMap())
- size += pair_key_sb.second->getSize();
+ for (const char key : _sub_structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getSize();
+ }
+
return size;
}
inline unsigned int HasStateBlocks::getLocalSize() const
+{
+ return getLocalSize(structure_);
+}
+
+inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
{
unsigned int size = 0;
- for (const auto& pair_key_sb : getStateBlockMap())
- size += pair_key_sb.second->getLocalSize();
+ for (const char key : _sub_structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ size += sb->getLocalSize();
+ }
+
return size;
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c25761dc9c73a56d9d7e0e728c0d7e794844aa8d..68abd7c7fbffdd9760a312d0ef971d2043d8cbab 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -975,19 +975,29 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
if (metric)
{
cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp();
- cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- cout << endl;
- }
- if (state_blocks)
- {
- cout << " sb:";
- for (const auto& sb : F->getStateBlockVec())
- {
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ << F->getTimeStamp().get();
+ cout << ",\t x = ( " << std::setprecision(2) << endl;
+ for (auto sb_name: F->getStructure()){
+ auto sb = F->getStateBlock(sb_name);
+ cout << " " << sb_name;
+ if (state_blocks){
+ cout << "," << (sb->isFixed() ? "Fix" : "Est");
+ }
+ cout << ": " << sb->getState().transpose() << "\n";
}
- cout << endl;
+ cout << " )" << endl;
+ // cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ // cout << endl;
}
+ // if (state_blocks)
+ // {
+ // cout << " sb:";
+ // for (const auto& sb : F->getStateBlockVec())
+ // {
+ // cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ // }
+ // cout << endl;
+ // }
if (depth >= 2)
{
// Captures =======================================================================================
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c667b7193bc3b566782351371c7d4f314799a93b..2eafec4c0306d51fdc9a756f5f395342d2e6f792 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -3,6 +3,7 @@ namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
+ std::string _state_structure,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
@@ -12,6 +13,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
ProcessorBase(_type, _params_motion),
params_motion_(_params_motion),
processing_step_(RUNNING_WITHOUT_PACK),
+ state_structure_(_state_structure),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 5ad5e732d52937857a50e4ee3c4826ebfcf4f6d8..7841bfbbf4244eba7b559b889022446439bb117e 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -5,7 +5,7 @@ namespace wolf
{
ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
- ProcessorMotion("ProcessorOdom2D", 3, 3, 3, 2, 0, _params),
+ ProcessorMotion("ProcessorOdom2D", "PO", 3, 3, 3, 2, 0, _params),
params_odom_2D_(_params)
{
unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index f0241f7e1ce62ee203948fb6f37394b4bcd4e3ce..4282c09ef402a1d51b2086b049b2a9a6974fa0ff 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) :
- ProcessorMotion("ProcessorOdom3D", 7, 7, 6, 6, 0, _params),
+ ProcessorMotion("ProcessorOdom3D", "PO", 7, 7, 6, 6, 0, _params),
params_odom_3D_ (_params),
k_disp_to_disp_ (0),
k_disp_to_rot_ (0),