diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 14bcc2139d66765f8a3c78f2358d2c4606b91221..3745de04806c78f65463c39da49bcf9ba42cf0f8 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -96,8 +96,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// State blocks ----------------------------------------------------
public:
+ using HasStateBlocks::addStateBlock;
+ StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
StateBlockPtr getV() const;
- void setV(const StateBlockPtr _v_ptr);
+
protected:
virtual void setProblem(ProblemPtr _problem) final;
@@ -192,11 +194,6 @@ inline StateBlockPtr FrameBase::getV() const
return getStateBlock("V");
}
-inline void FrameBase::setV(const StateBlockPtr _v_ptr)
-{
- addStateBlock("V", _v_ptr);
-}
-
inline TrajectoryBasePtr FrameBase::getTrajectory() const
{
return trajectory_ptr_.lock();
@@ -217,6 +214,17 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
return constrained_by_list_;
}
+inline StateBlockPtr FrameBase::addStateBlock(const std::string& _sb_type,
+ const StateBlockPtr& _sb)
+{
+ if (isKeyOrAux())
+ HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
+ else
+ HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr);
+
+ return _sb;
+}
+
inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
trajectory_ptr_ = _trj_ptr;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index a5afc4664a826c19b3c58272164b8e3849d1c15c..676a83bc5c9ade574e1b10b2a21a6bf2740cd3cd 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -173,10 +173,9 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
bool process(const CaptureBasePtr capture_ptr);
// State blocks
- virtual StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr) override;
- virtual StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr) override { return this->addStateBlock(std::string(1, _key), _sb_ptr); }
- StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic);
- StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
+ using HasStateBlocks::addStateBlock;
+ StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
+ StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
StateBlockPtr getStateBlockDynamic(const std::string& _key) const;
StateBlockPtr getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const;
StateBlockPtr getStateBlockDynamic(const char _key) const { return getStateBlockDynamic(std::string(1,_key)); }
@@ -279,18 +278,10 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
return processor_list_;
}
-inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr)
-{
- assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
- HasStateBlocks::addStateBlock(_key, _sb_ptr);
- setStateBlockDynamic(_key, false);
- return _sb_ptr;
-}
-
inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
- HasStateBlocks::addStateBlock(_key, _sb_ptr);
+ HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
setStateBlockDynamic(_key, _dynamic);
return _sb_ptr;
}
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ade367fb88e7e0e987b087d31130dee0e78736ff..0f7d13f99c2af92d8779c11a7e705bb4ef94f99f 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -26,22 +26,20 @@ class HasStateBlocks
void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
- const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
+ const std::unordered_map<std::string, StateBlockPtr>& getStateBlockMap() const;
std::vector<StateBlockPtr> getStateBlockVec() const;
// Some typical shortcuts -- not all should be coded here, see notes below.
StateBlockPtr getP() const;
StateBlockPtr getO() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
virtual void fix();
virtual void unfix();
virtual bool isFixed() const;
- virtual StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
- virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return addStateBlock(std::string(1,_sb_type), _sb); }
+ virtual StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
+ virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); }
virtual unsigned int removeStateBlock(const std::string& _sb_type);
virtual unsigned int removeStateBlock(const char _sb_type);
bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
@@ -53,26 +51,26 @@ class HasStateBlocks
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
- std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+ std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
// Emplace base state blocks.
template<typename ... Args>
- inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+ inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
void registerNewStateBlocks(ProblemPtr _problem) const;
void removeStateBlocks(ProblemPtr _problem);
// States
- virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
+ virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
Eigen::VectorXd getState() const;
- void getState(Eigen::VectorXd& _state) const;
- unsigned int getSize() const;
- unsigned int getLocalSize() const;
+ void getState(Eigen::VectorXd& _state) const;
+ unsigned int getSize() const;
+ unsigned int getLocalSize() const;
private:
std::string structure_;
- std::map<std::string, StateBlockPtr> state_block_map_;
+ std::unordered_map<std::string, StateBlockPtr> state_block_map_;
};
@@ -97,7 +95,7 @@ inline HasStateBlocks::~HasStateBlocks()
//
}
-inline const std::map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+inline const std::unordered_map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
{
return state_block_map_;
}
@@ -112,15 +110,6 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
return sbv;
}
-inline wolf::StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb)
-{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- state_block_map_.emplace(_sb_type, _sb);
- if (!isInStructure(_sb_type))
- appendToStructure(_sb_type);
- return _sb;
-}
-
inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
{
return removeStateBlock(std::string(1, _sb_type));
@@ -132,24 +121,24 @@ inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type
}
template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor)
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
- state_block_map_.emplace(_sb_type, sb);
- if (!isInStructure(_sb_type))
- appendToStructure(_sb_type);
+
+ addStateBlock(_sb_type, sb, _problem);
+
return sb;
}
template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor)
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- std::shared_ptr<StateBlock> sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
- state_block_map_.emplace(_sb_type, sb);
- if (!isInStructure(_sb_type))
- appendToStructure(_sb_type);
+ auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+
+ addStateBlock(_sb_type, sb, _problem);
+
return sb;
}
@@ -180,16 +169,6 @@ inline wolf::StateBlockPtr HasStateBlocks::getO() const
return getStateBlock("O");
}
-inline void HasStateBlocks::setP(const StateBlockPtr _p_ptr)
-{
- addStateBlock("P", _p_ptr);
-}
-
-inline void HasStateBlocks::setO(const StateBlockPtr _o_ptr)
-{
- addStateBlock("O", _o_ptr);
-}
-
inline void HasStateBlocks::fix()
{
for (auto pair_key_sbp : state_block_map_)
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 6e78e69a888482381e870a0a00fef5d1aeaf2514..fbb54972b09b78f0a0e6a6e66701396a481d9cfc 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -26,7 +26,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P"))
{
assert(_p_ptr && "Pointer to dynamic position params is null!");
- addStateBlock("P", _p_ptr);
+ addStateBlock("P", _p_ptr, nullptr);
}
else
assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
@@ -34,7 +34,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O"))
{
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
- addStateBlock("O", _o_ptr);
+ addStateBlock("O", _o_ptr, nullptr);
}
else
assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
@@ -42,7 +42,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I"))
{
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
- addStateBlock("I", _intr_ptr);
+ addStateBlock("I", _intr_ptr, nullptr);
}
else
assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index ca5918624d01bb1d0d23481d4685f9d4006dd028..db17da69920a4ad27915bb3ddf27e84493504126 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -156,8 +156,8 @@ void FrameBase::setKey()
if (!isKeyOrAux())
registerNewStateBlocks(getProblem());
- // WOLF_DEBUG("Set Key", this->id());
type_ = KEY;
+
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
@@ -170,8 +170,8 @@ void FrameBase::setAux()
if (!isKeyOrAux())
registerNewStateBlocks(getProblem());
- // WOLF_DEBUG("Set Auxiliary", this->id());
type_ = AUXILIARY;
+
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
@@ -321,7 +321,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
if (this->isKey())
- registerNewStateBlocks(getProblem());
+ registerNewStateBlocks(_problem);
for (auto cap : capture_list_)
cap->setProblem(_problem);
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 1e24627be754f7bebdf9cf2945fa0fb6bc502bee..7c9b8b664573bd296902ad420c1e5a161605e635 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -21,11 +21,11 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
{
if (_p_ptr)
{
- addStateBlock("P", _p_ptr);
+ addStateBlock("P", _p_ptr, nullptr);
}
if (_o_ptr)
{
- addStateBlock("O", _o_ptr);
+ addStateBlock("O", _o_ptr, nullptr);
}
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c2fd1334f5c3d95f14f698be81c1ec72ac87c4ce..482fdf08bf67cc4f26929b840871ae044ceefa42 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -515,6 +515,7 @@ StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
+
// exsiting notification for this state block
if (notification_it != state_block_notification_map_.end())
{
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 0d47cd191f51c51126d38a591fea7de3614f53ae..6532dfc6a2b95d43451e00f8fad4797e7c53edf9 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -50,7 +50,7 @@ void SolverManager::update()
}
else
{
- WOLF_DEBUG("Tried to add an already added !");
+ WOLF_DEBUG("Tried to add a StateBlock that was already added !");
}
}
else
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index b90e9188c1e1cefbec01f02ca33c99da6511bf89..8b11f1cb3981206700b4ce54ad5529df54ae6d29 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -4,6 +4,20 @@
namespace wolf
{
+StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ state_block_map_.emplace(_sb_type, _sb);
+ if (!isInStructure(_sb_type))
+ appendToStructure(_sb_type);
+
+ // conditionally register to problem
+ if(_problem)
+ _problem->notifyStateBlock(_sb, ADD);
+
+ return _sb;
+}
+
void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) const
{
if (_problem != nullptr)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 79fc2c60a55ef6b0c9df2700e47adafcf27c6d23..bf13016352ac0399328d1fb93ac0892dca210a10 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -79,6 +79,10 @@ target_link_libraries(gtest_feature_base ${PROJECT_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PROJECT_NAME})
+# HasStateBlocks classes test
+wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
+target_link_libraries(gtest_has_state_blocks ${PROJECT_NAME})
+
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
target_link_libraries(gtest_local_param ${PROJECT_NAME})
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d8b46e9b721fa4f9947138eee2655ed3ad25bff0
--- /dev/null
+++ b/test/gtest_has_state_blocks.cpp
@@ -0,0 +1,171 @@
+/**
+ * \file gtest_has_state_blocks.cpp
+ *
+ * Created on: Mar 24, 2020
+ * \author: jsola
+ */
+
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/frame/frame_base.h"
+#include "core/sensor/sensor_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace wolf;
+
+class HasStateBlocksTest : public testing::Test
+{
+
+ public:
+
+ ProblemPtr problem;
+ SensorBasePtr S0, S1;
+ FrameBasePtr F0, F1;
+ CaptureBasePtr C0, C1;
+ LandmarkBasePtr L0, L1;
+ StateBlockPtr sbp0, sbv0, sbp1, sbv1;
+ StateQuaternionPtr sbo0, sbo1;
+
+
+ virtual void SetUp()
+ {
+ problem = Problem::create("POV", 3);
+
+ sbp0 = std::make_shared<StateBlock>(3); // size 3
+ sbo0 = std::make_shared<StateQuaternion>();
+ sbv0 = std::make_shared<StateBlock>(3); // size 3
+ sbp1 = std::make_shared<StateBlock>(3); // size 3
+ sbo1 = std::make_shared<StateQuaternion>();
+ sbv1 = std::make_shared<StateBlock>(3); // size 3
+
+ F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
+
+ }
+ virtual void TearDown(){}
+
+};
+
+
+TEST_F(HasStateBlocksTest, Notifications_setKey_add)
+{
+ Notification n;
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->link(problem->getTrajectory());
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->setKey();
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
+ ASSERT_EQ(n, ADD);
+}
+
+TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
+{
+ Notification n;
+
+ // First add SB, than make KF
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->link(problem->getTrajectory());
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ F0->addStateBlock("V", sbv0);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ F0->setKey();
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
+ ASSERT_EQ(n, ADD);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+}
+
+TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
+{
+ Notification n;
+
+ // first make KF, then add SB
+
+ F1->link(problem->getTrajectory());
+ F1->setKey();
+
+ F1->addStateBlock("P", sbp1);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
+ ASSERT_EQ(n, ADD);
+
+ // add another SB
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
+
+ F1->addStateBlock("V", sbv1);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
+ ASSERT_EQ(n, ADD);
+
+}
+
+TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
+{
+
+ CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+
+
+ Notification n;
+
+ // Add SB, make KF
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->link(problem->getTrajectory());
+ F0->addStateBlock("V", sbv0);
+ F0->setKey();
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+ // solve. This will clear all notifications
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ // Notify again the same SB
+
+ problem->notifyStateBlock(sbv0, ADD);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+ // solve again
+
+ report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ // Add again the same SB. This should crash
+
+ ASSERT_DEATH( F0->addStateBlock("V", sbv0) , "" );
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+//::testing::GTEST_FLAG(filter) = "TestGroup.DummyTestExample";
+ return RUN_ALL_TESTS();
+}
+