Added isDynamic functions

src/sensor_base.cpp

@@ -368,6 +368,44 @@ StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeSta
         return getStateBlockPtrStatic(_i);
 }
 
+bool SensorBase::isDynamic(unsigned int _i, CaptureBasePtr& cap) const
+{
+    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
+    {
+        cap = lastKeyCapture();
+
+        return cap != nullptr;
+    }
+    else
+        return false;
+}
+
+bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const
+{
+    if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
+    {
+        cap = lastCapture(_ts);
+
+        return cap != nullptr;
+    }
+    else
+        return false;
+}
+
+bool SensorBase::isDynamic(unsigned int _i) const
+{
+    CaptureBasePtr& cap;
+
+    return isDynamic(_i,cap);
+}
+
+bool SensorBase::isDynamic(unsigned int _i, const TimeStamp& _ts) const
+{
+    CaptureBasePtr& cap;
+
+    return isDynamic(_i,_ts,cap);
+}
+
 void SensorBase::setProblem(ProblemPtr _problem)
 {
     NodeBase::setProblem(_problem);

src/sensor_base.h

@@ -117,6 +117,10 @@ 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;
         StateBlockPtr getPPtr(const TimeStamp _ts);
         StateBlockPtr getOPtr(const TimeStamp _ts);
         StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);