diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 2f725e18d723463e810441c83f7fc81acc206785..09d7bc68dee5511c28ee456bb42467e89168bbb4 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -26,8 +26,8 @@ class IsMotion
// Queries to the processor:
virtual TimeStamp getTimeStamp() const = 0;
- virtual VectorComposite getState() const = 0;
- virtual VectorComposite getState(const TimeStamp& _ts) const = 0;
+ virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
std::string getStateStructure(){return state_structure_;};
void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 80017c423035ff9bd981bea1f85eb7adf06e9a67..6d2b81e9ed4425a693c7f669779d1f51a0118845 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -166,8 +166,8 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
// Queries to the processor:
TimeStamp getTimeStamp() const override;
- VectorComposite getState() const override;
- VectorComposite getState(const TimeStamp& _ts) const override;
+ VectorComposite getState(const StateStructure& _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 8880fc0a2cfb09a46712a502601a801a1f923a39..b1af55f7b8c6adafc23d9ef4139b154537f0f9ff 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -74,12 +74,12 @@ class HasStateBlocks
VectorComposite getState(const StateStructure& structure="") const;
void setState(const VectorComposite& _state, const bool _notify = true);
void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
- void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure="", const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateStructure& _structure="", const bool _notify = true);
void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
VectorXd getStateVector(const StateStructure& structure="") const;
- unsigned int getSize(const StateStructure& _sub_structure="") const;
- unsigned int getLocalSize(const StateStructure& _sub_structure="") const;
+ unsigned int getSize(const StateStructure& _structure="") const;
+ unsigned int getLocalSize(const StateStructure& _structure="") const;
// Perturb state
void perturb(double amplitude = 0.01);
@@ -225,13 +225,13 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _
}
}
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify)
{
StateStructure structure;
- if (_sub_structure == "")
+ if (_structure == "")
structure = structure_;
else
- structure = _sub_structure;
+ structure = _structure;
int size = getSize(structure);
assert(_state.size() == size && "In FrameBase::setState wrong state size");
@@ -286,14 +286,14 @@ inline void HasStateBlocks::setState(const StateStructure& _structure, const std
}
-//// _sub_structure can be either stateblock structure of the node or a subset of this structure
-inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_structure) const
+//// _structure can be either stateblock structure of the node or a subset of this structure
+inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) const
{
StateStructure structure;
- if (_sub_structure == "")
+ if (_structure == "")
structure = structure_;
else
- structure = _sub_structure;
+ structure = _structure;
VectorXd state(getSize(structure));
@@ -312,21 +312,31 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_struct
inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
{
+ const StateStructure& structure = (_structure == "" ? structure_ : _structure);
+
VectorComposite state;
- for (auto& pair_key_sb : state_block_map_)
+
+ for (const auto ckey : structure)
{
- state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+ const auto& key = string(1,ckey); // ckey is char
+
+ auto state_it = state_block_map_.find(key);
+
+ if (state_it != state_block_map_.end())
+
+ state.emplace(key, state_it->second->getState());
}
+
+// for (auto& pair_key_sb : state_block_map_)
+// {
+// state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+// }
return state;
}
-inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
+inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) const
{
- StateStructure structure;
- if (_sub_structure == "")
- structure = structure_;
- else
- structure = _sub_structure;
+ const StateStructure& structure = (_structure == "" ? structure_ : _structure);
unsigned int size = 0;
for (const char key : structure)
@@ -341,7 +351,6 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure
return size;
}
-//<<<<<<< HEAD
inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
{
const auto& it = std::find_if(state_block_map_.begin(),
@@ -380,13 +389,13 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
}
}
-inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
+inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _structure) const
{
StateStructure structure;
- if (_sub_structure == "")
+ if (_structure == "")
structure = structure_;
else
- structure = _sub_structure;
+ structure = _structure;
unsigned int size = 0;
for (const char key : structure)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index caf9e1115582963c64980a02bf10f15a8b0c108e..813077742ceca8ee0b7b60913fab479145c4298b 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -452,7 +452,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
// 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();
+ const auto& prc_state = prc->getState(structure);
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
@@ -498,7 +498,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
for (const auto& prc : processor_is_motion_list_)
{
- const auto& prc_state = prc->getState(_ts);
+ const auto& prc_state = prc->getState(_ts, structure);
// transfer processor vector blocks to problem state
for (const auto& pair_key_vec : prc_state)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c403a87828f9266089f63c8e8cb274cb03616729..65960d878ac414308b1032f913e4a9f9e69cf243 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -427,8 +427,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
-VectorComposite ProcessorMotion::getState() const
+VectorComposite ProcessorMotion::getState(const StateStructure& _structure) const
{
+ const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
+
+
if (origin_ptr_ == nullptr or
origin_ptr_->isRemoving() or
last_ptr_ == nullptr or
@@ -446,7 +449,7 @@ VectorComposite ProcessorMotion::getState() const
// this may happen when in the very first frame where the capture has no motion info --> empty buffer
if (last_ptr_->getBuffer().empty())
{
- return last_ptr_->getFrame()->getState(state_structure_);
+ return last_ptr_->getFrame()->getState(structure);
}
/* Doing this:
@@ -463,7 +466,7 @@ VectorComposite ProcessorMotion::getState() const
*/
// Get state of origin
- const auto& x_origin = origin_ptr_->getFrame()->getState();
+ const auto& x_origin = getOrigin()->getFrame()->getState(state_structure_);
// Get most rescent motion
const auto& motion = last_ptr_->getBuffer().back();
@@ -497,16 +500,35 @@ VectorComposite ProcessorMotion::getState() const
statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
}
- return state;
+ if (_structure == "")
+ return state;
+
+ else
+ {
+ // remove states not requested by structure
+ auto pair_key_vec_it = state.begin();
+ while (pair_key_vec_it != state.end())
+ {
+ if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+ pair_key_vec_it = state.erase(pair_key_vec_it);
+
+ else
+ pair_key_vec_it ++;
+
+ }
+ return state;
+ }
}
// _x needs to have the size of the processor state
-VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
+VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStructure& _structure) const
{
assert(_ts.ok());
+ const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
+
// We need to search for the capture containing a motion buffer with the queried time stamp
CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
@@ -523,7 +545,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
// this may happen when in the very first frame where the capture has no motion info --> empty buffer
if (capture_motion->getBuffer().empty())
{
- return capture_motion->getFrame()->getState(state_structure_);
+ return capture_motion->getFrame()->getState(structure);
}
/* Doing this:
@@ -541,7 +563,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
// Get state of origin
CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- const auto& x_origin = cap_orig->getFrame()->getState();
+ const auto& x_origin = cap_orig->getFrame()->getState(state_structure_);
// Get motion at time stamp
const auto& motion = capture_motion->getBuffer().getMotion(_ts);
@@ -576,8 +598,24 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), state);
}
- // return success
- return state;
+ if (_structure == "")
+ return state;
+
+ else
+ {
+ // remove states not requested by structure
+ auto pair_key_vec_it = state.begin();
+ while (pair_key_vec_it != state.end())
+ {
+ if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+ pair_key_vec_it = state.erase(pair_key_vec_it);
+
+ else
+ pair_key_vec_it ++;
+
+ }
+ return state;
+ }
}
}
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index 4e3d610109e92b8fe6846b93050bc64af29a6f72..1369fd5a87dc31f4d0a7af14a929f0d23637b041 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -178,6 +178,46 @@ TEST_F(HasStateBlocksTest, stateBlockKey)
ASSERT_TRUE(key == "");
}
+TEST_F(HasStateBlocksTest, getState_structure)
+{
+ F0->addStateBlock("V", sbv0); // now KF0 is POV
+
+ WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure());
+
+ auto state0 = F0->getState();
+ WOLF_DEBUG("getState() = ", state0);
+ ASSERT_EQ(state0.size(), 3);
+ ASSERT_TRUE(state0.count("P"));
+ ASSERT_TRUE(state0.count("O"));
+ ASSERT_TRUE(state0.count("V"));
+
+ state0 = F0->getState("PO");
+ WOLF_DEBUG("getState(\"PO\") = ", state0);
+ ASSERT_EQ(state0.size(), 2);
+ ASSERT_TRUE(state0.count("P"));
+ ASSERT_TRUE(state0.count("O"));
+ ASSERT_FALSE(state0.count("V"));
+
+ state0 = F0->getState("PV");
+ WOLF_DEBUG("getState(\"PV\") = ", state0);
+ ASSERT_EQ(state0.size(), 2);
+ ASSERT_TRUE(state0.count("P"));
+ ASSERT_FALSE(state0.count("O"));
+ ASSERT_TRUE(state0.count("V"));
+
+ state0 = F0->getState("OW"); // W does not exist
+ WOLF_DEBUG("getState(\"OW\") = ", state0);
+ ASSERT_EQ(state0.size(), 1);
+ ASSERT_FALSE(state0.count("P"));
+ ASSERT_TRUE(state0.count("O"));
+ ASSERT_FALSE(state0.count("V"));
+ ASSERT_FALSE(state0.count("W"));
+
+
+
+
+}
+
int main(int argc, char **argv)
{
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 0e13554d43eb92416f04321d6039f930b8d5a6e5..1470052b849465705eb860b34600d8d624dfb61c 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -16,6 +16,7 @@
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
#include "dummy/solver_manager_dummy.h"
+#include "core/yaml/parser_yaml.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
@@ -489,10 +490,77 @@ TEST(Problem, check)
ASSERT_TRUE(problem->check(true, std::cout));
}
+TEST(Problem, getState)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_odom_3d.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ auto S = P->getHardware()->getSensorList().front();
+
+ auto C = std::make_shared<CaptureOdom3d>(0.0, S, 0.1*Vector6d::Ones(), 0.01*Matrix6d::Identity());
+
+ for (TimeStamp t = 0.0; t <= 3.9; t += 0.1)
+ {
+ C->setTimeStamp(t);
+ C->process();
+ }
+ P->print(4,1,1,1);
+
+ // get at t = origin
+ WOLF_DEBUG("P (0) = ", P->getState(0, "P")); // partial structure
+ WOLF_DEBUG("PO(0) = ", P->getState(0, "PO")); // all but explicit structure
+ WOLF_DEBUG("x (0) = ", P->getState(TimeStamp(0))); // own structure
+ ASSERT_EQ(P->getState(0, "P").size(), 1);
+ ASSERT_EQ(P->getState(0, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(0)).size(), 2);
+
+ // get at t = before KF
+ WOLF_DEBUG("P (1) = ", P->getState(1, "P"));
+ WOLF_DEBUG("PO(1) = ", P->getState(1, "PO"));
+ WOLF_DEBUG("x (1) = ", P->getState(1));
+ ASSERT_EQ(P->getState(1, "P").size(), 1);
+ ASSERT_EQ(P->getState(1, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(1)).size(), 2);
+
+ // get at t = KF
+ WOLF_DEBUG("P (2) = ", P->getState(2, "P"));
+ WOLF_DEBUG("PO(2) = ", P->getState(2, "PO"));
+ WOLF_DEBUG("x (2) = ", P->getState(2));
+ ASSERT_EQ(P->getState(2, "P").size(), 1);
+ ASSERT_EQ(P->getState(2, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(2)).size(), 2);
+
+ // get at t = after last KF
+ WOLF_DEBUG("P (3) = ", P->getState(3, "P"));
+ WOLF_DEBUG("PO(3) = ", P->getState(3, "PO"));
+ WOLF_DEBUG("x (3) = ", P->getState(3));
+ ASSERT_EQ(P->getState(3, "P").size(), 1);
+ ASSERT_EQ(P->getState(3, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(3)).size(), 2);
+
+ // get at t = last processed capture
+ WOLF_DEBUG("P (3.9) = ", P->getState(3.9, "P"));
+ WOLF_DEBUG("PO(3.9) = ", P->getState(3.9, "PO"));
+ WOLF_DEBUG("x (3.9) = ", P->getState(3.9));
+ ASSERT_EQ(P->getState(3.9, "P").size(), 1);
+ ASSERT_EQ(P->getState(3.9, "PO").size(), 2);
+ ASSERT_EQ(P->getState(TimeStamp(3.9)).size(), 2);
+
+ // get at t = current state
+ WOLF_DEBUG("P () = ", P->getState("P"));
+ WOLF_DEBUG("PO() = ", P->getState("PO"));
+ WOLF_DEBUG("x () = ", P->getState());
+
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
+ ::testing::GTEST_FLAG(filter) = "Problem.getState";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index bcf69745cea695fef25bbb2238b0c4fddf334799..410ebdf1faea8175c8e73435670fdb46de85fd9e 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -101,6 +101,66 @@ TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
+TEST_F(ProcessorMotion_test, getState_structure)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_TRUE (processor->getState("P").count("P"));
+ ASSERT_FALSE(processor->getState("P").count("O"));
+ ASSERT_FALSE(processor->getState("O").count("P"));
+ ASSERT_TRUE (processor->getState("O").count("O"));
+
+ WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState("V"));
+ ASSERT_EQ (processor->getState("V").size(), 0);
+}
+
+
+TEST_F(ProcessorMotion_test, getState_time_structure)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_TRUE (processor->getState(7, "P").count("P"));
+ ASSERT_FALSE(processor->getState(7, "P").count("O"));
+ ASSERT_FALSE(processor->getState(7, "O").count("P"));
+ ASSERT_TRUE (processor->getState(7, "O").count("O"));
+
+ WOLF_DEBUG("processor->getState(7, \"V\") = ", processor->getState(7, "V"));
+ ASSERT_EQ (processor->getState(7, "V").size(), 0);
+}
+
+
TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0337cdc0bbc589d0d03054107643e632bb6be7b4
--- /dev/null
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -0,0 +1,44 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111