Merge branch '380-sensorbase-last_capture_' into 'devel'

Resolve "SensorBase::last_capture_"

Closes #380

See merge request !401

include/core/sensor/sensor_base.h

@@ -104,6 +104,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         Eigen::VectorXd noise_std_; // std of sensor noise
         Eigen::MatrixXd noise_cov_; // cov matrix of noise
 
+        CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
+
         void setProblem(ProblemPtr _problem) override final;
 
     public:
@@ -166,9 +168,10 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
     public:
         const ProcessorBasePtrList& getProcessorList() const;
 
-        CaptureBasePtr lastCapture(void) const;
-        CaptureBasePtr lastKeyCapture(void) const;
-        CaptureBasePtr lastCapture(const TimeStamp& _ts) const;
+        CaptureBasePtr getLastCapture() const;
+        void setLastCapture(CaptureBasePtr);
+        void updateLastCapture();
+        CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts) const;
 
         bool process(const CaptureBasePtr capture_ptr);
 
@@ -293,6 +296,11 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
     return processor_list_;
 }
 
+inline CaptureBasePtr SensorBase::getLastCapture() const
+{
+    return last_capture_;
+}
+
 inline StateBlockPtr SensorBase::addStateBlock(const char& _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");

src/capture/capture_base.cpp

@@ -70,6 +70,10 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
         is_removing_ = true;
         CaptureBasePtr this_C = shared_from_this();  // keep this alive while removing it
 
+        // SensorBase::last_capture_
+        if (getSensor() and getSensor()->getLastCapture() == this_C)
+            getSensor()->updateLastCapture();
+
         // remove downstream
         while (!constrained_by_list_.empty())
         {
@@ -228,6 +232,12 @@ void CaptureBase::setProblem(ProblemPtr _problem)
     NodeBase::setProblem(_problem);
     registerNewStateBlocks(_problem);
 
+    // update SensorBase::last_capture_
+    if (getSensor() and
+        getSensor()->getLastCapture() and
+        getSensor()->getLastCapture()->getTimeStamp() < time_stamp_)
+        getSensor()->setLastCapture(shared_from_this());
+
     for (auto ft : feature_list_)
         ft->setProblem(_problem);
 }

src/sensor/sensor_base.cpp

@@ -23,7 +23,8 @@ SensorBase::SensorBase(const std::string& _type,
         calib_size_(0),
         sensor_id_(++sensor_id_count_), // simple ID factory
         noise_std_(_noise_size),
-        noise_cov_(_noise_size, _noise_size)
+        noise_cov_(_noise_size, _noise_size),
+        last_capture_(nullptr)
 {
     assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
     assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
@@ -58,7 +59,8 @@ SensorBase::SensorBase(const std::string& _type,
         calib_size_(0),
         sensor_id_(++sensor_id_count_), // simple ID factory
         noise_std_(_noise_std),
-        noise_cov_(_noise_std.size(), _noise_std.size())
+        noise_cov_(_noise_std.size(), _noise_std.size()),
+        last_capture_(nullptr)
 {
     setNoiseStd(_noise_std);
 
@@ -214,74 +216,64 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
     noise_cov_ = _noise_cov;
 }
 
-CaptureBasePtr SensorBase::lastCapture(void) const
+void SensorBase::setLastCapture(CaptureBasePtr cap)
 {
-    // we search for the most recent Capture of this sensor which belongs to a KeyFrame
-    CaptureBasePtr capture = nullptr;
-    if (getProblem())
-    {
-        // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
-        auto trajectory = getProblem()->getTrajectory();
-        TrajectoryRevIter frame_rev_it = trajectory->rbegin();
-        while (frame_rev_it != trajectory->rend())
-        {
-            capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
-            if (capture)
-                // found the most recent Capture made by this sensor !
-                break;
-            frame_rev_it++;
-        }
-    }
-    return capture;
+    assert(cap);
+    assert(cap->getSensor() == shared_from_this());
+    assert(cap->getTimeStamp().ok());
+    assert(not last_capture_ or last_capture_->getTimeStamp() < cap->getTimeStamp());
+    last_capture_ = cap;
 }
 
-CaptureBasePtr SensorBase::lastKeyCapture(void) const
+void SensorBase::updateLastCapture()
 {
     // we search for the most recent Capture of this sensor which belongs to a KeyFrame
-    CaptureBasePtr capture = nullptr;
     if (getProblem())
     {
+        // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
         auto trajectory = getProblem()->getTrajectory();
         TrajectoryRevIter frame_rev_it = trajectory->rbegin();
         while (frame_rev_it != trajectory->rend())
         {
-            if ((*frame_rev_it)->isKey())
+            auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+            if (capture and not capture->isRemoving())
             {
-                capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
-                if (capture)
-                    // found the most recent Capture made by this sensor !
-                    break;
+                // found the most recent Capture made by this sensor !
+                last_capture_ = capture;
+                return;
             }
             frame_rev_it++;
         }
     }
-    return capture;
+
+    // no captures found
+    last_capture_ = nullptr;
 }
 
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
+CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
 {
     // we search for the most recent Capture of this sensor before _ts
-    CaptureBasePtr capture = nullptr;
-    if (getProblem())
-    {
-        // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
-        auto trajectory = getProblem()->getTrajectory();
+    if (not getProblem())
+        return nullptr;
 
-        // We iterate in reverse since we're likely to find it close to the rbegin() place.
-        TrajectoryRevIter frame_rev_it = trajectory->rbegin();
-        while (frame_rev_it != trajectory->rend())
+    // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+    auto trajectory = getProblem()->getTrajectory();
+
+    // We iterate in reverse since we're likely to find it close to the rbegin() place.
+    TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+    while (frame_rev_it != trajectory->rend())
+    {
+        if ((*frame_rev_it)->getTimeStamp() <= _ts)
         {
-            if ((*frame_rev_it)->getTimeStamp() <= _ts)
-            {
-                capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
-                if (capture)
-                    // found the most recent Capture made by this sensor !
-                    break;
-            }
-            frame_rev_it++;
+            auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+            if (capture)
+                // found the most recent Capture made by this sensor !
+                return capture;
         }
+        frame_rev_it++;
     }
-    return capture;
+
+    return nullptr;
 }
 
 StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
@@ -405,8 +397,7 @@ bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) c
 {
     if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
     {
-        _cap = lastCapture();
-        //        cap = lastKeyCapture();
+        _cap = last_capture_;
 
         return _cap != nullptr;
     }
@@ -418,7 +409,7 @@ bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, C
 {
     if (isStateBlockDynamic(_key))
     {
-        _cap = lastCapture(_ts);
+        _cap = findLastCaptureBefore(_ts);
 
         return _cap != nullptr;
     }
@@ -561,6 +552,12 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
                                       << " -X-> Sen" << id();
             log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
         }
+        // check last_capture_
+        if (getProblem()->getTimeStamp().ok())
+        {
+            inconsistency_explanation << "SensorBase::last_capture_ is not the actual last capture\n";
+            log.assertTrue((last_capture_ != findLastCaptureBefore(getProblem()->getTimeStamp())), inconsistency_explanation);
+        }
         return log;
 }