diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
index 835034af6fa66ecdc8f5e0b00de9b1b2c67199fe..1203b217e4f07c3e02a287f63b0bace4ba46ede2 100644
--- a/demos/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -60,8 +60,6 @@ class ProcessorRangeBearing : public ProcessorBase
virtual bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
virtual bool voteForKeyFrame () const override {return false;}
- VectorComposite stateZero() const override {return VectorComposite("PO", {2,1});}
-
/** \brief store key frame
*
* Returns true if the key frame should be stored
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 8f5bb0e509d0d6d563b189857cfcf60f7720b6b9..cdf26f2e3d87e234a66aabe3ef1370eb49fde8e8 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -313,6 +313,7 @@ class Problem : public std::enable_shared_from_this<Problem>
VectorComposite getState ( ) const;
VectorComposite getState (const TimeStamp& _ts ) const;
TimeStamp getTimeStamp ( ) const;
+ VectorComposite stateZero ( const StateStructure& _structure = "" ) const;
// Zero state provider
Eigen::VectorXd zeroState ( ) const;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index b91a14395ec146661485ed4e07e9df42199b3d7a..00d5698016447ede93473c94fc36b2a57179beeb 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -362,8 +362,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
void setVotingAuxActive(bool _voting_active = true);
- virtual VectorComposite stateZero() const = 0;
-
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 7875c7974af939d5982413e7fd5455287e0e40ce..0b873564bb7339e9acf63f1739076b9baa4ec29a 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -75,7 +75,6 @@ class ProcessorOdom2d : public ProcessorMotion
virtual Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
- virtual VectorComposite stateZero() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 7f76c128e5fcfce94baff60bdac907b9fbf1d0b3..49eaf4fad2788dde0499021ec89fb58d228cb65b 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -93,7 +93,6 @@ class ProcessorOdom3d : public ProcessorMotion
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
- virtual VectorComposite stateZero() const override;
bool voteForKeyFrame() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d437e313bbdd87b2dba64d1fd364932290509b39..dd67509d1d7169309a815aa6775859760c46202b 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -23,6 +23,7 @@
#include "core/utils/loader.h"
#include "core/utils/check_log.h"
#include "core/math/covariance.h"
+#include "core/state_block/factory_state_block.h"
// IRI libs includes
@@ -379,42 +380,6 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
_time_stamp);
}
-//Eigen::VectorXd Problem::getState() const
-//{
-// Eigen::VectorXd state(getFrameStructureSize());
-// getState(state);
-// return state;
-//}
-
-//void Problem::getState(Eigen::VectorXd& _state) const
-//{
-// TimeStamp ts; // throwaway timestamp
-// getStateAndStamp(_state, ts);
-//}
-
-//void Problem::getStateAndStamp(VectorComposite& _state, TimeStamp& _ts) const
-//{
-// if (!processor_is_motion_list_.empty())
-// {
-// // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
-// std::list<TimeStamp> proc_is_motion_current_ts;
-// for (auto proc: processor_is_motion_list_){
-// proc_is_motion_current_ts.push_back(proc->getTimeStamp());
-// }
-// auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
-// getState(*min_it, _state);
-// _ts = *min_it;
-// }
-// else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
-// {
-// // kind of redundant with getState(_ts, _state)
-// _ts = trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp();
-// _state = trajectory_ptr_->getLastKeyOrAuxFrame()->getStateVector();
-// }
-// else
-// _state = zeroState();
-//}
-
TimeStamp Problem::getTimeStamp ( ) const
{
if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
@@ -435,32 +400,26 @@ TimeStamp Problem::getTimeStamp ( ) const
}
}
-VectorComposite Problem::getState() const
+
+VectorComposite Problem::getState(const StateStructure& _structure) const
{
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
VectorComposite 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->getState();
+ state = last_kf_or_aux->getState(structure);
else
- state = VectorComposite();
+ state = stateZero(structure);
}
else // Compose from different processor motion
{
- // get a timestamp where all processor motions have state
- std::list<TimeStamp> proc_motion_ts;
- for (const auto& prc : processor_is_motion_list_)
- {
- proc_motion_ts.push_back(prc->getTimeStamp());
- }
- auto min_ts = *(std::min_element(proc_motion_ts.begin(), proc_motion_ts.end()));
-
// compose the states of all processor motions into one only state
for (const auto& prc : processor_is_motion_list_)
{
- const auto& prc_state = prc->getState(min_ts);
+ const auto& prc_state = prc->getState();
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
@@ -472,19 +431,20 @@ VectorComposite Problem::getState() const
return state;
}
-VectorComposite Problem::getState (const TimeStamp& _ts) const
+VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
{
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
{
const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
if (last_kf_or_aux)
{
- return last_kf_or_aux->getState();
+ return last_kf_or_aux->getState(structure);
}
else
{
- return VectorComposite();
+ return stateZero(structure);
}
}
@@ -505,93 +465,6 @@ VectorComposite Problem::getState (const TimeStamp& _ts) const
return state;
}
-
-
-//// Problem of this implmentation: if more state
-//void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
-//{
-// // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
-// FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
-// if (processor_is_motion_list_.empty()){
-// if (closest_frame != nullptr)
-// _state = closest_frame->getState();
-// else
-// _state = zeroState();
-// }
-//
-// // RETRIEVE FROM PROCESSOR MOTION
-// // TODO: current implementation messy, would be much easier with a state being an std::unordered_map
-// else {
-// // Iterate over the problem state structure and get the corresponding state
-// // in the first processor is motion that provides it
-// // finally check if the state to concatenate list has the same total size as the problem state_size
-//
-// // an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors
-// std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered
-//
-// for (auto proc: processor_is_motion_list_)
-// {
-// Eigen::VectorXd proc_state = proc->getState(_ts);
-//
-// int idx = 0;
-// for (char sb_name: proc->getStateStructure()){
-// // not already there
-// if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
-// if (sb_name == 'O'){
-// int size_sb = dim_ == 3 ? 4 : 1; // really bad: should be more transparent
-// states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
-// idx += size_sb;
-// }
-// else{
-// int size_sb = dim_ == 3 ? 3 : 2;
-// states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
-// idx += size_sb;
-// }
-// }
-// }
-// }
-//
-// int concat_size = 0;
-// for (auto state_map_it: states_to_concat_map){
-// concat_size += state_map_it.second.size();
-// }
-// // assert(concat_size == state_size_ && "Problem with the concatenated size: " );
-//
-// // fill the state value from the state concatenation in the order dictated by frame_structure_
-// int idx = 0;
-// _state.resize(state_size_);
-// for (char sb_name: frame_structure_){
-// Eigen::VectorXd sb_state;
-// int size_sb; // really bad...
-// if (sb_name == 'O'){
-// size_sb = dim_ == 3 ? 4 : 1;
-// }
-// else {
-// size_sb = dim_ == 3 ? 3 : 2;
-// }
-// if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
-// sb_state = states_to_concat_map[sb_name];
-// }
-// else {
-// // Should be taken from the last state but too messy already
-// sb_state.resize(size_sb);
-// sb_state.setZero();
-// }
-//
-// _state.segment(idx, size_sb) = sb_state;
-// idx += size_sb;
-//
-// }
-// }
-//}
-
-//Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
-//{
-// Eigen::VectorXd state(getFrameStructureSize());
-// getState(_ts, state);
-// return state;
-//}
-
SizeEigen Problem::getFrameStructureSize() const
{
return state_size_;
@@ -647,7 +520,30 @@ void Problem::clearProcessorIsMotion(IsMotionPtr proc)
appendToStructure(pm->getStateStructure());
}
-Eigen::VectorXd Problem::zeroState() const
+VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
+{
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
+ VectorComposite state;
+ for (const auto& ckey : structure)
+ {
+ const auto& key = string(1,ckey);
+ VectorXd vec;
+ if (key == "O")
+ {
+ if (dim_ == 2) vec = VectorXd::Zero(1);
+ if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
+ }
+ else
+ {
+ vec = VectorXd::Zero(dim_);
+ }
+ state.emplace(key, vec);
+ }
+ return state;
+}
+
+Eigen::VectorXd Problem::zeroState ( ) const
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 5087f744b269a675008574c41f4b7f2fce66d074..faf01a3a5231df0ec63d142796141d4fa372e430 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -104,13 +104,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case FIRST_TIME_WITHOUT_KF :
{
// There is no KF: create own origin
- setOrigin(stateZero(), _incoming_ptr->getTimeStamp());
+ setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
{
// cannot joint to the KF: create own origin
- setOrigin(stateZero(), _incoming_ptr->getTimeStamp());
+ setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_ON_INCOMING :
@@ -122,7 +122,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case FIRST_TIME_WITH_KF_AFTER_INCOMING :
{
// cannot joint to the KF: create own origin
- setOrigin(stateZero(), _incoming_ptr->getTimeStamp());
+ setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
case RUNNING_WITHOUT_KF :
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 2a8eaaf940144c90829e87b9d8318598a9f78679..284090e238f3c75327424cb76f3946e277e23bdf 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -97,12 +97,6 @@ void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
_x_plus_delta["O"] = Vector1d(pi2pi(_x.at("O")(0) + _delta(2)));
}
-VectorComposite ProcessorOdom2d::stateZero() const
-{
- return VectorComposite("PO", {2,1});
-}
-
-
bool ProcessorOdom2d::voteForKeyFrame() const
{
// Distance criterion
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 9d67f6777ff819ef47f43fe306bcbe2fe3e1b294..5b01e98b3ad8fbbe6f31bedc4f25b1a9cfe76747 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -229,13 +229,6 @@ Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_prei
return delta_corrected;
}
-VectorComposite ProcessorOdom3d::stateZero() const
-{
- VectorComposite zero;
- SE3::identity(zero);
- return zero;
-}
-
FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
auto fac_odom = FactorBase::emplace<FactorOdom3d>(_feature_motion,
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 2451e1d6de9cb438df4c0ebb2e2c4907c2c000aa..74b61fd77105cf63459df24c0aae11832a90b284 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -82,7 +82,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
FrameBasePtr kfrm = getProblem()->emplaceFrame(getStateStructure(),
KEY,
- stateZero(),
+ getProblem()->getState(getStateStructure()),
incoming_ptr_->getTimeStamp());
incoming_ptr_->link(kfrm);
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 414f75577a95af8d36ca81488ebf04517327cdf8..8485d267034febde4f08911108c0773fd67ab892 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -11,12 +11,6 @@
namespace wolf
{
-VectorComposite ProcessorTrackerFeatureDummy::stateZero() const
-{
- return VectorComposite("PO", {2,1});
-}
-
-
unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index beec959fd5033d13966e4212aaca28fe95555aad..d06ac67503acfda0a55d9418bac339359b3d9625 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -41,8 +41,6 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature);
virtual ~ProcessorTrackerFeatureDummy();
- virtual VectorComposite stateZero() const override;
-
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ParamsProcessorTrackerFeatureDummy);
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index ccac76f48d978ebdf3e2fa84df94447cc325e729..d36efd3f804cd71ff38fc7b835dc1b5230c59885 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -25,12 +25,6 @@ ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
//
}
-VectorComposite ProcessorTrackerLandmarkDummy::stateZero() const
-{
- return VectorComposite("PO", {2,1});
-}
-
-
unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index ae6bb2ec291b92e2c7369e31624608d4b9c3fe1f..5140a2b2762db740944019ac1595e7d37bf07a2c 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -35,8 +35,6 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
virtual ~ProcessorTrackerLandmarkDummy();
- virtual VectorComposite stateZero() const override;
-
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ParamsProcessorTrackerLandmarkDummy);
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index aa3e8fafafcef9a742e662a812c070ac450742e9..2a0a6b48c8f5a3d697fc6dcfdee689c89aec86d7 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -27,10 +27,6 @@ public:
ProcessorLoopClosure("LOOP CLOSURE DUMMY", 0, _params_loop_closure),
factor_created(&factor_created){};
std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
- virtual VectorComposite stateZero() const override
- {
- return VectorComposite("PO", {2,1});
- }
protected:
virtual bool voteComputeFeatures() override { return true;};