diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h index 03b1e1cb895a46d4235500f7fb5c7c27fa2acd0a..413ecce13ebd2bb179134e255fe1d1af228efe13 100644 --- a/include/core/ceres_wrapper/ceres_manager.h +++ b/include/core/ceres_wrapper/ceres_manager.h @@ -90,7 +90,7 @@ class CeresManager : public SolverManager ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr); - virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) override; + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override; virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override; }; @@ -105,7 +105,7 @@ inline ceres::Solver::Options& CeresManager::getSolverOptions() return ceres_options_; } -inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) +inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const { return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end(); diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index 049cb951d07c5e1c0b70cdf9be9a1d9dea992261..f5ed62680e815ec182a9a0db28efe1c3894c6090 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -69,7 +69,7 @@ public: virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr); - virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr); + virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const; protected: @@ -94,7 +94,7 @@ protected: virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0; - virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) = 0; + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0; }; } // namespace wolf diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index f6d2914da4ff77e594ee0b2722627897f6fb23e5..e03be2d6ce6c5a7dac729efeef040a4d312ae11e 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -165,7 +165,7 @@ bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) return state_blocks_.find(state_ptr) != state_blocks_.end() && isStateBlockRegisteredDerived(state_ptr); } -bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) +bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const { return isFactorRegisteredDerived(fac_ptr); } diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 29f4f132dc646705cca3c633798a8785c2494fea..d05b40a11f11a30c8e29a346e28339b7ff99124d 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -46,9 +46,9 @@ public: virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){}; virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){}; virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;}; - virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;}; + virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;}; virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;}; - virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;}; + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;}; }; TEST(Problem, create) diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 496dab16c21dc0de603c0b2169d47a89a85514ed..92fc07b84ba8228a4c1b3ef90790a35dd28405b1 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -45,7 +45,7 @@ class SolverManagerWrapper : public SolverManager return state_block_fixed_.at(st); }; - bool isFactorRegistered(const FactorBasePtr& fac_ptr) + bool isFactorRegistered(const FactorBasePtr& fac_ptr) const { return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end(); }; @@ -63,7 +63,7 @@ class SolverManagerWrapper : public SolverManager virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){}; virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){}; virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;}; - virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;}; + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;}; // The following are dummy implementations bool hasConverged() { return true; } diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index 50a78bc0f7b4705a9e17ddf1429ad6ee81aafe48..f71bc579b49ced7d6e389d917c690eed06e00566 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -38,9 +38,9 @@ struct DummySolverManager : public SolverManager virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){}; virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){}; virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;}; - virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;}; + virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;}; virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;}; - virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;}; + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;}; }; /// Set to true if you want debug info