Skip to content
Snippets Groups Projects
Commit 90953f6b authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Added Ceres_Manager `getCeresProblem()` to ease the creation of the gnss...

Added Ceres_Manager `getCeresProblem()` to ease the creation of the gnss plugin. May need to revert in the future
parent 448e8fbb
No related branches found
No related tags found
No related merge requests found
Pipeline #2852 passed
...@@ -9,7 +9,7 @@ ...@@ -9,7 +9,7 @@
//wolf includes //wolf includes
#include "base/solver/solver_manager.h" #include "base/solver/solver_manager.h"
#include "base/ceres_wrapper/cost_function_wrapper.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" #include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
namespace ceres { namespace ceres {
...@@ -48,6 +48,11 @@ class CeresManager : public SolverManager ...@@ -48,6 +48,11 @@ class CeresManager : public SolverManager
ceres::Solver::Summary getSummary(); ceres::Solver::Summary getSummary();
std::unique_ptr<ceres::Problem>& getCeresProblem()
{
return ceres_problem_;
}
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
......
...@@ -259,6 +259,9 @@ void CeresManager::addFactor(const FactorBasePtr& fac_ptr) ...@@ -259,6 +259,9 @@ void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr; 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"); 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(), fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
...@@ -296,11 +299,15 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr) ...@@ -296,11 +299,15 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
state_ptr->getSize(), state_ptr->getSize(),
local_parametrization_ptr); local_parametrization_ptr);
if (state_ptr->isFixed())
ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
updateStateBlockStatus(state_ptr); updateStateBlockStatus(state_ptr);
} }
void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr) void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
{ {
//std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
assert(state_ptr); assert(state_ptr);
ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr)); ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
state_blocks_local_param_.erase(state_ptr); state_blocks_local_param_.erase(state_ptr);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment