Make many getters const

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
@@ -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,
@@ -271,7 +271,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         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;
 
 };
 

src/problem/problem.cpp

@@ -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;
@@ -1055,7 +1055,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;
@@ -1444,7 +1444,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);