changed isDynamic for isStateBlockDynamic

src/sensor_base.cpp

@@ -342,33 +342,25 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
 
 StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
 {
-    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
-    {
-        CaptureBasePtr cap = lastKeyCapture();
-        if (cap)
-            return cap->getStateBlockPtr(_i);
-        else
-            return getStateBlockPtrStatic(_i);
-    }
-    else
-        return getStateBlockPtrStatic(_i);
+    CaptureBasePtr cap;
+
+    if (isStateBlockDynamic(_i, cap))
+        return cap->getStateBlockPtr(_i);
+
+    return getStateBlockPtrStatic(_i);
 }
 
 StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
 {
-    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
-    {
-        CaptureBasePtr cap = lastCapture(_ts);
-        if (cap)
-            return cap->getStateBlockPtr(_i);
-        else
-            return getStateBlockPtrStatic(_i);
-    }
-    else
-        return getStateBlockPtrStatic(_i);
+    CaptureBasePtr cap;
+
+    if (isStateBlockDynamic(_i, _ts, cap))
+        return cap->getStateBlockPtr(_i);
+
+    return getStateBlockPtrStatic(_i);
 }
 
-bool SensorBase::isDynamic(unsigned int _i, CaptureBasePtr& cap) const
+bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const
 {
     if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
@@ -380,7 +372,7 @@ bool SensorBase::isDynamic(unsigned int _i, CaptureBasePtr& cap) const
         return false;
 }
 
-bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const
 {
     if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
@@ -392,18 +384,18 @@ bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr
         return false;
 }
 
-bool SensorBase::isDynamic(unsigned int _i) const
+bool SensorBase::isStateBlockDynamic(unsigned int _i) const
 {
     CaptureBasePtr& cap;
 
-    return isDynamic(_i,cap);
+    return isStateBlockDynamic(_i,cap);
 }
 
-bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts) const
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const
 {
     CaptureBasePtr& cap;
 
-    return isDynamic(_i,_ts,cap);
+    return isStateBlockDynamic(_i,_ts,cap);
 }
 
 void SensorBase::setProblem(ProblemPtr _problem)

src/sensor_base.h

@@ -117,10 +117,11 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
         void resizeStateBlockVec(unsigned int _size);
 
-        bool isDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const;
-        bool isDynamic(unsigned int _i, CaptureBasePtr& cap) const;
-        bool isDynamic(unsigned int _i, const TimeStamp& _ts) const;
-        bool isDynamic(unsigned int _i) const;
+        bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const;
+        bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const;
+        bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const;
+        bool isStateBlockDynamic(unsigned int _i) const;
+
         StateBlockPtr getPPtr(const TimeStamp _ts);
         StateBlockPtr getOPtr(const TimeStamp _ts);
         StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
@@ -138,6 +139,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         void unfixExtrinsics();
         void fixIntrinsics();
         void unfixIntrinsics();
+
         /** \brief Add an absolute constraint to a parameter
          *
          * Add an absolute constraint to a parameter
@@ -198,6 +200,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
                                 const Eigen::MatrixXs& _cov,
                                 unsigned int _start_idx = 0,
                                 int _size = -1);
+
         SizeEigen getCalibSize() const;
         Eigen::VectorXs getCalibration() const;
 
@@ -323,7 +326,7 @@ inline void SensorBase::addParameterPrior(const unsigned int _i, const TimeStamp
 {
     CaptureBasePtr cap;
     // i is dynamic? //TODO
-    if (isDynamic(_i, _ts, cap))
+    if (isStateBlockDynamic(_i, _ts, cap))
         addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
     else
         addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
@@ -334,7 +337,7 @@ inline void SensorBase::addParameterPrior(const unsigned int _i, const Eigen::Ve
 {
     CaptureBasePtr cap;
     // i is dynamic? //TODO
-    if (isDynamic(_i, cap))
+    if (isStateBlockDynamic(_i, cap))
         addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
     else
         addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);