Merge branch 'kfpackmanager_alt' into kfpackmanager

src/problem.cpp

@@ -704,7 +704,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                 {
                     if (i==0) cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
                     if (i==2) cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    auto sb = S->getStateBlockPtrAuto(i);
+                    auto sb = S->getStateBlockPtrDynamic(i);
                     if (sb)
                     {
                         cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";

src/sensor_base.cpp

@@ -218,6 +218,23 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
     noise_cov_ = _noise_cov;
 }
 
+CaptureBasePtr SensorBase::lastCapture(void)
+{
+    // we search for the most recent Capture of this sensor
+    CaptureBasePtr capture = nullptr;
+    FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
+    FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+    while (frame_rev_it != frame_list.rend())
+    {
+        CaptureBasePtr 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;
+}
+
 CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
 {
     // we search for the most recent Capture of this sensor before _ts
@@ -255,44 +272,17 @@ StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
 
 StateBlockPtr SensorBase::getPPtr()
 {
-    ProblemPtr P = getProblem();
-    if (P)
-    {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getPPtr(KF->getTimeStamp());
-        }
-    }
-    return state_block_vec_[0];
+    return getStateBlockPtrDynamic(0);
 }
 
 StateBlockPtr SensorBase::getOPtr()
 {
-    ProblemPtr P = getProblem();
-    if (P)
-    {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getOPtr(KF->getTimeStamp());
-        }
-    }
-    return state_block_vec_[1];
+    return getStateBlockPtrDynamic(1);
 }
 
 StateBlockPtr SensorBase::getIntrinsicPtr()
 {
-    ProblemPtr P = getProblem();
-    if (P)
-    {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getIntrinsicPtr(KF->getTimeStamp());
-        }
-    }
-    return state_block_vec_[2];
+    return getStateBlockPtrDynamic(2);
 }
 
 wolf::Size SensorBase::computeCalibSize() const
@@ -346,30 +336,32 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
     return _proc_ptr;
 }
 
-StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
 {
-    assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
-    if (((_i < 2) && extrinsicsInCaptures()) || ((_i >= 2) && intrinsicsInCaptures()))
+    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
-        CaptureBasePtr cap = lastCapture(_ts);
-        return cap->getStateBlockPtr(_i);
+        CaptureBasePtr cap = lastCapture();
+        if (cap)
+            return cap->getStateBlockPtr(_i);
+        else
+            return getStateBlockPtrStatic(_i);
     }
     else
-        return state_block_vec_[_i];
+        return getStateBlockPtrStatic(_i);
 }
 
-StateBlockPtr SensorBase::getStateBlockPtrAuto(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
 {
-    ProblemPtr P = getProblem();
-    if (P)
+    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
-        FrameBasePtr KF = P->getLastKeyFramePtr();
-        if (KF)
-        {
-            return getStateBlockPtrDynamic(_i, KF->getTimeStamp());
-        }
+        CaptureBasePtr cap = lastCapture(_ts);
+        if (cap)
+            return cap->getStateBlockPtr(_i);
+        else
+            return getStateBlockPtrStatic(_i);
     }
-    return state_block_vec_[_i];
+    else
+        return getStateBlockPtrStatic(_i);
 }
 
 } // namespace wolf

src/sensor_base.h

@@ -102,6 +102,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
         ProcessorBaseList& getProcessorList();
 
+        CaptureBasePtr lastCapture(void);
         CaptureBasePtr lastCapture(const TimeStamp& _ts);
 
         bool process(const CaptureBasePtr capture_ptr);
@@ -110,8 +111,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
         StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
+        StateBlockPtr getStateBlockPtrDynamic(unsigned int _i);
         StateBlockPtr getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts);
-        StateBlockPtr getStateBlockPtrAuto(unsigned int _i);
         void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
         void resizeStateBlockVec(int _size);