diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 65e87f56e76718036e44d73031329e98c361afe4..4df0ebf96b97a28d4feb0f387cbe08de4814e2cb 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -56,9 +56,9 @@ class ProcessorRangeBearing : public ProcessorBase
// Implementation of pure virtuals from ProcessorBase
virtual void processCapture (CaptureBasePtr _capture) override;
virtual void processKeyFrame (FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override {};
- virtual bool triggerInCapture (CaptureBasePtr) override { return true;};
- virtual bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override {return false;}
- virtual bool voteForKeyFrame () override {return false;}
+ virtual bool triggerInCapture (CaptureBasePtr) const override { return true;};
+ virtual bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) const override {return false;}
+ virtual bool voteForKeyFrame () const override {return false;}
private:
// control variables
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 83ec82f21b55bf6991dd7c3a9f0e91b43d10e520..8d4f838d2fade0527e8dde60f82885ca70b419fa 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -61,9 +61,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
// properties
unsigned int id() const;
- unsigned int trackId(){return track_id_;}
+ unsigned int trackId() const {return track_id_;}
void setTrackId(unsigned int _tr_id){track_id_ = _tr_id;}
- unsigned int landmarkId(){return landmark_id_;}
+ unsigned int landmarkId() const {return landmark_id_;}
void setLandmarkId(unsigned int _lmk_id){landmark_id_ = _lmk_id;}
// values
@@ -94,7 +94,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
const FactorBasePtrList& getConstrainedByList() const;
// all factors
- void getFactorList(FactorBasePtrList & _fac_list);
+ void getFactorList(FactorBasePtrList & _fac_list) const;
void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 2ee09400160a1001190a14aa4bc23812d373fe1c..aed6ff774ed52c5d0d4023188515823feca7ffab 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -137,14 +137,14 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
FrameBasePtr getNextFrame() const;
const CaptureBasePtrList& getCaptureList() const;
- CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr);
- CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
- CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
+ CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
+ CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
+ CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
- void getFactorList(FactorBasePtrList& _fac_list);
+ void getFactorList(FactorBasePtrList& _fac_list) const;
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
void link(TrajectoryBasePtr);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index aae0dfb0cda3fc03d13b13b5f6c50cc460301a3c..a7432b15148d942630fa470ea79331d0ca074059 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -234,7 +234,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - This decision is made at problem level, not at processor configuration level.
* - Therefore, if you want to permanently configure a processor for not creating key frames, see Processor::voting_active_ and its accessors.
*/
- bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
+ bool permitKeyFrame(ProcessorBasePtr _processor_ptr) const;
/** \brief New key frame callback
*
@@ -250,7 +250,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - This decision is made at problem level, not at processor configuration level.
* - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors.
*/
- bool permitAuxFrame(ProcessorBasePtr _processor_ptr);
+ bool permitAuxFrame(ProcessorBasePtr _processor_ptr) const;
/** \brief New auxiliary frame callback
*
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c416ed976e01b3d072c5520a2c31a4e76b7f291b..e9290f5ca9674cfe8eab8df5bad4defadd406247 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -274,13 +274,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
- virtual bool triggerInCapture(CaptureBasePtr) = 0;
+ virtual bool triggerInCapture(CaptureBasePtr) const = 0;
/** \brief trigger in key-frame
*
* Returns true if processKeyFrame() should be called after the provided KF arrived.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) = 0;
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const = 0;
/** \brief Vote for KeyFrame generation
*
@@ -289,7 +289,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() = 0;
+ virtual bool voteForKeyFrame() const = 0;
/** \brief Vote for Auxiliary Frame generation
*
@@ -298,7 +298,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*
* WARNING! This function only votes! It does not create Auxiliary Frames!
*/
- virtual bool voteForAuxFrame(){return false;};
+ virtual bool voteForAuxFrame() const {return false;};
virtual bool permittedKeyFrame() final;
@@ -318,7 +318,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
void captureCallback(CaptureBasePtr _capture_ptr);
-// SensorBasePtr getSensor();
SensorBasePtr getSensor() const;
private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
@@ -370,11 +369,6 @@ inline unsigned int ProcessorBase::id() const
return processor_id_;
}
-//inline SensorBasePtr ProcessorBase::getSensor()
-//{
-// return sensor_ptr_.lock();
-//}
-
inline SensorBasePtr ProcessorBase::getSensor() const
{
return sensor_ptr_.lock();
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 6c8bf6a34605dd5da4c9d4766f619b3c7cbc6941..105ad861187988621cecc7af257950a14acec2c7 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -48,7 +48,7 @@ class ProcessorDiffDrive : public ProcessorOdom2D
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) override;
+ Eigen::MatrixXs& _jacobian_calib) const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
index 459b8f3d9a123960be1d7b99b3ca322317726bb7..7f3bfa875ff15d870aa6a5793eb62b241308c43a 100644
--- a/include/core/processor/processor_loopclosure.h
+++ b/include/core/processor/processor_loopclosure.h
@@ -73,13 +73,13 @@ protected:
*
* The ProcessorLoopClosure only processes incoming KF, then it returns false.
*/
- virtual bool triggerInCapture(CaptureBasePtr) override {return false;}
+ virtual bool triggerInCapture(CaptureBasePtr) const override {return false;}
/** \brief trigger in key-frame
*
* Returns true if processKeyFrame() should be called after the provided KF arrived.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override;
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) const override;
/** \brief Called by process(). Tells if computeFeatures() will be called
*/
@@ -160,7 +160,7 @@ protected:
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() override
+ virtual bool voteForKeyFrame() const override
{
return false;
};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index d8a1361ac8e4f3590c7b569f3de18e6a114fb742..eee6c0e1c41289c54d7a3b81b08fdfb07de7bad4 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -164,13 +164,13 @@ class ProcessorMotion : public ProcessorBase
/** \brief Fill a reference to the state integrated so far
* \param _x the returned state vector
*/
- void getCurrentState(Eigen::VectorXs& _x);
+ void getCurrentState(Eigen::VectorXs& _x) const;
void getCurrentTimeStamp(TimeStamp& _ts) const { _ts = getBuffer().get().back().ts_; }
/** \brief Get the state integrated so far
* \return the state vector
*/
- Eigen::VectorXs getCurrentState();
+ Eigen::VectorXs getCurrentState() const;
TimeStamp getCurrentTimeStamp() const;
/** \brief Fill the state corresponding to the provided time-stamp
@@ -178,13 +178,13 @@ class ProcessorMotion : public ProcessorBase
* \param _x the returned state
* \return if state in the provided time-stamp could be resolved
*/
- bool getState(const TimeStamp& _ts, Eigen::VectorXs& _x);
+ bool getState(const TimeStamp& _ts, Eigen::VectorXs& _x) const;
/** \brief Get the state corresponding to the provided time-stamp
* \param _ts the time stamp
* \return the state vector
*/
- Eigen::VectorXs getState(const TimeStamp& _ts);
+ Eigen::VectorXs getState(const TimeStamp& _ts) const;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
@@ -241,15 +241,15 @@ class ProcessorMotion : public ProcessorBase
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
- virtual bool triggerInCapture(CaptureBasePtr) override {return true;}
+ virtual bool triggerInCapture(CaptureBasePtr) const override {return true;}
/** \brief trigger in key-frame
*
* The ProcessorMotion only processes incoming captures, then it returns false.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override {return false;}
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) const override {return false;}
- virtual bool voteForKeyFrame() override;
+ virtual bool voteForKeyFrame() const override;
Scalar updateDt();
void integrateOneStep();
@@ -347,7 +347,7 @@ class ProcessorMotion : public ProcessorBase
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) = 0;
+ Eigen::MatrixXs& _jacobian_calib) const = 0;
/** \brief composes a delta-state on top of another delta-state
* \param _delta1 the first delta-state
@@ -360,7 +360,7 @@ class ProcessorMotion : public ProcessorBase
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _dt2,
- Eigen::VectorXs& _delta1_plus_delta2) = 0;
+ Eigen::VectorXs& _delta1_plus_delta2) const = 0;
/** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
* \param _delta1 the first delta-state
@@ -377,7 +377,7 @@ class ProcessorMotion : public ProcessorBase
const Scalar _dt2,
Eigen::VectorXs& _delta1_plus_delta2,
Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) = 0;
+ Eigen::MatrixXs& _jacobian2) const = 0;
/** \brief composes a delta-state on top of a state
* \param _x the initial state
@@ -390,7 +390,7 @@ class ProcessorMotion : public ProcessorBase
virtual void statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta,
const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta) = 0;
+ Eigen::VectorXs& _x_plus_delta) const = 0;
/** \brief Delta zero
* \return a delta state equivalent to the null motion.
@@ -431,9 +431,9 @@ class ProcessorMotion : public ProcessorBase
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
- Motion motionZero(const TimeStamp& _ts);
+ Motion motionZero(const TimeStamp& _ts) const;
- bool hasCalibration() {return calib_size_ > 0;}
+ bool hasCalibration() const {return calib_size_ > 0;}
public:
//getters
@@ -489,12 +489,12 @@ inline void ProcessorMotion::resetDerived()
// Blank function, to be implemented in derived classes
}
-inline bool ProcessorMotion::voteForKeyFrame()
+inline bool ProcessorMotion::voteForKeyFrame() const
{
return false;
}
-inline Eigen::VectorXs ProcessorMotion::getState(const TimeStamp& _ts)
+inline Eigen::VectorXs ProcessorMotion::getState(const TimeStamp& _ts) const
{
Eigen::VectorXs x(getProblem()->getFrameStructureSize());
getState(_ts, x);
@@ -506,14 +506,14 @@ inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const
return getBuffer().get().back().ts_;
}
-inline Eigen::VectorXs ProcessorMotion::getCurrentState()
+inline Eigen::VectorXs ProcessorMotion::getCurrentState() const
{
Eigen::VectorXs x(getProblem()->getFrameStructureSize());
getCurrentState(x);
return x;
}
-inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
+inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x) const
{
// ensure integrity
assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
@@ -564,7 +564,7 @@ inline MotionBuffer& ProcessorMotion::getBuffer()
return last_ptr_->getBuffer();
}
-inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
+inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
{
return Motion(_ts,
VectorXs::Zero(data_size_), // data
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index a4f6e3358fc0f71745a3df1ce15cf48cc880a7f6..3de67b9a26f89976b6ef109c880516add6af27f5 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -47,7 +47,7 @@ class ProcessorOdom2D : public ProcessorMotion
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorOdom2D, ProcessorParamsOdom2D);
- virtual bool voteForKeyFrame() override;
+ virtual bool voteForKeyFrame() const override;
protected:
virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
@@ -56,21 +56,21 @@ class ProcessorOdom2D : public ProcessorMotion
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) override;
+ Eigen::MatrixXs& _jacobian_calib) const override;
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) override;
+ Eigen::VectorXs& _delta1_plus_delta2) const override;
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2,
Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) override;
+ Eigen::MatrixXs& _jacobian2) const override;
virtual void statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta,
const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta) override;
+ Eigen::VectorXs& _x_plus_delta) const override;
virtual Eigen::VectorXs deltaZero() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index 22f4903c209d11bf31f0e08f302c0ce7a6d8d77f..f9c0906865741bc72912af136ae68121e075707c 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -75,23 +75,23 @@ class ProcessorOdom3D : public ProcessorMotion
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) override;
+ Eigen::MatrixXs& _jacobian_calib) const override;
void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) override;
+ Eigen::VectorXs& _delta1_plus_delta2) const override;
void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2,
Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) override;
+ Eigen::MatrixXs& _jacobian2) const override;
void statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta,
const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta) override;
+ Eigen::VectorXs& _x_plus_delta) const override;
Eigen::VectorXs deltaZero() const override;
- bool voteForKeyFrame() override;
+ bool voteForKeyFrame() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
@@ -116,13 +116,6 @@ class ProcessorOdom3D : public ProcessorMotion
Scalar min_disp_var_; // floor displacement variance when no motion
Scalar min_rot_var_; // floor orientation variance when no motion
- // Eigen::Map helpers
- Eigen::Map<const Eigen::Vector3s> p1_, p2_;
- Eigen::Map<Eigen::Vector3s> p_out_;
- Eigen::Map<const Eigen::Quaternions> q1_, q2_;
- Eigen::Map<Eigen::Quaternions> q_out_;
- void remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x2, Eigen::VectorXs& _x_out);
-
};
inline Eigen::VectorXs ProcessorOdom3D::deltaZero() const
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 280252a64942995f964e5bc87ec4248da774434d..e275246714cc3b4f73be3dd8548b37bc4b99177c 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -139,13 +139,13 @@ class ProcessorTracker : public ProcessorBase
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
- virtual bool triggerInCapture(CaptureBasePtr) override;
+ virtual bool triggerInCapture(CaptureBasePtr) const override;
/** \brief trigger in key-frame
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override {return false;}
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const override {return false;}
/** Pre-process incoming Capture
*
@@ -203,7 +203,7 @@ class ProcessorTracker : public ProcessorBase
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() override = 0;
+ virtual bool voteForKeyFrame() const override = 0;
/** \brief Advance the incoming Capture to become the last.
*
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index a808a421da22a74ad417b6cdc38024f308ff59bf..32da47d13e3fc0393fb7b8b95098d4f6db0751f0 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -147,7 +147,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() = 0;
+ virtual bool voteForKeyFrame() const = 0;
// We overload the advance and reset functions to update the lists of matches
void advanceDerived();
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index c11adecc7b16a9c0c2d367200af6ee9a55d46c64..8809426f8654803fc7012068ff9d89cd1d70ea4c 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -129,7 +129,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() = 0;
+ virtual bool voteForKeyFrame() const = 0;
// We overload the advance and reset functions to update the lists of matches
void advanceDerived();
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 2615ad411adbe9c32efea2bf543cb24657e5ed97..1d8bfb343d851a7c5890a8442d844d3bc7b44e9e 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -84,23 +84,23 @@ class TrackMatrix
void remove (FeatureBasePtr _ftr);
void remove (const SizeStd& _track_id);
void remove (CaptureBasePtr _cap);
- SizeStd numTracks ();
- SizeStd trackSize (const SizeStd& _track_id);
- Track track (const SizeStd& _track_id);
- Snapshot snapshot (CaptureBasePtr _capture);
+ SizeStd numTracks () const;
+ SizeStd trackSize (const SizeStd& _track_id) const;
+ Track track (const SizeStd& _track_id) const;
+ Snapshot snapshot (CaptureBasePtr _capture) const;
vector<FeatureBasePtr>
- trackAsVector(const SizeStd& _track_id);
+ trackAsVector(const SizeStd& _track_id) const;
list<FeatureBasePtr>
- snapshotAsList(CaptureBasePtr _cap);
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
- FeatureBasePtr firstFeature(const SizeStd& _track_id);
- FeatureBasePtr lastFeature (const SizeStd& _track_id);
- FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
- CaptureBasePtr firstCapture(const SizeStd& _track_id);
+ snapshotAsList(CaptureBasePtr _cap) const;
+ TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const;
+ FeatureBasePtr firstFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr lastFeature (const SizeStd& _track_id) const;
+ FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap) const;
+ CaptureBasePtr firstCapture(const SizeStd& _track_id) const;
// tracks across captures that belong to keyframe
// SizeStd numKeyframeTracks();
- Track trackAtKeyframes(size_t _track_id);
+ Track trackAtKeyframes(size_t _track_id) const;
// bool markKeyframe(CaptureBasePtr _capture);
// bool unmarkKeyframe(CaptureBasePtr _capture);
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 012fd71348ae2e3946330486fc856bbb7853726b..9e39b4c290b6c4d0bf9461bf34cd0a7b723a6452 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -160,8 +160,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
public:
const ProcessorBasePtrList& getProcessorList() const;
- CaptureBasePtr lastKeyCapture(void);
- CaptureBasePtr lastCapture(const TimeStamp& _ts);
+ CaptureBasePtr lastKeyCapture(void) const;
+ CaptureBasePtr lastCapture(const TimeStamp& _ts) const;
bool process(const CaptureBasePtr capture_ptr);
@@ -169,22 +169,22 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
- StateBlockPtr getStateBlock(unsigned int _i);
- StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts);
+ StateBlockPtr getStateBlock(unsigned int _i) const;
+ StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts) const;
void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(unsigned int _size);
- bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap);
- bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap);
- bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts);
- bool isStateBlockDynamic(unsigned int _i);
-
- StateBlockPtr getP(const TimeStamp _ts);
- StateBlockPtr getO(const TimeStamp _ts);
- StateBlockPtr getIntrinsic(const TimeStamp _ts);
- StateBlockPtr getP() ;
- StateBlockPtr getO();
- StateBlockPtr getIntrinsic();
+ bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const;
+ bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const;
+ bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const;
+ bool isStateBlockDynamic(unsigned int _i) const;
+
+ StateBlockPtr getP(const TimeStamp _ts) const;
+ StateBlockPtr getO(const TimeStamp _ts) const;
+ StateBlockPtr getIntrinsic(const TimeStamp _ts) const;
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
+ StateBlockPtr getIntrinsic() const;
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
void setIntrinsic(const StateBlockPtr _intr_ptr);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 0645be841b4dd9c68cc2a6f557146101f199b26e..60f81c58afcb881fe8be35aae225c2ee55ac2c69 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -53,7 +53,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
public:
// factors
- void getFactorList(FactorBasePtrList & _fac_list);
+ void getFactorList(FactorBasePtrList & _fac_list) const;
protected:
FrameBaseConstIter computeFrameOrder(FrameBasePtr _frame_ptr);
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 97ef674f4f6cbd73a9cfb9b3e38b5e74c9344739..2f6ceb15bdd8a478a8cff81ef1fb9ace00db3e0f 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -96,7 +96,7 @@ const FactorBasePtrList& FeatureBase::getFactorList() const
return factor_list_;
}
-void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
+void FeatureBase::getFactorList(FactorBasePtrList & _fac_list) const
{
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 633d579eb3f26bfa2305d93e583e71cfd3c7422b..d4202bc490276d0bc8fc858ca6ec54645948619b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -344,7 +344,7 @@ void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
capture_list_.remove(_capt_ptr);
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
+CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
{
for (CaptureBasePtr capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
@@ -352,7 +352,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const st
return nullptr;
}
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr)
+CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
{
for (CaptureBasePtr capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
@@ -360,7 +360,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr)
return nullptr;
}
-CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
+CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
{
CaptureBasePtrList captures;
for (CaptureBasePtr capture_ptr : getCaptureList())
@@ -385,7 +385,7 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) cons
return nullptr;
}
-void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
+void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const
{
for (auto c_ptr : getCaptureList())
c_ptr->getFactorList(_fac_list);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 49a9fab6bf8450592dd78e8b6ac69fd4bfe7a0a3..c4b9677728e442846d422071719e4ec22b3c37d7 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -404,7 +404,7 @@ Eigen::VectorXs Problem::zeroState() const
return state;
}
-bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
+bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
{
// This should implement a black list of processors that have forbidden keyframe creation
// This decision is made at problem level, not at processor configuration level.
@@ -425,7 +425,7 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
}
-bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr)
+bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
{
// This should implement a black list of processors that have forbidden auxiliary frame creation
// This decision is made at problem level, not at processor configuration level.
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index d4605c6f0ab27ef247dd64885de922eece4b0fc6..79de4b220a5e3ea71233aed1a760936ba28f5042 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -51,7 +51,7 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib)
+ Eigen::MatrixXs& _jacobian_calib) const
{
/*
* Differential drive model -----------------------------------------------
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
index c447def54083db4688aec24215e3a3492aa948b0..49b90b0382cf9d4cefc069d7cde7426cdd0b60d4 100644
--- a/src/processor/processor_loopclosure.cpp
+++ b/src/processor/processor_loopclosure.cpp
@@ -71,7 +71,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe_ptr, const Sca
postProcess();
}
-bool ProcessorLoopClosure::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
+bool ProcessorLoopClosure::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) const
{
return true;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index d54036d2d3a9b4a00c80daeceddb836920daed10..862929e78620bcae03b3deed36a64c9909dbf080 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -335,7 +335,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
-bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
+bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) const
{
CaptureMotionPtr capture_motion;
if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 0f094674afb3c902dbb3bac9e983929a170f1839..bf825c29f5f994bb09bfbe839ef56c98d37905f4 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -15,7 +15,7 @@ ProcessorOdom2D::~ProcessorOdom2D()
void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
const Eigen::VectorXs& _calib, const Scalar _dt, Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _jacobian_calib)
+ Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _jacobian_calib) const
{
assert(_data.size() == data_size_ && "Wrong _data vector size");
assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
@@ -44,7 +44,7 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
}
void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2)
+ Eigen::VectorXs& _delta1_plus_delta2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
@@ -57,7 +57,7 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2)
+ Eigen::MatrixXs& _jacobian2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
@@ -82,7 +82,7 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
}
void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta)
+ Eigen::VectorXs& _x_plus_delta) const
{
assert(_x.size() == x_size_ && "Wrong _x vector size");
assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
@@ -92,7 +92,7 @@ void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::Vec
_x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
}
-bool ProcessorOdom2D::voteForKeyFrame()
+bool ProcessorOdom2D::voteForKeyFrame() const
{
// Distance criterion
if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2D_->dist_traveled)
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index c2821646896d2573f7bcaa1ed828f63c0d7813d2..180293e74f0c19689e7ca30919a4afb542ab8726 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -9,9 +9,7 @@ ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) :
k_disp_to_rot_ (0),
k_rot_to_rot_ (0),
min_disp_var_ (0.1), // around 10cm error
- min_rot_var_ (0.1), // around 6 degrees error
- p1_(nullptr), p2_(nullptr), p_out_(nullptr),
- q1_(nullptr), q2_(nullptr), q_out_(nullptr)
+ min_rot_var_ (0.1) // around 6 degrees error
{
//
}
@@ -40,18 +38,18 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib)
+ Eigen::MatrixXs& _jacobian_calib) const
{
assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3D.");
Scalar disp, rot; // displacement and rotation of this motion step
if (_data.size() == (long int)6)
{
// rotation in vector form
- _delta.head<3>() = _data.head<3>();
- new (&q_out_) Eigen::Map<Eigen::Quaternions>(_delta.data() + 3);
- q_out_ = v2q(_data.tail<3>());
- disp = _data.head<3>().norm();
- rot = _data.tail<3>().norm();
+ _delta.head<3>() = _data.head<3>();
+ Eigen::Map<Eigen::Quaternions> q(_delta.data() + 3);
+ q = v2q(_data.tail<3>());
+ disp = _data.head<3>().norm();
+ rot = _data.tail<3>().norm();
}
else
{
@@ -85,14 +83,21 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2)
+ Eigen::MatrixXs& _jacobian2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
assert(_jacobian1.rows() == delta_cov_size_ && _jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
assert(_jacobian2.rows() == delta_cov_size_ && _jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
- remap(_delta1, _delta2, _delta1_plus_delta2);
+
+ Eigen::Map<const Eigen::Vector3s> dp1 (_delta1.data() );
+ Eigen::Map<const Eigen::Quaternions> dq1 (_delta1.data() + 3 );
+ Eigen::Map<const Eigen::Vector3s> dp2 (_delta2.data() );
+ Eigen::Map<const Eigen::Quaternions> dq2 (_delta2.data() + 3 );
+ Eigen::Map<Eigen::Vector3s> dp_out (_delta1_plus_delta2.data() );
+ Eigen::Map<Eigen::Quaternions> dq_out (_delta1_plus_delta2.data() + 3 );
+
/* Jacobians of D' = D (+) d
* with: D = [Dp Dq]
* d = [dp dq]
@@ -109,47 +114,63 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
*/
// temporaries
- Eigen::Matrix3s DR = q1_.matrix();
- Eigen::Matrix3s dR = q2_.matrix();
+ Eigen::Matrix3s dR1 = dq1.matrix();
+ Eigen::Matrix3s dR2 = dq2.matrix();
// fill Jacobians
_jacobian1.setIdentity();
- _jacobian1.block<3, 3>(0, 3) = -DR * skew(p2_); // (Sola 16, ex. B.3.2 and Sec. 7.2.3)
- _jacobian1.block<3, 3>(3, 3) = dR.transpose(); // (Sola 16, Sec. 7.2.3)
+ _jacobian1.block<3, 3>(0, 3) = -dR1 * skew(dp2); // (Sola 16, ex. B.3.2 and Sec. 7.2.3)
+ _jacobian1.block<3, 3>(3, 3) = dR2.transpose(); // (Sola 16, Sec. 7.2.3)
_jacobian2.setIdentity();
- _jacobian2.block<3, 3>(0, 0) = DR; // (Sola 16, Sec. 7.2.3)
+ _jacobian2.block<3, 3>(0, 0) = dR1; // (Sola 16, Sec. 7.2.3)
// perform composition here to avoid aliasing problems if _delta1 and _delta_plus_delta share the same storage
- p_out_ = p1_ + q1_ * p2_;
- q_out_ = q1_ * q2_;
+ dp_out = dp1 + dq1 * dp2;
+ dq_out = dq1 * dq2;
}
void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2)
+ Eigen::VectorXs& _delta1_plus_delta2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- remap(_delta1, _delta2, _delta1_plus_delta2);
- p_out_ = p1_ + q1_ * p2_;
- q_out_ = q1_ * q2_;
+
+ Eigen::Map<const Eigen::Vector3s> dp1 (_delta1.data() );
+ Eigen::Map<const Eigen::Quaternions> dq1 (_delta1.data() + 3 );
+ Eigen::Map<const Eigen::Vector3s> dp2 (_delta2.data() );
+ Eigen::Map<const Eigen::Quaternions> dq2 (_delta2.data() + 3 );
+ Eigen::Map<Eigen::Vector3s> dp_out (_delta1_plus_delta2.data() );
+ Eigen::Map<Eigen::Quaternions> dq_out (_delta1_plus_delta2.data() + 3 );
+
+
+ dp_out = dp1 + dq1 * dp2;
+ dq_out = dq1 * dq2;
}
void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt,
- Eigen::VectorXs& _x_plus_delta)
+ Eigen::VectorXs& _x_plus_delta) const
{
assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
- remap(_x.head(x_size_), _delta, _x_plus_delta); //we take only the x_sixe_ first elements of the state Vectors (Position + Orientation)
- p_out_ = p1_ + q1_ * p2_;
- q_out_ = q1_ * q2_;
+
+ Eigen::Map<const Eigen::Vector3s> p (_x.data() );
+ Eigen::Map<const Eigen::Quaternions> q (_x.data() + 3 );
+ Eigen::Map<const Eigen::Vector3s> dp (_delta.data() );
+ Eigen::Map<const Eigen::Quaternions> dq (_delta.data() + 3 );
+ Eigen::Map<Eigen::Vector3s> p_out (_x_plus_delta.data() );
+ Eigen::Map<Eigen::Quaternions> q_out (_x_plus_delta.data() + 3 );
+
+
+ p_out = p + q * dp;
+ q_out = q * dq;
}
-bool ProcessorOdom3D::voteForKeyFrame()
+bool ProcessorOdom3D::voteForKeyFrame() const
{
//WOLF_DEBUG( "Time span : " , getBuffer().get().back().ts_ - getBuffer().get().front().ts_ );
//WOLF_DEBUG( " last ts : ", getBuffer().get().back().ts_);
@@ -219,15 +240,6 @@ FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, Cap
return fac_odom;
}
-void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x2, Eigen::VectorXs& _x_out)
-{
- new (&p1_) Eigen::Map<const Eigen::Vector3s>(_x1.data());
- new (&q1_) Eigen::Map<const Eigen::Quaternions>(_x1.data() + 3);
- new (&p2_) Eigen::Map<const Eigen::Vector3s>(_x2.data());
- new (&q2_) Eigen::Map<const Eigen::Quaternions>(_x2.data() + 3);
- new (&p_out_) Eigen::Map<Eigen::Vector3s>(_x_out.data());
- new (&q_out_) Eigen::Map<Eigen::Quaternions>(_x_out.data() + 3);
-}
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 75659594a9af9cff6da09e9eae9c62ce8d974c83..b18436fd41da8ef3c6f2991bbd2937760ab2eb75 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -302,7 +302,7 @@ void ProcessorTracker::computeProcessingStep()
}
}
-bool ProcessorTracker::triggerInCapture(CaptureBasePtr)
+bool ProcessorTracker::triggerInCapture(CaptureBasePtr) const
{
return true;
}
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index 67c0b74964537f3824f13ef6d8b52050535527b9..6d7dd0f50da30dc79e3c5c9cca980bc3364a0349 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -22,7 +22,7 @@ TrackMatrix::~TrackMatrix()
//
}
-Track TrackMatrix::track(const SizeStd& _track_id)
+Track TrackMatrix::track(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id);
@@ -30,7 +30,7 @@ Track TrackMatrix::track(const SizeStd& _track_id)
return Track();
}
-Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
+Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture) const
{
if (_capture && snapshots_.count(_capture) > 0)
return snapshots_.at(_capture);
@@ -120,17 +120,17 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
}
}
-size_t TrackMatrix::numTracks()
+size_t TrackMatrix::numTracks() const
{
return tracks_.size();
}
-size_t TrackMatrix::trackSize(const SizeStd& _track_id)
+size_t TrackMatrix::trackSize(const SizeStd& _track_id) const
{
return track(_track_id).size();
}
-FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id)
+FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).begin()->second;
@@ -138,7 +138,7 @@ FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id)
return nullptr;
}
-FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id)
+FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).rbegin()->second;
@@ -146,7 +146,7 @@ FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id)
return nullptr;
}
-vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
+vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
{
vector<FeatureBasePtr> vec;
if (tracks_.count(_track_id))
@@ -158,7 +158,7 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
return vec;
}
-std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
+std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap) const
{
std::list<FeatureBasePtr> lst;
if (snapshots_.count(_cap))
@@ -167,7 +167,7 @@ std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
return lst;
}
-TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
+TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const
{
TrackMatches pairs;
@@ -191,7 +191,7 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
return pairs;
}
-FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap)
+FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap) const
{
if (snapshot(_cap).count(_track_id))
return snapshot(_cap).at(_track_id);
@@ -199,12 +199,12 @@ FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _ca
return nullptr;
}
-CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id)
+CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
{
return firstFeature(_track_id)->getCapture();
}
-Track TrackMatrix::trackAtKeyframes(size_t _track_id)
+Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
{
// We assemble a track_kf on the fly by checking each capture's frame.
if (tracks_.count(_track_id))
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index accc54225043d1a4352596f65f4622d8c54464ca..755a5d7e02864db5b180c6e65bb824bedb65e759 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -218,7 +218,7 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
noise_cov_ = _noise_cov;
}
-CaptureBasePtr SensorBase::lastKeyCapture(void)
+CaptureBasePtr SensorBase::lastKeyCapture(void) const
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
CaptureBasePtr capture = nullptr;
@@ -238,7 +238,7 @@ CaptureBasePtr SensorBase::lastKeyCapture(void)
return capture;
}
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
+CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
{
// we search for the most recent Capture of this sensor before _ts
CaptureBasePtr capture = nullptr;
@@ -260,32 +260,32 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
return capture;
}
-StateBlockPtr SensorBase::getP(const TimeStamp _ts)
+StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
{
return getStateBlock(0, _ts);
}
-StateBlockPtr SensorBase::getO(const TimeStamp _ts)
+StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
{
return getStateBlock(1, _ts);
}
-StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
+StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
{
return getStateBlock(2, _ts);
}
-StateBlockPtr SensorBase::getP()
+StateBlockPtr SensorBase::getP() const
{
return getStateBlock(0);
}
-StateBlockPtr SensorBase::getO()
+StateBlockPtr SensorBase::getO() const
{
return getStateBlock(1);
}
-StateBlockPtr SensorBase::getIntrinsic()
+StateBlockPtr SensorBase::getIntrinsic() const
{
return getStateBlock(2);
}
@@ -342,7 +342,7 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
processor_list_.remove(_proc_ptr);
}
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i) const
{
CaptureBasePtr cap;
@@ -352,7 +352,7 @@ StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
return getStateBlockPtrStatic(_i);
}
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
@@ -362,7 +362,7 @@ StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
return getStateBlockPtrStatic(_i);
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
+bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap) const
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
@@ -374,7 +374,7 @@ bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
return false;
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap)
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap) const
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
@@ -386,14 +386,14 @@ bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, Capt
return false;
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i)
+bool SensorBase::isStateBlockDynamic(unsigned int _i) const
{
CaptureBasePtr cap;
return isStateBlockDynamic(_i,cap);
}
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts)
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 35a1f9997e7ab5bffa8f4dbf4f8ac7c44415b66b..10bb5b1ec1b6c7d3a9f800b7e5f443d8b6fed0ac 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -44,7 +44,7 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
updateLastFrames();
}
-void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
+void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
{
for(auto fr_ptr : getFrameList())
fr_ptr->getFactorList(_fac_list);
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index d11dcf09f36616d0379945a0bea78a3ad1769b98..05b2f34494472cfa9fde355b91d0b48a6609d25a 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -42,7 +42,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrLis
return _features_out.size();
}
-bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
+bool ProcessorTrackerFeatureDummy::voteForKeyFrame() const
{
WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() );
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index 4ba333dfb0bb9ee6009a70781f223efccb316ec6..0590d45758707561cfc86b879b1943011e77ed73 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -75,7 +75,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame();
+ virtual bool voteForKeyFrame() const;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index dd1147fdc1a09b60059bc69047adb46ba24ae16f..532906ac27f319486b619fd6f8e0f882d3676197 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -58,7 +58,7 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
return _features_out.size();
}
-bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
+bool ProcessorTrackerLandmarkDummy::voteForKeyFrame() const
{
std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_landmark_dummy_->min_features_for_keyframe;
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index 9977abc480ab2c1cff695b732286cd7857f5276d..106f9ba2a1ea683eb763fc76e7f334e2bf8509b3 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -63,7 +63,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame();
+ virtual bool voteForKeyFrame() const;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 7302ae4833a94a7aeb0ed9ed304b4ea3026b453c..6caa5f868adceb291690590455bea94520771b0f 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -40,7 +40,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib)
+ Eigen::MatrixXs& _jacobian_calib) const
{
Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
}
@@ -48,7 +48,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _dt2,
- Eigen::VectorXs& _delta1_plus_delta2)
+ Eigen::VectorXs& _delta1_plus_delta2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
@@ -58,7 +58,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
const Scalar _dt2,
Eigen::VectorXs& _delta1_plus_delta2,
Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2)
+ Eigen::MatrixXs& _jacobian2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
@@ -66,7 +66,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
virtual void statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta,
const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta)
+ Eigen::VectorXs& _x_plus_delta) const
{
Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
}
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index f697a948afecf8f83b8275220f30497bd6e75130..069f630631f03529da0d7d993f17dacb300f89a2 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -35,7 +35,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
const Scalar _dt,
Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib)
+ Eigen::MatrixXs& _jacobian_calib) const
{
Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
}
@@ -43,7 +43,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
const Eigen::VectorXs& _delta2,
const Scalar _dt2,
- Eigen::VectorXs& _delta1_plus_delta2)
+ Eigen::VectorXs& _delta1_plus_delta2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
@@ -53,7 +53,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
const Scalar _dt2,
Eigen::VectorXs& _delta1_plus_delta2,
Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2)
+ Eigen::MatrixXs& _jacobian2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
@@ -61,7 +61,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
virtual void statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta,
const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta)
+ Eigen::VectorXs& _x_plus_delta) const
{
Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
}