diff --git a/src/ceres_wrapper/cost_function_wrapper.h b/src/ceres_wrapper/cost_function_wrapper.h index 65566f2fa314c6a37024e8335990210f347cd437..ffd88191475c23ae409651baedb0099f3aaa4064 100644 --- a/src/ceres_wrapper/cost_function_wrapper.h +++ b/src/ceres_wrapper/cost_function_wrapper.h @@ -17,14 +17,19 @@ WOLF_PTR_TYPEDEFS(CostFunctionWrapper); class CostFunctionWrapper : public ceres::CostFunction { - public: + private: + ConstraintBasePtr constraint_ptr_; + public: + CostFunctionWrapper(ConstraintBasePtr _constraint_ptr); virtual ~CostFunctionWrapper(); virtual bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const; + + ConstraintBasePtr getConstraintPtr() const; }; inline CostFunctionWrapper::CostFunctionWrapper(ConstraintBasePtr _constraint_ptr) : @@ -44,6 +49,11 @@ inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, doub return constraint_ptr_->evaluate(parameters, residuals, jacobians); } +inline ConstraintBasePtr CostFunctionWrapper::getConstraintPtr() const +{ + return constraint_ptr_; +} + } // namespace wolf diff --git a/src/ceres_wrapper/local_parametrization_wrapper.h b/src/ceres_wrapper/local_parametrization_wrapper.h index 1d81970a963c374cf91a5b283cf443774cb05729..d9e0d9e82671ceb232e47760e2f74a119304ff3e 100644 --- a/src/ceres_wrapper/local_parametrization_wrapper.h +++ b/src/ceres_wrapper/local_parametrization_wrapper.h @@ -30,6 +30,8 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization virtual int GlobalSize() const; virtual int LocalSize() const; + + LocalParametrizationBasePtr getLocalParametrizationPtr() const; }; using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>; @@ -55,6 +57,11 @@ inline int LocalParametrizationWrapper::LocalSize() const return local_parametrization_ptr_->getLocalSize(); } +inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametrizationPtr() const +{ + return local_parametrization_ptr_; +} + } // namespace wolf #endif