diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h index f937332f15aec0235ff3b9ada7254cffba47461d..d156fd80521b31da81975ea11f55d9949fbb3754 100644 --- a/include/base/ceres_wrapper/ceres_manager.h +++ b/include/base/ceres_wrapper/ceres_manager.h @@ -9,7 +9,7 @@ //wolf includes #include "base/solver/solver_manager.h" #include "base/ceres_wrapper/cost_function_wrapper.h" -#include "local_parametrization_wrapper.h" +#include "base/ceres_wrapper/local_parametrization_wrapper.h" #include "base/ceres_wrapper/create_numeric_diff_cost_function.h" namespace ceres { @@ -48,6 +48,11 @@ class CeresManager : public SolverManager ceres::Solver::Summary getSummary(); + std::unique_ptr<ceres::Problem>& getCeresProblem() + { + return ceres_problem_; + } + virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 08d05aaa19abbfedee7bde87dc9450630ea16fe5..5871b8fa12ee6f9ce5c3aff8741e80444ab9e166 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -259,6 +259,9 @@ void CeresManager::addFactor(const FactorBasePtr& fac_ptr) auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr; + //std::cout << "Added residual block corresponding to constraint " << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl; + + assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals"); assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map"); fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(), @@ -296,11 +299,15 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr) state_ptr->getSize(), local_parametrization_ptr); + if (state_ptr->isFixed()) + ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr)); + updateStateBlockStatus(state_ptr); } void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr) { + //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; assert(state_ptr); ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr)); state_blocks_local_param_.erase(state_ptr);