WIP: Gnss

MR open while Gitflow transition is on.

Set to merge to origin/devel

src/ceres_wrapper/ceres_manager.cpp

@@ -259,7 +259,9 @@ void CeresManager::addConstraint(const ConstraintBasePtr& ctr_ptr)
 
     auto loss_func_ptr = (ctr_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
 
-    assert(ctr_2_residual_idx_.find(ctr_ptr) == ctr_2_residual_idx_.end() && "adding a constraint that is already in the ctr_2_residual_idx_ map");
+    //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()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
 
     ctr_2_residual_idx_[ctr_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
                                                                     loss_func_ptr,
@@ -281,6 +283,7 @@ void CeresManager::removeConstraint(const ConstraintBasePtr& _ctr_ptr)
 
 void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
 {
+    //  std::cout << "CeresManager::addStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
     ceres::LocalParameterization* local_parametrization_ptr = nullptr;
 
     if (state_ptr->hasLocalParametrization() &&
@@ -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);
@@ -309,6 +316,7 @@ void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
 void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
 {
     assert(state_ptr != nullptr);
+    //  std::cout << "CeresManager::updateStateBlockStatus " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
     if (state_ptr->isFixed())
         ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
     else