diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 2c4fc3f9a4aff42e77746ca1e094dd450feb626c..919e6f941e7bd4e830927ff7854109bd1cd2d746 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -9,316 +9,358 @@ 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 (sb2)
- {
- 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)
{
- auto cost_func_ptr = createCostFunction(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");
- 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;
- ctr_2_residual_idx_[ctr_ptr] =
- ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
- loss_func_ptr, res_block_mem);
+ 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((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,
+ res_block_mem);
+
+ 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());
+ 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;
-
- 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();
- }
-
- ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
- state_ptr->getSize(),
- local_parametrization_ptr);
- updateStateBlockStatus(state_ptr);
+ 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()));
+
+ local_parametrization_ptr = p.first->second.get();
+ }
+
+ ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
+ state_ptr->getSize(),
+ local_parametrization_ptr);
+
+ updateStateBlockStatus(state_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()
+{
+ // Check numbers
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_costfunction_.size());
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size());
+ assert(ceres_problem_->NumParameterBlocks() == state_blocks_.size());
+
+ // Check parameter blocks
+ for (auto&& state_block_pair : state_blocks_)
+ assert(ceres_problem_->HasParameterBlock(state_block_pair.second.data()));
+
+ // Check residual blocks
+ for (auto&& ctr_res_pair : ctr_2_residual_idx_)
+ {
+ // costfunction - residual
+ assert(ctr_2_costfunction_.find(ctr_res_pair.first) != ctr_2_costfunction_.end());
+ assert(ctr_2_costfunction_[ctr_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second));
+
+ // constraint - residual
+ assert(ctr_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second))->constraint_ptr_);
+
+ // parameter blocks - state blocks
+ std::vector<Scalar*> param_blocks;
+ ceres_problem_->GetParameterBlocksForResidualBlock(ctr_res_pair.second, ¶m_blocks);
+ auto i = 0;
+ for (const StateBlockPtr& st : ctr_res_pair.first->getStateBlockPtrVector())
+ {
+ assert(getAssociatedMemBlockPtr(st) == param_blocks[i]);
+ i++;
+ }
+ }
}
} // namespace wolf
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 94c3d3dbe01e066205cd6ceb7179c22b1dc1df42..3311b1c0761146d75456be3909aa881c993fffa8 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 dbcfdbdff4bbf554181c516bdde93753b038f746..65566f2fa314c6a37024e8335990210f347cd437 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/feature_base.cpp b/src/feature_base.cpp
index d57a9a88f2a5c74a1c38e81fc9a16a59b11d6f3b..eb3351129c6b42a51aeef0e2fa98c2fff18df6b1 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -59,7 +59,10 @@ ConstraintBasePtr FeatureBase::addConstraint(ConstraintBasePtr _co_ptr)
_co_ptr->setProblem(getProblem());
// add constraint to be added in solver
if (getProblem() != nullptr)
- getProblem()->addConstraintPtr(_co_ptr);
+ {
+ if (_co_ptr->getStatus() == CTR_ACTIVE)
+ getProblem()->addConstraintPtr(_co_ptr);
+ }
else
WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM.");
return _co_ptr;
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 453e54cb58a5c11297866da827539b640c0e2c59..0891b05cd2bda0b47e1226586f65bc3b2d7ad26d 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
+ state_block_vec_(3), // allow for 6 state blocks by default. Resize in derived constructors if needed.
is_removing_(false),
frame_id_(++frame_id_count_),
type_(NON_KEY_FRAME),
diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index cd14a779efb7fa38be48b1815a86d1cf09a61214..81351162ead4b06d53f2170e66f6c986855f2f88 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -27,7 +27,7 @@
#include "ceres_wrapper/ceres_manager.h"
int main()
- {
+{
/*
* ============= PROBLEM DEFINITION ==================
*
@@ -117,12 +117,12 @@ int main()
// sensor odometer 2D
IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>();
+ intrinsics_odo->k_disp_to_disp = 0.1;
+ intrinsics_odo->k_rot_to_rot = 0.1;
SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
// processor odometer 2D
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
- params_odo->voting_active = true;
- params_odo->time_tolerance = 0.1;
params_odo->max_time_span = 999;
params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement
params_odo->angle_turned = 999;
@@ -133,8 +133,8 @@ int main()
// sensor Range and Bearing
IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
- intrinsics_rb->noise_range_metres_std = 0.1;
intrinsics_rb->noise_bearing_degrees_std = 1.0;
+ intrinsics_rb->noise_range_metres_std = 0.1;
SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
// NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
@@ -143,7 +143,6 @@ int main()
// processor Range and Bearing
ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
- params_rb->voting_active = false;
params_rb->time_tolerance = 0.01;
ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
@@ -168,10 +167,10 @@ int main()
// STEP 1 --------------------------------------------------------------
// initialize
- TimeStamp t(0.0); // t : 0.0
+ TimeStamp t(0.0);
Vector3s x(0,0,0);
Matrix3s P = Matrix3s::Identity() * 0.1;
- problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
+ problem->setPrior(x, P, t, 0.5); // KF1
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
@@ -179,14 +178,14 @@ int main()
ranges << 1.0; // see drawing
bearings << M_PI/2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
- sensor_rb ->process(cap_rb); // L1 : (1,2)
+ sensor_rb ->process(cap_rb);
// STEP 2 --------------------------------------------------------------
- t += 1.0; // t : 1.0
+ t += 1.0;
// motion
CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
- sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
+ sensor_odo ->process(cap_motion); // KF2
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -194,14 +193,14 @@ int main()
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
- sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
+ sensor_rb ->process(cap_rb);
// STEP 3 --------------------------------------------------------------
- t += 1.0; // t : 2.0
+ t += 1.0;
// motion
cap_motion ->setTimeStamp(t);
- sensor_odo ->process(cap_motion); // KF3 : (2,0,0)
+ sensor_odo ->process(cap_motion); // KF3
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -210,6 +209,7 @@ int main()
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+
problem->print(1,0,1,0);
@@ -226,12 +226,9 @@ int main()
for (auto sen : problem->getHardwarePtr()->getSensorList())
for (auto sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ sb->setState(VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto kf : problem->getTrajectoryPtr()->getFrameList())
- if (kf->isKey())
- for (auto sb : kf->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ kf->setState(Vector3s::Random() * 0.5); // We perturb A LOT !
for (auto lmk : problem->getMapPtr()->getLandmarkList())
for (auto sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index aec47485db1277e9ec150ddeeed5b33f2773419c..8f08f8ae81744c6491285848c42b5dfbf3874056 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