From 8ca57aef38ea09676ebb0fd6730dc8d500a5c1b3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 29 Nov 2018 12:04:37 +0100
Subject: [PATCH] bug fixed, my fault

---
 src/ceres_wrapper/ceres_manager.cpp       | 472 +++++++++++-----------
 src/ceres_wrapper/ceres_manager.h         |  58 +--
 src/ceres_wrapper/cost_function_wrapper.h |   4 +-
 src/solver/solver_manager.cpp             | 252 ++++++------
 4 files changed, 396 insertions(+), 390 deletions(-)

diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 01e0b7782..c206e657c 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -9,317 +9,323 @@ namespace wolf {
 
 CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
                            const ceres::Solver::Options& _ceres_options) :
-  SolverManager(_wolf_problem),
-  ceres_options_(_ceres_options)
+    SolverManager(_wolf_problem),
+    ceres_options_(_ceres_options)
 {
-  ceres::Covariance::Options covariance_options;
+    ceres::Covariance::Options covariance_options;
 #if CERES_VERSION_MINOR >= 13
-  covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
-  covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
+    covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+    covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
 #elif CERES_VERSION_MINOR >= 10
-  covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
+    covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
 #else
-  covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+    covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
 #endif
-  covariance_options.num_threads = 1;
-  covariance_options.apply_loss_function = false;
-  //covariance_options.null_space_rank = -1;
-  covariance_ = wolf::make_unique<ceres::Covariance>(covariance_options);
+    covariance_options.num_threads = 1;
+    covariance_options.apply_loss_function = false;
+    //covariance_options.null_space_rank = -1;
+    covariance_ = wolf::make_unique<ceres::Covariance>(covariance_options);
 
-  ceres::Problem::Options problem_options;
-  problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-  problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
-  problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+    ceres::Problem::Options problem_options;
+    problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+    problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
+    problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
 
-  ceres_problem_ = wolf::make_unique<ceres::Problem>(problem_options);
+    ceres_problem_ = wolf::make_unique<ceres::Problem>(problem_options);
 }
 
 CeresManager::~CeresManager()
 {
-  while (!ctr_2_residual_idx_.empty())
-    removeConstraint(ctr_2_residual_idx_.begin()->first);
+    while (!ctr_2_residual_idx_.empty())
+        removeConstraint(ctr_2_residual_idx_.begin()->first);
 }
 
 std::string CeresManager::solveImpl(const ReportVerbosity report_level)
 {
-  // run Ceres Solver
-  ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_);
+    // Check
+    #ifdef _WOLF_DEBUG
+        check();
+    #endif
 
-  std::string report;
+    // run Ceres Solver
+    ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_);
 
-  //return report
-  if (report_level == ReportVerbosity::BRIEF)
-    report = summary_.BriefReport();
-  else if (report_level == ReportVerbosity::FULL)
-    report = summary_.FullReport();
+    std::string report;
 
-  return report;
+    //return report
+    if (report_level == ReportVerbosity::BRIEF)
+        report = summary_.BriefReport();
+    else if (report_level == ReportVerbosity::FULL)
+        report = summary_.FullReport();
+
+    return report;
 }
 
 void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
 {
-  // update problem
-  update();
-
-  // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
-  wolf_problem_->clearCovariance();
-
-  // CREATE DESIRED COVARIANCES LIST
-  std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
-  std::vector<std::pair<const double*, const double*>> double_pairs;
-
-  switch (_blocks)
-  {
-  case CovarianceBlocksToBeComputed::ALL:
-  {
-    // first create a vector containing all state blocks
-    std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
-    //frame state blocks
-    for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
-      if (fr_ptr->isKey())
-        for (auto sb : fr_ptr->getStateBlockVec())
-          if (sb)
-            all_state_blocks.push_back(sb);
-
-    // landmark state blocks
-    for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+    // update problem
+    update();
+
+    // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+    wolf_problem_->clearCovariance();
+
+    // CREATE DESIRED COVARIANCES LIST
+    std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
+    std::vector<std::pair<const double*, const double*>> double_pairs;
+
+    switch (_blocks)
     {
-      landmark_state_blocks = l_ptr->getUsedStateBlockVec();
-      all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
-    }
-    // double loop all against all (without repetitions)
-    for (unsigned int i = 0; i < all_state_blocks.size(); i++)
-      for  (unsigned int j = i; j < all_state_blocks.size(); j++)
-      {
-        state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
-        double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
-                                  getAssociatedMemBlockPtr(all_state_blocks[j]));
-      }
-    break;
-  }
-  case CovarianceBlocksToBeComputed::ALL_MARGINALS:
-  {
-    // first create a vector containing all state blocks
-    for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
-      if (fr_ptr->isKey())
-        for (auto sb : fr_ptr->getStateBlockVec())
-          if (sb)
-            for(auto sb2 : fr_ptr->getStateBlockVec())
-              if (sb)
-              {
-                state_block_pairs.emplace_back(sb, sb2);
-                double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
-                if (sb == sb2) break;
-              }
-
-    // landmark state blocks
-    for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
-      for (auto sb : l_ptr->getUsedStateBlockVec())
-        for(auto sb2 : l_ptr->getUsedStateBlockVec())
+        case CovarianceBlocksToBeComputed::ALL:
         {
-          state_block_pairs.emplace_back(sb, sb2);
-          double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
-          if (sb == sb2) break;
+            // first create a vector containing all state blocks
+            std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
+            //frame state blocks
+            for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+                if (fr_ptr->isKey())
+                    for (auto sb : fr_ptr->getStateBlockVec())
+                        if (sb)
+                            all_state_blocks.push_back(sb);
+
+            // landmark state blocks
+            for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+            {
+                landmark_state_blocks = l_ptr->getUsedStateBlockVec();
+                all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
+            }
+            // double loop all against all (without repetitions)
+            for (unsigned int i = 0; i < all_state_blocks.size(); i++)
+                for  (unsigned int j = i; j < all_state_blocks.size(); j++)
+                {
+                    state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
+                    double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
+                                              getAssociatedMemBlockPtr(all_state_blocks[j]));
+                }
+            break;
         }
-    //            // loop all marginals (PO marginals)
-    //            for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
-    //            {
-    //                state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
-    //                state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
-    //                state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
-    //
-    //                double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
-    //                double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
-    //                double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
-    //            }
-    break;
-  }
-  case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
-  {
-    //robot-robot
-    auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
-
-    state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
-    state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
-    state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
-
-    double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
-                              getAssociatedMemBlockPtr(last_key_frame->getPPtr()));
-    double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
-                              getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
-    double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
-                              getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
-
-    // landmarks
-    std::vector<StateBlockPtr> landmark_state_blocks;
-    for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
-    {
-      // load state blocks vector
-      landmark_state_blocks = l_ptr->getUsedStateBlockVec();
-
-      for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
-      {
-        // robot - landmark
-        state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
-        state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
-        double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
-                                  getAssociatedMemBlockPtr((*state_it)));
-        double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
-                                  getAssociatedMemBlockPtr((*state_it)));
-
-        // landmark marginal
-        for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
+        case CovarianceBlocksToBeComputed::ALL_MARGINALS:
         {
-          state_block_pairs.emplace_back(*state_it, *next_state_it);
-          double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
-                                    getAssociatedMemBlockPtr((*next_state_it)));
+            // first create a vector containing all state blocks
+            for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+                if (fr_ptr->isKey())
+                    for (auto sb : fr_ptr->getStateBlockVec())
+                        if (sb)
+                            for(auto sb2 : fr_ptr->getStateBlockVec())
+                                if (sb)
+                                {
+                                    state_block_pairs.emplace_back(sb, sb2);
+                                    double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+                                    if (sb == sb2)
+                                        break;
+                                }
+
+            // landmark state blocks
+            for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+                for (auto sb : l_ptr->getUsedStateBlockVec())
+                    for(auto sb2 : l_ptr->getUsedStateBlockVec())
+                    {
+                        state_block_pairs.emplace_back(sb, sb2);
+                        double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+                        if (sb == sb2)
+                            break;
+                    }
+            //            // loop all marginals (PO marginals)
+            //            for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
+            //            {
+            //                state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
+            //                state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
+            //                state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
+            //
+            //                double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
+            //                double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
+            //                double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
+            //            }
+            break;
+        }
+        case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
+        {
+            //robot-robot
+            auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
+
+            state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
+            state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
+            state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
+
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+                                      getAssociatedMemBlockPtr(last_key_frame->getPPtr()));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+                                      getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
+                                      getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
+
+            // landmarks
+            std::vector<StateBlockPtr> landmark_state_blocks;
+            for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+            {
+                // load state blocks vector
+                landmark_state_blocks = l_ptr->getUsedStateBlockVec();
+
+                for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
+                {
+                    // robot - landmark
+                    state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
+                    state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
+                    double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+                                              getAssociatedMemBlockPtr((*state_it)));
+                    double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
+                                              getAssociatedMemBlockPtr((*state_it)));
+
+                    // landmark marginal
+                    for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
+                    {
+                      state_block_pairs.emplace_back(*state_it, *next_state_it);
+                      double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
+                                                getAssociatedMemBlockPtr((*next_state_it)));
+                    }
+                }
+            }
+            break;
         }
-      }
     }
-    break;
-  }
-  }
-  //std::cout << "pairs... " << double_pairs.size() << std::endl;
-
-  // COMPUTE DESIRED COVARIANCES
-  if (covariance_->Compute(double_pairs, ceres_problem_.get()))
-  {
-    // STORE DESIRED COVARIANCES
-    for (unsigned int i = 0; i < double_pairs.size(); i++)
+    //std::cout << "pairs... " << double_pairs.size() << std::endl;
+
+    // COMPUTE DESIRED COVARIANCES
+    if (covariance_->Compute(double_pairs, ceres_problem_.get()))
     {
-      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
-      covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
-      wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
-      //std::cout << "getted covariance " << std::endl << cov << std::endl;
+        // STORE DESIRED COVARIANCES
+        for (unsigned int i = 0; i < double_pairs.size(); i++)
+        {
+            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
+            covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+            wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
+            //std::cout << "getted covariance " << std::endl << cov << std::endl;
+        }
     }
-  }
-  else
-    std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+    else
+        std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
 }
 
 void CeresManager::computeCovariances(const StateBlockList& st_list)
 {
-  //std::cout << "CeresManager: computing covariances..." << std::endl;
+    //std::cout << "CeresManager: computing covariances..." << std::endl;
 
-  // update problem
-  update();
+    // update problem
+    update();
 
-  // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
-  wolf_problem_->clearCovariance();
+    // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+    wolf_problem_->clearCovariance();
 
-  // CREATE DESIRED COVARIANCES LIST
-  std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
-  std::vector<std::pair<const double*, const double*>> double_pairs;
+    // CREATE DESIRED COVARIANCES LIST
+    std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
+    std::vector<std::pair<const double*, const double*>> double_pairs;
 
-  // double loop all against all (without repetitions)
-  for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
-    for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
-    {
-      state_block_pairs.emplace_back(*st_it1, *st_it2);
-      double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
-                                getAssociatedMemBlockPtr((*st_it2)));
-    }
+    // double loop all against all (without repetitions)
+    for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
+        for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
+        {
+            state_block_pairs.emplace_back(*st_it1, *st_it2);
+            double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
+                                      getAssociatedMemBlockPtr((*st_it2)));
+        }
 
-  //std::cout << "pairs... " << double_pairs.size() << std::endl;
+    //std::cout << "pairs... " << double_pairs.size() << std::endl;
 
-  // COMPUTE DESIRED COVARIANCES
-  if (covariance_->Compute(double_pairs, ceres_problem_.get()))
-    // STORE DESIRED COVARIANCES
-    for (unsigned int i = 0; i < double_pairs.size(); i++)
-    {
-      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
-      covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
-      wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
-      //std::cout << "getted covariance " << std::endl << cov << std::endl;
-    }
-  else
-    std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+    // COMPUTE DESIRED COVARIANCES
+    if (covariance_->Compute(double_pairs, ceres_problem_.get()))
+        // STORE DESIRED COVARIANCES
+        for (unsigned int i = 0; i < double_pairs.size(); i++)
+        {
+            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
+            covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+            wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
+            //std::cout << "getted covariance " << std::endl << cov << std::endl;
+        }
+    else
+        std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
 }
 
 void CeresManager::addConstraint(const ConstraintBasePtr& ctr_ptr)
 {
-  assert(ctr_2_costfunction_.find(ctr_ptr) == ctr_2_costfunction_.end() && "adding a constraint that is already in the ctr_2_costfunction_ map");
+    assert(ctr_2_costfunction_.find(ctr_ptr) == ctr_2_costfunction_.end() && "adding a constraint that is already in the ctr_2_costfunction_ map");
 
-  auto cost_func_ptr = createCostFunction(ctr_ptr);
-  ctr_2_costfunction_[ctr_ptr] = cost_func_ptr;
+    auto cost_func_ptr = createCostFunction(ctr_ptr);
+    ctr_2_costfunction_[ctr_ptr] = cost_func_ptr;
 
-  std::vector<Scalar*> res_block_mem;
-  res_block_mem.reserve(ctr_ptr->getStateBlockPtrVector().size());
-  for (const StateBlockPtr& st : ctr_ptr->getStateBlockPtrVector())
-  {
-    res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
-  }
+    std::vector<Scalar*> res_block_mem;
+    res_block_mem.reserve(ctr_ptr->getStateBlockPtrVector().size());
+    for (const StateBlockPtr& st : ctr_ptr->getStateBlockPtrVector())
+    {
+        res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
+    }
 
-  auto loss_func_ptr = (ctr_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
+    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");
+    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");
 
-  ctr_2_residual_idx_[ctr_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
-                                                                  loss_func_ptr,
-                                                                  res_block_mem);
+    ctr_2_residual_idx_[ctr_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
+                                                                    loss_func_ptr,
+                                                                    res_block_mem);
 
-  assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+    assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
 }
 
 void CeresManager::removeConstraint(const ConstraintBasePtr& _ctr_ptr)
 {
-  assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end() && "removing a constraint that is not in the ctr_2_residual map");
+    assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end() && "removing a constraint that is not in the ctr_2_residual map");
 
-  ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
-  ctr_2_residual_idx_.erase(_ctr_ptr);
-  ctr_2_costfunction_.erase(_ctr_ptr);
+    ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
+    ctr_2_residual_idx_.erase(_ctr_ptr);
+    ctr_2_costfunction_.erase(_ctr_ptr);
 
-  assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+    assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
 }
 
 void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
 {
-  ceres::LocalParameterization* local_parametrization_ptr = nullptr;
+    ceres::LocalParameterization* local_parametrization_ptr = nullptr;
 
-  if (state_ptr->hasLocalParametrization() &&
-      state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
-  {
-    auto p = state_blocks_local_param_.emplace(
-          state_ptr,
-          std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrizationPtr()));
+    if (state_ptr->hasLocalParametrization() &&
+        state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
+    {
+        auto p = state_blocks_local_param_.emplace(state_ptr,
+                                                   std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrizationPtr()));
 
-    local_parametrization_ptr = p.first->second.get();
-  }
+        local_parametrization_ptr = p.first->second.get();
+    }
 
-  ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
-                                    state_ptr->getSize(),
-                                    local_parametrization_ptr);
+    ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
+                                      state_ptr->getSize(),
+                                      local_parametrization_ptr);
 }
 
 void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
 {
-  assert(state_ptr);
-  ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+    assert(state_ptr);
+    ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
 }
 
 void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
 {
-  assert(state_ptr != nullptr);
-  if (state_ptr->isFixed())
-    ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
-  else
-    ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
+    assert(state_ptr != nullptr);
+    if (state_ptr->isFixed())
+        ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
+    else
+        ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
 }
 
 ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr)
 {
-  assert(_ctr_ptr != nullptr);
+    assert(_ctr_ptr != nullptr);
 
-  // analitic & autodiff jacobian
-  if (_ctr_ptr->getJacobianMethod() == JAC_ANALYTIC || _ctr_ptr->getJacobianMethod() == JAC_AUTO)
-    return std::make_shared<CostFunctionWrapper>(_ctr_ptr);
+    // analitic & autodiff jacobian
+    if (_ctr_ptr->getJacobianMethod() == JAC_ANALYTIC || _ctr_ptr->getJacobianMethod() == JAC_AUTO)
+        return std::make_shared<CostFunctionWrapper>(_ctr_ptr);
 
-  // numeric jacobian
-  else if (_ctr_ptr->getJacobianMethod() == JAC_NUMERIC)
-    return createNumericDiffCostFunction(_ctr_ptr);
+    // numeric jacobian
+    else if (_ctr_ptr->getJacobianMethod() == JAC_NUMERIC)
+        return createNumericDiffCostFunction(_ctr_ptr);
 
-  else
-    throw std::invalid_argument( "Wrong Jacobian Method!" );
+    else
+        throw std::invalid_argument( "Wrong Jacobian Method!" );
 }
 
 void CeresManager::check()
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 94c3d3dbe..3311b1c07 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -26,60 +26,62 @@ WOLF_PTR_TYPEDEFS(CeresManager);
 
 class CeresManager : public SolverManager
 {
-protected:
+    protected:
 
-  std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
-  std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
+        std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
+        std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
 
-  std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
+        std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
 
-  ceres::Solver::Options ceres_options_;
-  ceres::Solver::Summary summary_;
-  std::unique_ptr<ceres::Problem> ceres_problem_;
-  std::unique_ptr<ceres::Covariance> covariance_;
+        ceres::Solver::Options ceres_options_;
+        ceres::Solver::Summary summary_;
+        std::unique_ptr<ceres::Problem> ceres_problem_;
+        std::unique_ptr<ceres::Covariance> covariance_;
 
-public:
+    public:
 
-  CeresManager(const ProblemPtr& _wolf_problem,
-               const ceres::Solver::Options& _ceres_options
-                = ceres::Solver::Options());
+        CeresManager(const ProblemPtr& _wolf_problem,
+                     const ceres::Solver::Options& _ceres_options
+                     = ceres::Solver::Options());
 
-  ~CeresManager();
+        ~CeresManager();
 
-  ceres::Solver::Summary getSummary();
+        ceres::Solver::Summary getSummary();
 
-  virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
-                                  = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
+        virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
+                                        = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
 
-  virtual void computeCovariances(const StateBlockList& st_list) override;
+        virtual void computeCovariances(const StateBlockList& st_list) override;
 
-  ceres::Solver::Options& getSolverOptions();
+        ceres::Solver::Options& getSolverOptions();
 
-private:
+    private:
 
-  std::string solveImpl(const ReportVerbosity report_level) override;
+        std::string solveImpl(const ReportVerbosity report_level) override;
 
-  void addConstraint(const ConstraintBasePtr& ctr_ptr) override;
+        void addConstraint(const ConstraintBasePtr& ctr_ptr) override;
 
-  void removeConstraint(const ConstraintBasePtr& ctr_ptr) override;
+        void removeConstraint(const ConstraintBasePtr& ctr_ptr) override;
 
-  void addStateBlock(const StateBlockPtr& state_ptr) override;
+        void addStateBlock(const StateBlockPtr& state_ptr) override;
 
-  void removeStateBlock(const StateBlockPtr& state_ptr) override;
+        void removeStateBlock(const StateBlockPtr& state_ptr) override;
 
-  void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
+        void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
 
-  ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
+        ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
+
+        void check();
 };
 
 inline ceres::Solver::Summary CeresManager::getSummary()
 {
-  return summary_;
+    return summary_;
 }
 
 inline ceres::Solver::Options& CeresManager::getSolverOptions()
 {
-  return ceres_options_;
+    return ceres_options_;
 }
 
 } // namespace wolf
diff --git a/src/ceres_wrapper/cost_function_wrapper.h b/src/ceres_wrapper/cost_function_wrapper.h
index dbcfdbdff..65566f2fa 100644
--- a/src/ceres_wrapper/cost_function_wrapper.h
+++ b/src/ceres_wrapper/cost_function_wrapper.h
@@ -17,10 +17,8 @@ WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
 
 class CostFunctionWrapper : public ceres::CostFunction
 {
-    protected:
-        ConstraintBasePtr constraint_ptr_;
-
     public:
+        ConstraintBasePtr constraint_ptr_;
 
         CostFunctionWrapper(ConstraintBasePtr _constraint_ptr);
 
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index aec47485d..8f08f8ae8 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -6,179 +6,179 @@
 namespace wolf {
 
 SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
-  wolf_problem_(_wolf_problem)
+          wolf_problem_(_wolf_problem)
 {
-  assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
+    assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
 }
 
 void SolverManager::update()
 {
-  // REMOVE CONSTRAINTS
-  auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
-  while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
-  {
-    if (ctr_notification_it->notification_ == REMOVE)
+    // REMOVE CONSTRAINTS
+    auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
+    while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
     {
-      removeConstraint(ctr_notification_it->constraint_ptr_);
-      ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
-    }
-    else
-      ctr_notification_it++;
-  }
-
-  StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
-
-  for (StateBlockPtr& state : states)
-  {
-    const auto notifications = state->consumeNotifications();
-
-    for (const auto notif : notifications)
-    {
-      switch (notif)
-      {
-      case StateBlock::Notification::ADD:
-      {
-        const bool registered = state_blocks_.find(state)!=state_blocks_.end();
-
-        // call addStateBlock only if first time added.
-        if (!registered)
+        if (ctr_notification_it->notification_ == REMOVE)
         {
-          state_blocks_.emplace(state, state->getState());
-          addStateBlock(state);
+            removeConstraint(ctr_notification_it->constraint_ptr_);
+            ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
         }
+        else
+            ctr_notification_it++;
+    }
 
-        WOLF_DEBUG_COND(registered, "Tried adding an already registered StateBlock.");
-
-        break;
-      }
-      case StateBlock::Notification::UPDATE_STATE:
-      {
-        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
-                        "Updating the state of an unregistered StateBlock !");
-
-        assert(state_blocks_.find(state)!=state_blocks_.end() &&
-            "Updating the state of an unregistered StateBlock !");
-
-        Eigen::VectorXs new_state = state->getState();
-        // We assume the same size for the states in both WOLF and the solver.
-        std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
-
-        break;
-      }
-      case StateBlock::Notification::UPDATE_FIX:
-      {
-        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
-                        "Updating the fix state of an unregistered StateBlock !");
+    StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
 
-        assert(state_blocks_.find(state)!=state_blocks_.end() &&
-            "Updating the fix state of an unregistered StateBlock !");
+    for (StateBlockPtr& state : states)
+    {
+        const auto notifications = state->consumeNotifications();
 
-        if (state_blocks_.find(state)!=state_blocks_.end())
+        for (const auto notif : notifications)
         {
-            updateStateBlockStatus(state);
+            switch (notif)
+            {
+                case StateBlock::Notification::ADD:
+                {
+                    const bool registered = state_blocks_.find(state)!=state_blocks_.end();
+
+                    // call addStateBlock only if first time added.
+                    if (!registered)
+                    {
+                        state_blocks_.emplace(state, state->getState());
+                        addStateBlock(state);
+                    }
+
+                    WOLF_DEBUG_COND(registered, "Tried adding an already registered StateBlock.");
+
+                    break;
+                }
+                case StateBlock::Notification::UPDATE_STATE:
+                {
+                    WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+                                    "Updating the state of an unregistered StateBlock !");
+
+                    assert(state_blocks_.find(state)!=state_blocks_.end() &&
+                           "Updating the state of an unregistered StateBlock !");
+
+                    Eigen::VectorXs new_state = state->getState();
+                    // We assume the same size for the states in both WOLF and the solver.
+                    std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
+
+                    break;
+                }
+                case StateBlock::Notification::UPDATE_FIX:
+                {
+                    WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+                                    "Updating the fix state of an unregistered StateBlock !");
+
+                    assert(state_blocks_.find(state)!=state_blocks_.end() &&
+                           "Updating the fix state of an unregistered StateBlock !");
+
+                    if (state_blocks_.find(state)!=state_blocks_.end())
+                    {
+                        updateStateBlockStatus(state);
+                    }
+
+                    break;
+                }
+                case StateBlock::Notification::REMOVE:
+                {
+                    WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+                                    "Tried to remove a StateBlock that was not added !");
+
+                    if (state_blocks_.find(state)!=state_blocks_.end())
+                    {
+                        removeStateBlock(state);
+                        state_blocks_.erase(state);
+                    }
+
+                    break;
+                }
+                default:
+                    throw std::runtime_error("SolverManager::update: State Block notification "
+                            "must be ADD, STATE_UPDATE, FIX_UPDATE or REMOVE.");
+            }
         }
+    }
 
-        break;
-      }
-      case StateBlock::Notification::REMOVE:
-      {
-        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
-                        "Tried to remove a StateBlock that was not added !");
+    states.clear();
 
-        if (state_blocks_.find(state)!=state_blocks_.end())
+    // ADD CONSTRAINTS
+    while (!wolf_problem_->getConstraintNotificationList().empty())
+    {
+        switch (wolf_problem_->getConstraintNotificationList().front().notification_)
         {
-            removeStateBlock(state);
-            state_blocks_.erase(state);
+            case Notification::ADD:
+            {
+                addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+
+                break;
+            }
+            default:
+                throw std::runtime_error("SolverManager::update:"
+                        " Constraint notification must be ADD or REMOVE.");
         }
 
-        break;
-      }
-      default:
-        throw std::runtime_error("SolverManager::update: State Block notification "
-                                 "must be ADD, STATE_UPDATE, FIX_UPDATE or REMOVE.");
-      }
+        wolf_problem_->getConstraintNotificationList().pop_front();
     }
-  }
 
-  states.clear();
+    assert(wolf_problem_->getConstraintNotificationList().empty() &&
+           "wolf problem's constraints notification list not empty after update");
+    assert(wolf_problem_->getNotifiedStateBlockList().empty() &&
+           "wolf problem's state_blocks notification list not empty after update");
 
-  // ADD CONSTRAINTS
-  while (!wolf_problem_->getConstraintNotificationList().empty())
-  {
-    switch (wolf_problem_->getConstraintNotificationList().front().notification_)
-    {
-    case Notification::ADD:
+    // UPDATE ALL STATES
+    for (StateBlockPtr& state : states)
     {
-      addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
-
-      break;
-    }
-    default:
-      throw std::runtime_error("SolverManager::update:"
-                               " Constraint notification must be ADD or REMOVE.");
+        Eigen::VectorXs new_state = state->getState();
+        std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
     }
-
-    wolf_problem_->getConstraintNotificationList().pop_front();
-  }
-
-  assert(wolf_problem_->getConstraintNotificationList().empty() &&
-         "wolf problem's constraints notification list not empty after update");
-  assert(wolf_problem_->getNotifiedStateBlockList().empty() &&
-         "wolf problem's state_blocks notification list not empty after update");
-
-  // UPDATE ALL STATES
-  for (StateBlockPtr& state : states)
-  {
-      Eigen::VectorXs new_state = state->getState();
-      std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
-  }
 }
 
 wolf::ProblemPtr SolverManager::getProblemPtr()
 {
-  return wolf_problem_;
+    return wolf_problem_;
 }
 
 std::string SolverManager::solve(const ReportVerbosity report_level)
 {
-  // update problem
-  update();
+    // update problem
+    update();
 
-  std::string report = solveImpl(report_level);
+    std::string report = solveImpl(report_level);
 
-  // update StateBlocks with optimized state value.
-  /// @todo whatif someone has changed the state notification during opti ??
+    // update StateBlocks with optimized state value.
+    /// @todo whatif someone has changed the state notification during opti ??
 
-  std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
-                                                     it_end = state_blocks_.end();
-  for (; it != it_end; ++it)
-  {
-    // Avoid usuless copies
-    if (!it->first->isFixed())
-      it->first->setState(it->second, false);
-  }
+    std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
+            it_end = state_blocks_.end();
+    for (; it != it_end; ++it)
+    {
+        // Avoid usuless copies
+        if (!it->first->isFixed())
+            it->first->setState(it->second, false);
+    }
 
-  return report;
+    return report;
 }
 
 Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
 {
-  auto it = state_blocks_.find(state_ptr);
+    auto it = state_blocks_.find(state_ptr);
 
-  if (it == state_blocks_.end())
-    throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+    if (it == state_blocks_.end())
+        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
 
-  return it->second;
+    return it->second;
 }
 
 Scalar* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
 {
-  auto it = state_blocks_.find(state_ptr);
+    auto it = state_blocks_.find(state_ptr);
 
-  if (it == state_blocks_.end())
-    throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+    if (it == state_blocks_.end())
+        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
 
-  return it->second.data();
+    return it->second.data();
 }
 
 } // namespace wolf
-- 
GitLab