diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index 9bff803ed457149143ceaec76dd6b56f8bc4d415..1fb338827f0bb4319d49467b331d3a2a04a79d29 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 1e690affe078716a233fb87e817fe431a9964d76..caffcf953de56ae5b4b97d8c2e6ed7cf356df53e 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);