diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 387fd71d5c82dd05fc4d14aab782254cca28557b..c660f7766163e8c9957e4663964dbda2dc5ec692 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -56,7 +56,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
bool process();
- unsigned int id();
+ unsigned int id() const;
TimeStamp getTimeStamp() const;
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
@@ -68,7 +68,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
public:
const FeatureBasePtrList& getFeatureList() const;
- void getFactorList(FactorBasePtrList& _fac_list);
+ void getFactorList(FactorBasePtrList& _fac_list) const;
SensorBasePtr getSensor() const;
virtual void setSensor(const SensorBasePtr sensor_ptr);
@@ -177,7 +177,7 @@ inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
return getStateBlock(2);
}
-inline unsigned int CaptureBase::id()
+inline unsigned int CaptureBase::id() const
{
return capture_id_;
}
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index b9875aaea28c99b29fcbcc3fa7b8a1a1132edd94..4ea8cb172cd8503788125d441c79569a9cf15f75 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -84,16 +84,16 @@ class CaptureMotion : public CaptureBase
// Buffer's initial conditions for pre-integration
VectorXs getCalibrationPreint() const;
void setCalibrationPreint(const VectorXs& _calib_preint);
- MatrixXs getJacobianCalib();
- MatrixXs getJacobianCalib(const TimeStamp& _ts);
+ MatrixXs getJacobianCalib() const;
+ MatrixXs getJacobianCalib(const TimeStamp& _ts) const;
// Get delta preintegrated, and corrected for changes on calibration params
- VectorXs getDeltaCorrected(const VectorXs& _calib_current);
- VectorXs getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts);
- VectorXs getDeltaPreint();
- VectorXs getDeltaPreint(const TimeStamp& _ts);
- MatrixXs getDeltaPreintCov();
- MatrixXs getDeltaPreintCov(const TimeStamp& _ts);
+ VectorXs getDeltaCorrected(const VectorXs& _calib_current) const;
+ VectorXs getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts) const;
+ VectorXs getDeltaPreint() const;
+ VectorXs getDeltaPreint(const TimeStamp& _ts) const;
+ MatrixXs getDeltaPreintCov() const;
+ MatrixXs getDeltaPreintCov(const TimeStamp& _ts) const;
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const;
// Origin frame
@@ -142,12 +142,12 @@ inline MotionBuffer& CaptureMotion::getBuffer()
return buffer_;
}
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib()
+inline Eigen::MatrixXs CaptureMotion::getJacobianCalib() const
{
return getBuffer().get().back().jacobian_calib_;
}
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts)
+inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).jacobian_calib_;
}
@@ -178,22 +178,22 @@ inline void CaptureMotion::setCalibrationPreint(const VectorXs& _calib_preint)
calib_preint_ = _calib_preint;
}
-inline VectorXs CaptureMotion::getDeltaPreint()
+inline VectorXs CaptureMotion::getDeltaPreint() const
{
return getBuffer().get().back().delta_integr_;
}
-inline VectorXs CaptureMotion::getDeltaPreint(const TimeStamp& _ts)
+inline VectorXs CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_;
}
-inline MatrixXs CaptureMotion::getDeltaPreintCov()
+inline MatrixXs CaptureMotion::getDeltaPreintCov() const
{
return getBuffer().get().back().delta_integr_cov_;
}
-inline MatrixXs CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts)
+inline MatrixXs CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_cov_;
}
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 6593383e5137629b94a5f60cf17bc053b0b9991b..ee6ecda364be67b6cfbe07589d7df7a24ad8d069 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -138,7 +138,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Gets the apply loss function flag
*/
- bool getApplyLossFunction();
+ bool getApplyLossFunction() const;
/** \brief Returns a pointer to the frame constrained to
**/
@@ -214,7 +214,7 @@ inline FactorStatus FactorBase::getStatus() const
return status_;
}
-inline bool FactorBase::getApplyLossFunction()
+inline bool FactorBase::getApplyLossFunction() const
{
return apply_loss_function_;
}
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 65e9228f5b48a6c9687c95d3c1d10c5cce58eb3b..83ec82f21b55bf6991dd7c3a9f0e91b43d10e520 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -60,7 +60,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
// properties
- unsigned int id();
+ unsigned int id() const;
unsigned int trackId(){return track_id_;}
void setTrackId(unsigned int _tr_id){track_id_ = _tr_id;}
unsigned int landmarkId(){return landmark_id_;}
@@ -139,7 +139,7 @@ inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
return constrained_by_list_;
}
-inline unsigned int FeatureBase::id()
+inline unsigned int FeatureBase::id() const
{
return feature_id_;
}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index e3548b5300b69f29f9bca6992a12f4b2a64c4f04..2ee09400160a1001190a14aa4bc23812d373fe1c 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -75,7 +75,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
// Frame properties -----------------------------------------------
public:
- unsigned int id();
+ unsigned int id() const;
// get type
bool isKey() const;
@@ -141,8 +141,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
- FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
void getFactorList(FactorBasePtrList& _fac_list);
unsigned int getHits() const;
@@ -180,7 +180,7 @@ std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all
return frm;
}
-inline unsigned int FrameBase::id()
+inline unsigned int FrameBase::id() const
{
return frame_id_;
}
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 853ffc3c104dd9bb4e518b939e31a50ab1f219d4..21e5f1d0cd092a15554ef5cec46563a40317417b 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -54,7 +54,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
virtual YAML::Node saveToYaml() const;
// Properties
- unsigned int id();
+ unsigned int id() const;
void setId(unsigned int _id);
// Fix / unfix
@@ -90,7 +90,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
- MapBasePtr getMap();
+ MapBasePtr getMap() const;
void link(MapBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
@@ -124,7 +124,7 @@ std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all
return lmk;
}
-inline MapBasePtr LandmarkBase::getMap()
+inline MapBasePtr LandmarkBase::getMap() const
{
return map_ptr_.lock();
}
@@ -134,7 +134,7 @@ inline void LandmarkBase::setMap(const MapBasePtr _map_ptr)
map_ptr_ = _map_ptr;
}
-inline unsigned int LandmarkBase::id()
+inline unsigned int LandmarkBase::id() const
{
return landmark_id_;
}
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index c61662c7acbd4493298614496ea83f221ec273a1..aae0dfb0cda3fc03d13b13b5f6c50cc460301a3c 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -105,7 +105,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
- SensorBasePtr getSensor(const std::string& _sensor_name);
+ SensorBasePtr getSensor(const std::string& _sensor_name) const;
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -261,13 +261,13 @@ class Problem : public std::enable_shared_from_this<Problem>
const Scalar& _time_tolerance);
// State getters
- Eigen::VectorXs getCurrentState ( );
- void getCurrentState (Eigen::VectorXs& state);
- void getCurrentStateAndStamp (Eigen::VectorXs& state, TimeStamp& _ts);
- Eigen::VectorXs getState (const TimeStamp& _ts);
- void getState (const TimeStamp& _ts, Eigen::VectorXs& state);
+ Eigen::VectorXs getCurrentState ( ) const;
+ void getCurrentState (Eigen::VectorXs& state) const;
+ void getCurrentStateAndStamp (Eigen::VectorXs& state, TimeStamp& _ts) const;
+ Eigen::VectorXs getState (const TimeStamp& _ts) const;
+ void getState (const TimeStamp& _ts, Eigen::VectorXs& state) const;
// Zero state provider
- Eigen::VectorXs zeroState ( );
+ Eigen::VectorXs zeroState ( ) const;
bool priorIsSet() const;
// Map branch -----------------------------------------
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index d57846275c3550cfc208cfb849abdf676ce9d9fc..c416ed976e01b3d072c5520a2c31a4e76b7f291b 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -253,7 +253,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
- unsigned int id();
+ unsigned int id() const;
protected:
/** \brief process an incoming capture
@@ -318,8 +318,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
void captureCallback(CaptureBasePtr _capture_ptr);
- SensorBasePtr getSensor();
- const SensorBasePtr getSensor() const;
+// SensorBasePtr getSensor();
+ SensorBasePtr getSensor() const;
private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
@@ -365,17 +365,17 @@ inline bool ProcessorBase::isMotion() const
return false;
}
-inline unsigned int ProcessorBase::id()
+inline unsigned int ProcessorBase::id() const
{
return processor_id_;
}
-inline SensorBasePtr ProcessorBase::getSensor()
-{
- return sensor_ptr_.lock();
-}
+//inline SensorBasePtr ProcessorBase::getSensor()
+//{
+// return sensor_ptr_.lock();
+//}
-inline const SensorBasePtr ProcessorBase::getSensor() const
+inline SensorBasePtr ProcessorBase::getSensor() const
{
return sensor_ptr_.lock();
}
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 41fc77be5711986fadee3e4d16996826d2938373..0de934bca5dd2ef91eb77522c9db20a5eadfbc19 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -165,13 +165,13 @@ class ProcessorMotion : public ProcessorBase
* \param _x the returned state vector
*/
void getCurrentState(Eigen::VectorXs& _x);
- void getCurrentTimeStamp(TimeStamp& _ts){ _ts = getBuffer().get().back().ts_; }
+ void getCurrentTimeStamp(TimeStamp& _ts) const { _ts = getBuffer().get().back().ts_; }
/** \brief Get the state integrated so far
* \return the state vector
*/
Eigen::VectorXs getCurrentState();
- TimeStamp getCurrentTimeStamp();
+ TimeStamp getCurrentTimeStamp() const;
/** \brief Fill the state corresponding to the provided time-stamp
* \param _ts the time stamp
@@ -189,7 +189,7 @@ class ProcessorMotion : public ProcessorBase
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
*/
- const Eigen::MatrixXs getCurrentDeltaPreintCov();
+ const Eigen::MatrixXs getCurrentDeltaPreintCov() const;
/** \brief Provide the motion integrated so far
* \return the integrated motion
@@ -437,9 +437,9 @@ class ProcessorMotion : public ProcessorBase
public:
//getters
- CaptureMotionPtr getOrigin();
- CaptureMotionPtr getLast();
- CaptureMotionPtr getIncoming();
+ CaptureMotionPtr getOrigin() const;
+ CaptureMotionPtr getLast() const;
+ CaptureMotionPtr getIncoming() const;
Scalar getMaxTimeSpan() const;
Scalar getMaxBuffLength() const;
@@ -501,7 +501,7 @@ inline Eigen::VectorXs ProcessorMotion::getState(const TimeStamp& _ts)
return x;
}
-inline TimeStamp ProcessorMotion::getCurrentTimeStamp()
+inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const
{
return getBuffer().get().back().ts_;
}
@@ -519,7 +519,7 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
-inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov()
+inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov() const
{
return getBuffer().get().back().delta_integr_cov_;
}
@@ -572,17 +572,17 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
);
}
-inline CaptureMotionPtr ProcessorMotion::getOrigin()
+inline CaptureMotionPtr ProcessorMotion::getOrigin() const
{
return origin_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getLast()
+inline CaptureMotionPtr ProcessorMotion::getLast() const
{
return last_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getIncoming()
+inline CaptureMotionPtr ProcessorMotion::getIncoming() const
{
return incoming_ptr_;
}
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index a1e64518606e559ab6c80c4f8b669850e2f46b7f..280252a64942995f964e5bc87ec4248da774434d 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -117,9 +117,9 @@ class ProcessorTracker : public ProcessorBase
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
- virtual CaptureBasePtr getOrigin();
- virtual CaptureBasePtr getLast();
- virtual CaptureBasePtr getIncoming();
+ virtual CaptureBasePtr getOrigin() const;
+ virtual CaptureBasePtr getLast() const;
+ virtual CaptureBasePtr getIncoming() const;
protected:
/** \brief process an incoming capture
@@ -286,17 +286,17 @@ inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
new_features_incoming_.push_back(_feature_ptr);
}
-inline CaptureBasePtr ProcessorTracker::getOrigin()
+inline CaptureBasePtr ProcessorTracker::getOrigin() const
{
return origin_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getLast()
+inline CaptureBasePtr ProcessorTracker::getLast() const
{
return last_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getIncoming()
+inline CaptureBasePtr ProcessorTracker::getIncoming() const
{
return incoming_ptr_;
}
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 67e0c032d4d21ac4a5d3c5af17383b3b09610871..012fd71348ae2e3946330486fc856bbb7853726b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -148,10 +148,10 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
virtual ~SensorBase();
- unsigned int id();
+ unsigned int id() const;
- HardwareBasePtr getHardware();
+ HardwareBasePtr getHardware() const;
private:
void setHardware(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
@@ -240,8 +240,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void setNoiseStd(const Eigen::VectorXs & _noise_std);
void setNoiseCov(const Eigen::MatrixXs & _noise_std);
- Eigen::VectorXs getNoiseStd();
- Eigen::MatrixXs getNoiseCov();
+ Eigen::VectorXs getNoiseStd() const;
+ Eigen::MatrixXs getNoiseCov() const;
void setExtrinsicDynamic(bool _extrinsic_dynamic);
void setIntrinsicDynamic(bool _intrinsic_dynamic);
@@ -273,7 +273,7 @@ std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&...
return sen;
}
-inline unsigned int SensorBase::id()
+inline unsigned int SensorBase::id() const
{
return sensor_id_;
}
@@ -324,17 +324,17 @@ inline bool SensorBase::isIntrinsicDynamic() const
return intrinsic_dynamic_;
}
-inline Eigen::VectorXs SensorBase::getNoiseStd()
+inline Eigen::VectorXs SensorBase::getNoiseStd() const
{
return noise_std_;
}
-inline Eigen::MatrixXs SensorBase::getNoiseCov()
+inline Eigen::MatrixXs SensorBase::getNoiseCov() const
{
return noise_cov_;
}
-inline HardwareBasePtr SensorBase::getHardware()
+inline HardwareBasePtr SensorBase::getHardware() const
{
return hardware_ptr_.lock();
}
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 899327294dadc122747223403e26b6d0e58b67f4..1001062fcb522bf500dd89985a33669ee08c59ee 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -107,7 +107,7 @@ void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
feature_list_.remove(_ft_ptr);
}
-void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
+void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const
{
for (auto f_ptr : getFeatureList())
f_ptr->getFactorList(_fac_list);
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 90b282f2a5e5fdba4bcfd9fc209b8ff9f2c11e7e..47d26a131c25cffb0014ca5472ad744070929eaf 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -51,7 +51,7 @@ CaptureMotion::~CaptureMotion()
//
}
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
+Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current) const
{
VectorXs calib_preint = getCalibrationPreint();
VectorXs delta_preint = getBuffer().get().back().delta_integr_;
@@ -61,7 +61,7 @@ Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
return delta_corrected;
}
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts)
+Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts) const
{
Motion motion = getBuffer().getMotion(_ts);
VectorXs delta_preint = motion.delta_integr_;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 8a87eae58494bc5c8b56bdf4b068e5ffb42bfdcf..633d579eb3f26bfa2305d93e583e71cfd3c7422b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -369,7 +369,7 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
return captures;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
{
for (const FactorBasePtr& factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
@@ -377,7 +377,7 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, cons
return nullptr;
}
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
{
for (const FactorBasePtr& factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index bdb726c5bbaa9ecf90cfca064a81ec72fed060c8..37a841abeeaf8caf6daf56f12d3f00689bd3aa6d 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -234,7 +234,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return prc_ptr;
}
-SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
+SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
{
auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
getHardware()->getSensorList().end(),
@@ -321,14 +321,14 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
}
-Eigen::VectorXs Problem::getCurrentState()
+Eigen::VectorXs Problem::getCurrentState() const
{
Eigen::VectorXs state(getFrameStructureSize());
getCurrentState(state);
return state;
}
-void Problem::getCurrentState(Eigen::VectorXs& state)
+void Problem::getCurrentState(Eigen::VectorXs& state) const
{
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(state);
@@ -338,7 +338,7 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
state = zeroState();
}
-void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
+void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts) const
{
if (processor_motion_ptr_ != nullptr)
{
@@ -354,7 +354,7 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
state = zeroState();
}
-void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
+void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state) const
{
// try to get the state from processor_motion if any, otherwise...
if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
@@ -367,7 +367,7 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
}
}
-Eigen::VectorXs Problem::getState(const TimeStamp& _ts)
+Eigen::VectorXs Problem::getState(const TimeStamp& _ts) const
{
Eigen::VectorXs state(getFrameStructureSize());
getState(_ts, state);
@@ -394,7 +394,7 @@ std::string Problem::getFrameStructure() const
return frame_structure_;
}
-Eigen::VectorXs Problem::zeroState()
+Eigen::VectorXs Problem::zeroState() const
{
Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());