diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index 16311a1968a9fea0a8f2f80cdc593d4b6c8f353f..cd1439f96e437dc39dde137dd1014f37abee10db 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -144,6 +144,14 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa **/ CaptureBasePtr getCapture() const; + /** \brief Returns a pointer to its frame + **/ + FrameBasePtr getFrame() const; + + /** \brief Returns a pointer to its capture's sensor + **/ + SensorBasePtr getSensor() const; + /** \brief Returns the factor residual size **/ virtual unsigned int getSize() const = 0; diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 3b6b99fe8edb0aad8f2cbe26b582b42cffd4b359..72b36c519a0d8d0ce2cb7ac4748e0dfc110026d0 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -155,6 +155,18 @@ CaptureBasePtr FactorBase::getCapture() const return getFeature()->getCapture(); } +FrameBasePtr FactorBase::getFrame() const +{ + assert(getCapture() != nullptr && "calling getFrame before linking with a capture"); + return getCapture()->getFrame(); +} + +SensorBasePtr FactorBase::getSensor() const +{ + assert(getCapture() != nullptr && "calling getSensor before linking with a capture"); + return getCapture()->getSensor(); +} + void FactorBase::setStatus(FactorStatus _status) { if (getProblem() == nullptr)