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