better api and implementation for SensorBase dynamic state getters

include/core/capture/capture_base.h

@@ -139,8 +139,8 @@ class CaptureBase : public NodeStateBlocks
 
   public:
     // State blocks
-    StateBlockConstPtr getStateBlock(const char& _key) const;
-    StateBlockPtr      getStateBlock(const char& _key);
+    StateBlockConstPtr getStateBlock(const char& _key) const override;
+    StateBlockPtr      getStateBlock(const char& _key) override;
     StateBlockConstPtr getSensorP() const;
     StateBlockPtr      getSensorP();
     StateBlockConstPtr getSensorO() const;

include/core/common/node_state_blocks.h

@@ -106,8 +106,8 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
     bool               hasStateBlock(const StateBlockConstPtr& _sb) const;
     bool               setStateBlock(const char _sb_key, const StateBlockPtr& _sb);
     bool               stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
-    StateBlockConstPtr getStateBlock(const char& _sb_key) const;
-    StateBlockPtr      getStateBlock(const char& _sb_key);
+    virtual StateBlockConstPtr getStateBlock(const char& _sb_key) const;
+    virtual StateBlockPtr      getStateBlock(const char& _sb_key);
 
     // Register/remove state blocks to/from wolf::Problem
     void         setProblem(ProblemPtr _problem) override;

include/core/processor/processor_diff_drive.h

@@ -66,7 +66,7 @@ inline Eigen::VectorXd ProcessorDiffDrive::getCalibration(const CaptureBaseConst
     if (_capture)
         return _capture->getStateBlock('I')->getState();
     else
-        return getSensor()->getStateBlockDynamic('I')->getState();
+        return getSensor()->getStateBlock('I')->getState();
 }
 
 inline void ProcessorDiffDrive::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)

include/core/sensor/sensor_base.h

@@ -170,21 +170,16 @@ class SensorBase : public NodeStateBlocks
     bool process(const CaptureBasePtr capture_ptr);
 
     // State blocks
-    StateBlockConstPtr getStateBlockDynamic(const char& _key) const;
-    StateBlockPtr      getStateBlockDynamic(const char& _key);
-    StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
-    StateBlockPtr      getStateBlockDynamic(const char& _key, const TimeStamp& _ts);
+    StateBlockConstPtr getStateBlock(const char& _key) const override;
+    StateBlockPtr      getStateBlock(const char& _key) override;
+    StateBlockConstPtr getStateBlock(const char& _key, const TimeStamp& _ts) const;
+    StateBlockPtr      getStateBlock(const char& _key, const TimeStamp& _ts);
 
-    // Declare a state block as dynamic or static (with _dymanic = false)
-    void setStateBlockDynamic(const char& _key, bool _dynamic = true);
-
-    bool isStateBlockDynamic(const char& _key) const;
-    bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBaseConstPtr& _cap) const;
-    bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap);
-    bool isStateBlockInCapture(const char& _key, CaptureBaseConstPtr& _cap) const;
-    bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap);
-    bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const;
-    bool isStateBlockInCapture(const char& _key) const;
+    // get capture where the state block is
+    CaptureBaseConstPtr getCaptureContainingStateBlock(const char& _key, const TimeStamp& _ts) const;
+    CaptureBasePtr      getCaptureContainingStateBlock(const char& _key, const TimeStamp& _ts);
+    CaptureBaseConstPtr getCaptureContainingStateBlock(const char& _key) const;
+    CaptureBasePtr      getCaptureContainingStateBlock(const char& _key);
 
     StateBlockConstPtr getP(const TimeStamp _ts) const;
     StateBlockPtr      getP(const TimeStamp _ts);
@@ -199,11 +194,14 @@ class SensorBase : public NodeStateBlocks
     StateBlockConstPtr getIntrinsic() const;
     StateBlockPtr      getIntrinsic();
 
-  public:
-    // enable/disable the sensor
-    bool isEnabled() const;
-    void enable();
-    void disable();
+    // Declare/get dynamic
+    void setStateBlockDynamic(const char& _key, bool _dynamic = true);
+    bool isStateBlockDynamic(const char& _key) const;
+
+    // drift rates
+    bool            hasDrift(char _key) const;
+    void            setDriftStdRate(char _key, const Eigen::VectorXd& _drift_rate_std);
+    Eigen::VectorXd getDriftStdRate(char) const;
 
     // Set the state block as fixed or unfixed
     void fixExtrinsics();
@@ -211,10 +209,10 @@ class SensorBase : public NodeStateBlocks
     void fixIntrinsics();
     void unfixIntrinsics();
 
-    // drift rates
-    bool            hasDrift(char _key) const;
-    void            setDriftStdRate(char _key, const Eigen::VectorXd& _drift_rate_std);
-    Eigen::VectorXd getDriftStdRate(char) const;
+    // enable/disable the sensor
+    bool isEnabled() const;
+    void enable();
+    void disable();
 
     // noise
     virtual Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const = 0;

src/sensor/sensor_base.cpp

@@ -70,7 +70,7 @@ void SensorBase::fixExtrinsics()
 {
     for (auto key : std::string("PO"))
     {
-        auto sbp = getStateBlockDynamic(key);
+        auto sbp = getStateBlock(key);
         if (sbp != nullptr) sbp->fix();
     }
 }
@@ -79,7 +79,7 @@ void SensorBase::unfixExtrinsics()
 {
     for (auto key : std::string("PO"))
     {
-        auto sbp = getStateBlockDynamic(key);
+        auto sbp = getStateBlock(key);
         if (sbp != nullptr) sbp->unfix();
     }
 }
@@ -90,7 +90,7 @@ void SensorBase::fixIntrinsics()
     {
         if (key != 'P' and key != 'O')  // exclude extrinsics
         {
-            auto sbp = getStateBlockDynamic(key);
+            auto sbp = getStateBlock(key);
             if (sbp != nullptr) sbp->fix();
         }
     }
@@ -102,7 +102,7 @@ void SensorBase::unfixIntrinsics()
     {
         if (key != 'P' and key != 'O')  // exclude extrinsics
         {
-            auto sbp = getStateBlockDynamic(key);
+            auto sbp = getStateBlock(key);
             if (sbp != nullptr) sbp->unfix();
         }
     }
@@ -173,12 +173,11 @@ CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts)
     auto frame_map = getProblem()->getTrajectory()->getFrameMap();
     for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
     {
-        if (frame_rev_iter->second->getTimeStamp() <= _ts)
+        auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this_sensor());
+        if (capture and capture->getTimeStamp() <= _ts)
         {
-            auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this_sensor());
-            if (capture)
-                // found the most recent Capture made by this sensor !
-                return capture;
+            // found the Capture made by this sensor closest to (before) _ts
+            return capture;
         }
     }
 
@@ -187,62 +186,62 @@ CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts)
 
 StateBlockConstPtr SensorBase::getP(const TimeStamp _ts) const
 {
-    return getStateBlockDynamic('P', _ts);
+    return getStateBlock('P', _ts);
 }
 
 StateBlockConstPtr SensorBase::getO(const TimeStamp _ts) const
 {
-    return getStateBlockDynamic('O', _ts);
+    return getStateBlock('O', _ts);
 }
 
 StateBlockConstPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
 {
-    return getStateBlockDynamic('I', _ts);
+    return getStateBlock('I', _ts);
 }
 
 StateBlockConstPtr SensorBase::getP() const
 {
-    return getStateBlockDynamic('P');
+    return getStateBlock('P');
 }
 
 StateBlockConstPtr SensorBase::getO() const
 {
-    return getStateBlockDynamic('O');
+    return getStateBlock('O');
 }
 
 StateBlockConstPtr SensorBase::getIntrinsic() const
 {
-    return getStateBlockDynamic('I');
+    return getStateBlock('I');
 }
 
 StateBlockPtr SensorBase::getP(const TimeStamp _ts)
 {
-    return getStateBlockDynamic('P', _ts);
+    return getStateBlock('P', _ts);
 }
 
 StateBlockPtr SensorBase::getO(const TimeStamp _ts)
 {
-    return getStateBlockDynamic('O', _ts);
+    return getStateBlock('O', _ts);
 }
 
 StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
 {
-    return getStateBlockDynamic('I', _ts);
+    return getStateBlock('I', _ts);
 }
 
 StateBlockPtr SensorBase::getP()
 {
-    return getStateBlockDynamic('P');
+    return getStateBlock('P');
 }
 
 StateBlockPtr SensorBase::getO()
 {
-    return getStateBlockDynamic('O');
+    return getStateBlock('O');
 }
 
 StateBlockPtr SensorBase::getIntrinsic()
 {
-    return getStateBlockDynamic('I');
+    return getStateBlock('I');
 }
 
 bool SensorBase::process(const CaptureBasePtr capture_ptr)
@@ -279,102 +278,142 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
     processor_list_.remove(_proc_ptr);
 }
 
-StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key) const
+StateBlockConstPtr SensorBase::getStateBlock(const char& _key) const
 {
-    CaptureBaseConstPtr cap;
+    auto cap = getCaptureContainingStateBlock(_key);
 
-    if (isStateBlockInCapture(_key, cap)) return cap->getStateBlock(_key);
+    if (cap) return cap->getStateBlock(_key);
 
-    return getStateBlock(_key);
+    return NodeStateBlocks::getStateBlock(_key);
 }
 
-StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key)
+StateBlockPtr SensorBase::getStateBlock(const char& _key)
 {
-    CaptureBasePtr cap;
+    auto cap = getCaptureContainingStateBlock(_key);
 
-    if (isStateBlockInCapture(_key, cap)) return cap->getStateBlock(_key);
+    if (cap) return cap->getStateBlock(_key);
 
-    return getStateBlock(_key);
+    return NodeStateBlocks::getStateBlock(_key);
 }
 
-StateBlockConstPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
+StateBlockConstPtr SensorBase::getStateBlock(const char& _key, const TimeStamp& _ts) const
 {
-    CaptureBaseConstPtr cap;
+    auto cap = getCaptureContainingStateBlock(_key, _ts);
 
-    if (isStateBlockInCapture(_key, _ts, cap)) return cap->getStateBlock(_key);
+    if (cap) return cap->getStateBlock(_key);
 
-    return getStateBlock(_key);
+    return NodeStateBlocks::getStateBlock(_key);
 }
 
-StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlock(const char& _key, const TimeStamp& _ts)
 {
-    CaptureBasePtr cap;
+    auto cap = getCaptureContainingStateBlock(_key, _ts);
 
-    if (isStateBlockInCapture(_key, _ts, cap)) return cap->getStateBlock(_key);
+    if (cap) return cap->getStateBlock(_key);
 
-    return getStateBlock(_key);
+    return NodeStateBlocks::getStateBlock(_key);
 }
 
-bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBaseConstPtr& _cap) const
+CaptureBaseConstPtr SensorBase::getCaptureContainingStateBlock(const char& _key, const TimeStamp& _ts) const
 {
     if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
     {
-        _cap = last_capture_;
+        // we search for the Capture of this sensor closest to _ts
+        if (not getProblem()) return nullptr;
+
+        CaptureBaseConstPtr closest_cap(nullptr);
+
+        // We iterate in reverse
+        auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+        for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
+        {
+            // Get capture of this sensor
+            auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this_sensor());
+            if (capture and capture->has(_key))
+            {
+                if (not closest_cap)
+                    closest_cap = capture;
 
-        return _cap != nullptr;
+                else if (fabs(closest_cap->getTimeStamp() - _ts) > fabs(capture->getTimeStamp() - _ts))
+                    closest_cap = capture;
+                else
+                    break;
+            }
+        }
+        return closest_cap;
     }
     else
-        return false;
+        return nullptr;
 }
 
-bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap)
+CaptureBasePtr SensorBase::getCaptureContainingStateBlock(const char& _key, const TimeStamp& _ts)
 {
     if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
     {
-        _cap = last_capture_;
+        // we search for the Capture of this sensor closest to _ts
+        if (not getProblem()) return nullptr;
 
-        return _cap != nullptr;
-    }
-    else
-        return false;
-}
+        CaptureBasePtr closest_cap(nullptr);
 
-bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBaseConstPtr& _cap) const
-{
-    if (isStateBlockDynamic(_key))
-    {
-        _cap = findLastCaptureBefore(_ts);
+        // We iterate in reverse
+        auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+        for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
+        {
+            // Get capture of this sensor
+            auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this_sensor());
+            if (capture and capture->has(_key))
+            {
+                if (not closest_cap)
+                    closest_cap = capture;
 
-        return _cap != nullptr;
+                else if (fabs(closest_cap->getTimeStamp() - _ts) > fabs(capture->getTimeStamp() - _ts))
+                    closest_cap = capture;
+                else
+                    break;
+            }
+        }
+        return closest_cap;
     }
     else
-        return false;
+        return nullptr;
 }
 
-bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap)
+CaptureBaseConstPtr SensorBase::getCaptureContainingStateBlock(const char& _key) const
 {
-    if (isStateBlockDynamic(_key))
+    if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
     {
-        _cap = findLastCaptureBefore(_ts);
+        // we search for the most recent Capture of this sensor
+        if (not getProblem()) return nullptr;
 
-        return _cap != nullptr;
+        // We iterate in reverse
+        auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+        for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
+        {
+            // Get capture of this sensor
+            auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this_sensor());
+            if (capture and capture->has(_key)) return capture;
+        }
     }
-    else
-        return false;
-}
-
-bool SensorBase::isStateBlockInCapture(const char& _key) const
-{
-    CaptureBaseConstPtr cap;
-
-    return isStateBlockInCapture(_key, cap);
+    return nullptr;
 }
 
-bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const
+CaptureBasePtr SensorBase::getCaptureContainingStateBlock(const char& _key)
 {
-    CaptureBaseConstPtr cap;
+    if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
+    {
+        // we search for the most recent Capture of this sensor
+        if (not getProblem()) return nullptr;
 
-    return isStateBlockInCapture(_key, _ts, cap);
+        // We iterate in reverse
+        auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+        for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
+        {
+            // Get capture of this sensor
+            auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this_sensor());
+            if (capture and capture->has(_key)) return capture;
+        }
+    }
+    return nullptr;
 }
 
 void SensorBase::setProblem(ProblemPtr _problem)
@@ -429,7 +468,7 @@ void SensorBase::printState(bool          _factored_by,
     {
         for (auto key : getKeys())
         {
-            auto sb = getStateBlockDynamic(key);
+            auto sb = getStateBlock(key);
             if (sb)
             {
                 _stream << _tabs << "  " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", "
@@ -460,7 +499,7 @@ void SensorBase::printState(bool          _factored_by,
                 << "sb:";
         for (auto key : getKeys())
         {
-            auto sb = getStateBlockDynamic(key);
+            auto sb = getStateBlock(key);
             if (sb)
                 _stream << "    " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", "
                         << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] @ " << sb;

test/gtest_problem.cpp

@@ -753,6 +753,8 @@ TEST(Problem, transform)
 
     // sensor parameters should not be transformed
     ASSERT_MATRIX_APPROX(S->getStateBlock('b')->getState(), param_sensor, 1e-10);
+
+    WOLF_INFO("DONE!");
 }
 
 int main(int argc, char **argv)

test/gtest_sensor_base.cpp

@@ -68,9 +68,9 @@ void checkSensor(SensorBasePtr   S,
                  const VectorXd& _drift_std)
 {
     // state
-    ASSERT_MATRIX_APPROX(_state, S->getStateBlockDynamic(_key)->getState(), Constants::EPS);
+    ASSERT_MATRIX_APPROX(_state, S->getStateBlock(_key)->getState(), Constants::EPS);
     // fixed
-    ASSERT_EQ(S->getStateBlockDynamic(_key)->isFixed(), _fixed);
+    ASSERT_EQ(S->getStateBlock(_key)->isFixed(), _fixed);
     // dynamic
     ASSERT_EQ(S->isStateBlockDynamic(_key), _dynamic);
     // drift

test/gtest_sensor_odom.cpp

@@ -50,9 +50,9 @@ void checkSensor(SensorBasePtr   S,
                  const VectorXd& _drift_std)
 {
     // state
-    ASSERT_MATRIX_APPROX(_state, S->getStateBlockDynamic(_key)->getState(), Constants::EPS);
+    ASSERT_MATRIX_APPROX(_state, S->getStateBlock(_key)->getState(), Constants::EPS);
     // fixed
-    ASSERT_EQ(S->getStateBlockDynamic(_key)->isFixed(), _fixed);
+    ASSERT_EQ(S->getStateBlock(_key)->isFixed(), _fixed);
     // dynamic
     ASSERT_EQ(S->isStateBlockDynamic(_key), _dynamic);
     // drift

test/gtest_sensor_pose.cpp

@@ -45,9 +45,9 @@ void checkSensor(SensorBasePtr   S,
                  const VectorXd& _drift_std)
 {
     // state
-    ASSERT_MATRIX_APPROX(_state, S->getStateBlockDynamic(_key)->getState(), Constants::EPS);
+    ASSERT_MATRIX_APPROX(_state, S->getStateBlock(_key)->getState(), Constants::EPS);
     // fixed
-    ASSERT_EQ(S->getStateBlockDynamic(_key)->isFixed(), _fixed);
+    ASSERT_EQ(S->getStateBlock(_key)->isFixed(), _fixed);
     // dynamic
     ASSERT_EQ(S->isStateBlockDynamic(_key), _dynamic);
     // drift