Merge branch '292-hasstateblocks-addstateblock-needs-to-register-sb-s' into...

Merge branch '292-hasstateblocks-addstateblock-needs-to-register-sb-s' into adapting-to-multiple-processor-motion-285

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;

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;
 }

include/core/state_block/has_state_blocks.h

@@ -30,22 +30,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; }
@@ -57,11 +55,11 @@ 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;
@@ -78,7 +76,7 @@ class HasStateBlocks
 
     private:
         std::string structure_;
-        std::map<std::string, StateBlockPtr> state_block_map_;
+        std::unordered_map<std::string, StateBlockPtr> state_block_map_;
 
 };
 
@@ -103,7 +101,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_;
 }
@@ -118,15 +116,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));
@@ -138,24 +127,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;
 }
 
@@ -186,16 +175,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_)

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");

src/frame/frame_base.cpp

@@ -172,8 +172,8 @@ void FrameBase::setKey()
     if (!isKeyOrAux())
         registerNewStateBlocks(getProblem());
 
-    // WOLF_DEBUG("Set Key", this->id());
     type_ = KEY;
+
     if (getTrajectory())
     {
         getTrajectory()->sortFrame(shared_from_this());
@@ -186,8 +186,8 @@ void FrameBase::setAux()
     if (!isKeyOrAux())
         registerNewStateBlocks(getProblem());
 
-    // WOLF_DEBUG("Set Auxiliary", this->id());
     type_ = AUXILIARY;
+
     if (getTrajectory())
     {
         getTrajectory()->sortFrame(shared_from_this());
@@ -337,7 +337,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
 
     NodeBase::setProblem(_problem);
     if (this->isKey())
-        registerNewStateBlocks(getProblem());
+        registerNewStateBlocks(_problem);
 
     for (auto cap : capture_list_)
         cap->setProblem(_problem);

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);
     }
 
 }

src/problem/problem.cpp

@@ -549,6 +549,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())
     {

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

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)

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})

test/gtest_has_state_blocks.cpp

+/**
+ * \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();
+}
+