From 7f529bea988237446b8a0975bdb0f30151f8e39a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 8 Feb 2019 14:43:04 +0100
Subject: [PATCH] Added isDynamic functions

---
 src/sensor_base.cpp | 38 ++++++++++++++++++++++++++++++++++++++
 src/sensor_base.h   |  4 ++++
 2 files changed, 42 insertions(+)

diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 9bff803ed..1fb338827 100644
--- a/src/sensor_base.cpp
+++ b/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);
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 1e690affe..caffcf953 100644
--- a/src/sensor_base.h
+++ b/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);
-- 
GitLab