diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 527efd1f60afc7f1f688b795dbe0bf7fd4546628..65e87f56e76718036e44d73031329e98c361afe4 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -31,7 +31,7 @@ struct ProcessorParamsRangeBearing : public ProcessorParamsBase
{
//
}
- std::string print()
+ std::string print() const
{
return "\n" + ProcessorParamsBase::print();
}
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index b7788c27ffb4a06cdbc440ec091553d5b216440a..2e9bcf214d0ea586a6cf3466846ed41ef50f3096 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -29,7 +29,7 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std");
noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std");
}
- std::string print()
+ std::string print() const
{
return "" + IntrinsicsBase::print() + "\n"
+ "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
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 e44f6b6e98fa82390c5e4a344a374f898653edbf..7fa7e38eb1dbad2579222c2dff247b9614322594 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 and capture
@@ -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/common/params_base.h b/include/core/common/params_base.h
index 767ece49d1daa5e55496ba22d71f8cb34c8b8b7d..5295bc0ac17f941503a8f28e462962290a9ee82b 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -13,7 +13,7 @@ namespace wolf {
}
virtual ~ParamsBase() = default;
- std::string print()
+ std::string print() const
{
return "";
}
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/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index e9501f3b71bdb47c23d5cbd9d5f9bd4f243bfb5c..274c226ed779ced623f5ae8c0eb76c0e5bc4d291 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -86,14 +86,6 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
VectorXs residual();
-// /**
-// * \brief Returns the jacobians computation method
-// **/
-// virtual JacobianMethod getJacobianMethod() const
-// {
-// return JAC_AUTO;
-// }
-
protected:
Eigen::Vector3s calib_preint_;
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 03e9b056ae883487e679edfa49f0ecbd69a4d18a..aae0dfb0cda3fc03d13b13b5f6c50cc460301a3c 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -73,7 +73,7 @@ class Problem : public std::enable_shared_from_this<Problem>
std::string getFrameStructure() const;
// Hardware branch ------------------------------------
- HardwareBasePtr getHardware();
+ HardwareBasePtr getHardware() const;
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
@@ -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
*
@@ -155,7 +155,7 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectory();
+ TrajectoryBasePtr getTrajectory() const;
virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
const Eigen::MatrixXs& _prior_cov, //
const TimeStamp& _ts,
@@ -261,17 +261,17 @@ 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 -----------------------------------------
- MapBasePtr getMap();
+ MapBasePtr getMap() const;
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
@@ -280,13 +280,13 @@ class Problem : public std::enable_shared_from_this<Problem>
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov);
void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov);
- bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0, const int _col=0);
- bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
- bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
- bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
- bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
- bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& _covariance);
- bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
+ bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0, const int _col=0) const;
+ bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov) const;
+ bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0) const;
+ bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance) const;
+ bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance) const;
+ bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& _covariance) const;
+ bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) const;
// Solver management ----------------------------------------
@@ -335,12 +335,12 @@ class Problem : public std::enable_shared_from_this<Problem>
void print(int depth = 4, //
bool constr_by = false, //
bool metric = true, //
- bool state_blocks = false);
+ bool state_blocks = false) const;
void print(const std::string& depth, //
bool constr_by = false, //
bool metric = true, //
- bool state_blocks = false);
- bool check(int verbose_level = 0);
+ bool state_blocks = false) const;
+ bool check(int verbose_level = 0) const;
};
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 1eee919dea9af66f6bcaf2257c0e7ae58d5ba81b..c416ed976e01b3d072c5520a2c31a4e76b7f291b 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -180,7 +180,7 @@ class BufferPackKeyFrame : public Buffer<PackKeyFramePtr>
/**\brief Print buffer information
*
*/
- void print();
+ void print() const;
/**\brief Alias funct
*
@@ -224,7 +224,7 @@ struct ProcessorParamsBase : public ParamsBase
bool voting_aux_active = false; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
- std::string print()
+ std::string print() const
{
return ParamsBase::print() + "\n"
+ "voting_active: " + std::to_string(voting_active) + "\n"
@@ -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_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 7ce522cd32168dcde5cd28b2552c4718ed190afc..6c8bf6a34605dd5da4c9d4766f619b3c7cbc6941 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -22,7 +22,7 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsOdom2D
ProcessorParamsOdom2D(_unique_name, _server)
{
}
- std::string print()
+ std::string print() const
{
return "\n" + ProcessorParamsOdom2D::print();
}
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index f3dfae1a7c69fc2c12e295cd35a22afd6bdbf39e..9bc5027fd8e8cbcc0729aed9b897765b21862ec7 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -40,7 +40,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
angle_turned = _server.getParam<Scalar>(_unique_name + "/keyframe_vote/angle_turned");
unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std");
}
- std::string print()
+ std::string print() const
{
return "\n" + ProcessorParamsBase::print() + "\n"
+ "max_time_span: " + std::to_string(max_time_span) + "\n"
@@ -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_odom_2D.h b/include/core/processor/processor_odom_2D.h
index de27f227ba3f89b7c8cf4472a88050aa183c6540..a4f6e3358fc0f71745a3df1ce15cf48cc880a7f6 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -30,7 +30,7 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
cov_det = _server.getParam<Scalar>(_unique_name + "/keyframe_vote/cov_det");
}
- std::string print()
+ std::string print() const
{
return "\n" + ProcessorParamsMotion::print() + "\n"
+ "cov_det: " + std::to_string(cov_det) + "\n";
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index c08d8bffd35100597b3a7ae960f4452d97d09069..22f4903c209d11bf31f0e08f302c0ce7a6d8d77f 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -27,7 +27,7 @@ struct ProcessorParamsOdom3D : public ProcessorParamsMotion
{
//
}
- std::string print()
+ std::string print() const
{
return "\n" + ProcessorParamsMotion::print();
}
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index dc59acee329e3dbe09ba03e25cb1c385347bf8ff..280252a64942995f964e5bc87ec4248da774434d 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -27,7 +27,7 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe");
max_new_features = _server.getParam<int>(_unique_name + "/max_new_features");
}
- std::string print()
+ std::string print() const
{
return ProcessorParamsBase::print() + "\n"
+ "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
@@ -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
@@ -230,7 +230,7 @@ class ProcessorTracker : public ProcessorBase
FeatureBasePtrList& getNewFeaturesListLast();
- std::string print(){
+ std::string print() const {
return this->params_tracker_->print();
}
@@ -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 e9945a9b4ae269b0829651ebf85ed331f1dcaec1..012fd71348ae2e3946330486fc856bbb7853726b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -74,7 +74,7 @@ struct IntrinsicsBase: public ParamsBase
{
virtual ~IntrinsicsBase() = default;
using ParamsBase::ParamsBase;
- std::string print()
+ std::string print() const
{
return "";
}
@@ -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/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 8c6de5fed471b7ec6648b01c41b2e537e9aefe3a..47b0b7886ee9a123e39a5b8cb90576d8a2562ea6 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -22,8 +22,6 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
Scalar wheel_separation;
Scalar ticks_per_wheel_revolution;
- Scalar radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM.
-
IntrinsicsDiffDrive() = default;
IntrinsicsDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
@@ -33,16 +31,14 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
radius_right = _server.getParam<Scalar>(_unique_name + "/radius_right");
wheel_separation = _server.getParam<Scalar>(_unique_name + "/wheel_separation");
ticks_per_wheel_revolution = _server.getParam<Scalar>(_unique_name + "/ticks_per_wheel_revolution");
- radians_per_tick = 2.0 * M_PI / ticks_per_wheel_revolution;
}
- std::string print()
+ std::string print() const
{
return "\n" + IntrinsicsBase::print() + "\n"
+ "radius_left: " + std::to_string(radius_left) + "\n"
+ "radius_right: " + std::to_string(radius_right) + "\n"
+ "wheel_separation: " + std::to_string(wheel_separation) + "\n"
- + "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n"
- + "radians_per_tick: " + std::to_string(radians_per_tick) + "\n";
+ + "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n";
}
};
@@ -58,8 +54,15 @@ class SensorDiffDrive : public SensorBase
virtual ~SensorDiffDrive();
IntrinsicsDiffDriveConstPtr getParams() const;
- private:
+ Scalar getRadiansPerTick() const
+ {
+ return radians_per_tick;
+ }
+
+ protected:
IntrinsicsDiffDrivePtr params_diff_drive_;
+ Scalar radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM.
+
};
inline wolf::IntrinsicsDiffDriveConstPtr SensorDiffDrive::getParams() const
diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h
index 83b776add9b4bba4c1994f047d06dedd3e8e5c1f..463a9c3d95053f3753c3b59b2769cfb669b9a0e7 100644
--- a/include/core/sensor/sensor_odom_2D.h
+++ b/include/core/sensor/sensor_odom_2D.h
@@ -25,7 +25,7 @@ struct IntrinsicsOdom2D : public IntrinsicsBase
k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
}
- std::string print()
+ std::string print() const
{
return "\n" + IntrinsicsBase::print() + "\n"
+ "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h
index 1a727d613d8564c02d02f16abcf63410c96289b2..3e141b93916455d280990d2aed3d27d54d156835 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3D.h
@@ -36,7 +36,7 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var");
}
- std::string print()
+ std::string print() const
{
return "\n" + IntrinsicsBase::print() + "\n"
+ "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
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 cbc19727e70d352bc996c17a210f34cd280f423e..a2330905ad8be9ff5f871f8e5fc0c11a01b9ccde 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 794fbc19665d3dc2f4c92eb550d516a2ca72a39d..49a9fab6bf8450592dd78e8b6ac69fd4bfe7a0a3 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());
@@ -547,7 +547,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _
}
bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row,
- const int _col)
+ const int _col) const
{
//std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
//std::cout << "_row " << _row << std::endl;
@@ -566,10 +566,10 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
+ covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2));
else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
- covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose();
+ covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)).transpose();
else
{
WOLF_DEBUG("Could not find the requested covariance block.");
@@ -579,7 +579,7 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
return true;
}
-bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
+bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov) const
{
std::lock_guard<std::mutex> lock(mut_covariances_);
@@ -600,8 +600,8 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_12];
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_12].transpose();
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_12);
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_12).transpose();
}
else if (covariances_.find(pair_21) != covariances_.end())
{
@@ -610,8 +610,8 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_21].transpose();
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_21];
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_21).transpose();
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_21);
}
else
return false;
@@ -620,12 +620,12 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
return true;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col)
+bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col) const
{
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
-bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance)
+bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance) const
{
bool success(true);
int i = 0, j = 0;
@@ -661,19 +661,19 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
return success;
}
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
+bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov) const
{
FrameBasePtr frm = getLastKeyFrame();
return getFrameCovariance(frm, cov);
}
-bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov)
+bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov) const
{
FrameBasePtr frm = getLastKeyOrAuxFrame();
return getFrameCovariance(frm, cov);
}
-bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
+bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) const
{
bool success(true);
int i = 0, j = 0;
@@ -710,17 +710,17 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
return success;
}
-MapBasePtr Problem::getMap()
+MapBasePtr Problem::getMap() const
{
return map_ptr_;
}
-TrajectoryBasePtr Problem::getTrajectory()
+TrajectoryBasePtr Problem::getTrajectory() const
{
return trajectory_ptr_;
}
-HardwareBasePtr Problem::getHardware()
+HardwareBasePtr Problem::getHardware() const
{
return hardware_ptr_;
}
@@ -801,7 +801,7 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
+void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
{
using std::cout;
using std::endl;
@@ -1059,7 +1059,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << endl;
}
-bool Problem::check(int verbose_level)
+bool Problem::check(int verbose_level) const
{
using std::cout;
using std::endl;
@@ -1448,7 +1448,7 @@ bool Problem::check(int verbose_level)
return is_consistent;
}
-void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks)
+void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
{
if (depth.compare("T") == 0)
print(0, constr_by, metric, state_blocks);
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 1e8b098df1dd882ff16756c3f3345080637a43e0..c89ece40f29e39d36d167cf79c4bf852348f69b6 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -181,7 +181,7 @@ PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const CaptureBasePtr _
return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
}
-void BufferPackKeyFrame::print(void)
+void BufferPackKeyFrame::print(void) const
{
std::cout << "[ ";
for (auto iter : container_)
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index d0a776cc8c21e2ddb26b8be7f25d9e053ff8411e..d4605c6f0ab27ef247dd64885de922eece4b0fc6 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -38,7 +38,7 @@ void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
- radians_per_tick_ = sensor_diff_drive->getParams()->radians_per_tick;
+ radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 4922bd3bf9de928232781903919435d497f112ea..c2821646896d2573f7bcaa1ed828f63c0d7813d2 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -56,9 +56,9 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
else
{
// rotation in quaternion form
- _delta = _data;
- disp = _data.head<3>().norm();
- rot = 2 * acos(_data(3));
+ _delta = _data;
+ disp = _data.head<3>().norm();
+ rot = 2.0 * acos(_data(6)); // '6' is the real part of the quaternion
}
/* Jacobians of d = data2delta(data, dt)
* with: d = [Dp Dq]
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 2eb2c10403b727d94ae497b65f4e72941de876d0..d19ab8aa2e4d2ba5f9146bbdad2d45e5d855afc2 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -20,7 +20,7 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
2),
params_diff_drive_(_intrinsics)
{
- params_diff_drive_->radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
+ radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
getIntrinsic()->setState(Eigen::Vector3s(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
getIntrinsic()->unfix();
}
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 5d296342191c30f1632daa923677057e5b26daa3..b4647a0a83947463c8c5f8e9a975823c2f4f9fdb 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -8,14 +8,16 @@
#include "core/utils/utils_gtest.h"
// Classes under test
+#include "core/sensor/sensor_odom_2D.h"
#include "core/processor/processor_odom_2D.h"
#include "core/factor/factor_odom_2D.h"
+#include "core/capture/capture_odom_2D.h"
// Wolf includes
-#include "core/sensor/sensor_odom_2D.h"
#include "core/state_block/state_block.h"
#include "core/common/wolf.h"
#include "core/ceres_wrapper/ceres_manager.h"
+#include "core/capture/capture_pose.h"
// STL includes
#include <map>
@@ -26,7 +28,6 @@
// General includes
#include <iostream>
#include <iomanip> // std::setprecision
-#include "core/capture/capture_pose.h"
using namespace wolf;
using namespace Eigen;
@@ -234,6 +235,9 @@ TEST(Odom2D, VoteForKfAndSolve)
std::vector<Eigen::VectorXs> integrated_pose_vector;
std::vector<Eigen::MatrixXs> integrated_cov_vector;
+ integrated_pose_vector.push_back(integrated_pose);
+ integrated_cov_vector.push_back(integrated_cov);
+
// std::cout << "\nIntegrating data..." << std::endl;
t += dt;
@@ -249,10 +253,7 @@ TEST(Odom2D, VoteForKfAndSolve)
// Processor
capture->process();
ASSERT_TRUE(problem->check(0));
-// Matrix3s odom2d_delta_cov = processor_odom2d->integrateBufferCovariance(processor_odom2d->getBuffer());
Matrix3s odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_;
- // std::cout << "State(" << (t - t0) << ") : " << processor_odom2d->getCurrentState().transpose() << std::endl;
- // std::cout << "PRC cov: \n" << odom2d_delta_cov << std::endl;
// Integrate Delta
if (n==3 || n==6) // keyframes
@@ -291,12 +292,21 @@ TEST(Odom2D, VoteForKfAndSolve)
// Solve
std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
-// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+
+ // Check the 3 KFs' states and covariances
MatrixXs computed_cov;
- ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
- ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6);
+ int n = 0;
+ for (auto F : problem->getTrajectory()->getFrameList())
+ {
+ if (!F->isKey()) break;
+
+ ASSERT_POSE2D_APPROX(F->getState(), integrated_pose_vector[n] , 1e-6);
+ ASSERT_TRUE (F->getCovariance(computed_cov));
+ ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
+ n += 3;
+ }
}
TEST(Odom2D, KF_callback)
@@ -314,7 +324,7 @@ TEST(Odom2D, KF_callback)
Eigen::Matrix3s x0_cov = Eigen::Matrix3s::Identity() * 0.1;
VectorXs data(Vector2s(1, M_PI/4) ); // advance 1m
Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
- int N = 16; // number of process() steps
+ int N = 8; // number of process() steps
// NOTE: We integrate and create KFs as follows:
// n = 0 1 2 3 4 5 6 7 8
@@ -361,7 +371,7 @@ TEST(Odom2D, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data
- CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
+ auto capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
std::cout << "t: " << t << std::endl;
for (int n=1; n<=N; n++)
@@ -405,11 +415,16 @@ TEST(Odom2D, KF_callback)
Vector3s x_split = processor_odom2d->getState(t_split);
FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
+ // there must be 2KF and one F
+ ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3);
+ // The last KF must have TS = 0.08
+ ASSERT_EQ(problem->getLastKeyFrame()->getTimeStamp().getNanoSeconds(), 80000000);
+
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
ASSERT_TRUE(problem->check(0));
t += dt;
- capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
+ capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));
@@ -441,7 +456,7 @@ TEST(Odom2D, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
ASSERT_TRUE(problem->check(0));
t += dt;
- capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
+ capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));
@@ -485,7 +500,7 @@ TEST(Odom2D, KF_callback)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
-// testing::GTEST_FLAG(filter) = "Odom2D.KF_callback";
+// testing::GTEST_FLAG(filter) = "Odom2D.VoteForKfAndSolve";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index e585e304dabd1aa4371630c65220ec6325112a7a..2d758cc597e05ad1b7052651d3850ae1d68e0501 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -224,7 +224,6 @@ TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
{
-
Vector2s data;
Matrix2s data_cov; data_cov . setIdentity();
Vector3s calib(1,1,1);
@@ -265,7 +264,6 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
TEST_F(ProcessorDiffDriveTest, statePlusDelta)
{
-
Vector2s data;
Matrix2s data_cov; data_cov . setIdentity();
Vector3s calib(1,1,1);
@@ -306,7 +304,6 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
TEST_F(ProcessorDiffDriveTest, process)
{
-
Vector2s data;
Matrix2s data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
@@ -336,6 +333,61 @@ TEST_F(ProcessorDiffDriveTest, process)
problem->print(4,1,1,1);
}
+TEST_F(ProcessorDiffDriveTest, linear)
+{
+ Vector2s data;
+ Matrix2s data_cov; data_cov . setIdentity();
+ TimeStamp t = 0.0;
+ Vector3s x(0,0,0);
+ Matrix3s P; P.setIdentity();
+
+ auto F0 = problem->setPrior(x, P, t, 0.1);
+
+ // Straight one turn of the wheels, in one go
+ data(0) = 100.0 ; // one turn of the wheels
+ data(1) = 100.0 ;
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+ C->process();
+ WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+
+ // radius is 1.0m, 100 ticks per revolution, so advanced distance is
+ Scalar distance = 2 * M_PI * 1.0;
+
+ ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,0), 1e-6)
+}
+
+TEST_F(ProcessorDiffDriveTest, angular)
+{
+ Vector2s data;
+ Matrix2s data_cov; data_cov . setIdentity();
+ TimeStamp t = 0.0;
+ Vector3s x(0,0,0);
+ Matrix3s P; P.setIdentity();
+
+ auto F0 = problem->setPrior(x, P, t, 0.1);
+
+ // Straight one turn of the wheels, in one go
+ data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
+ data(1) = 20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+ C->process();
+ WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+
+ // this is a turn in place, so distance = 0;
+ Scalar distance = 0.0;
+
+ // radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is
+ Scalar angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5);
+
+ ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,angle), 1e-6)
+}
+
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 29ccea249acb27a6b0ac3bbbfb88b7bc0516d05c..363847f6c30ea0bcb36fce264f7aabcfe36fc298 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -30,7 +30,6 @@ TEST(DiffDrive, constructor)
TEST(DiffDrive, getParams)
{
auto intr = std::make_shared<IntrinsicsDiffDrive>();
- intr->radians_per_tick = 1;
intr->radius_left = 2;
intr->radius_right = 3;
intr->ticks_per_wheel_revolution = 4;
@@ -42,7 +41,7 @@ TEST(DiffDrive, getParams)
ASSERT_NE(sen->getParams(), nullptr);
- ASSERT_EQ(sen->getParams()->radians_per_tick, 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
+ ASSERT_EQ(sen->getRadiansPerTick(), 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
ASSERT_EQ(sen->getParams()->radius_left, 2);
ASSERT_EQ(sen->getParams()->radius_right, 3);
ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);
@@ -52,7 +51,6 @@ TEST(DiffDrive, getParams)
TEST(DiffDrive, create)
{
auto intr = std::make_shared<IntrinsicsDiffDrive>();
- intr->radians_per_tick = 1;
intr->radius_left = 2;
intr->radius_right = 3;
intr->ticks_per_wheel_revolution = 4;
@@ -66,7 +64,7 @@ TEST(DiffDrive, create)
ASSERT_NE(sen->getParams(), nullptr);
- ASSERT_EQ(sen->getParams()->radians_per_tick, 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
+ ASSERT_EQ(sen->getRadiansPerTick(), 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
ASSERT_EQ(sen->getParams()->radius_left, 2);
ASSERT_EQ(sen->getParams()->radius_right, 3);
ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);