diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index acf4585d32b2310294be3da40ea4c236503c4d78..b1b8fbbd3aa3d3e047a6e68445cf6281142b6e35 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -7,319 +7,319 @@
namespace wolf {
CeresManager::CeresManager(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;
- #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;
- #elif CERES_VERSION_MINOR >= 10
- covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
- #else
- covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
- #endif
- covariance_options.num_threads = 8;
- covariance_options.apply_loss_function = false;
- //covariance_options.null_space_rank = -1;
- covariance_ = new 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_ = new ceres::Problem(problem_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;
+#elif CERES_VERSION_MINOR >= 10
+ covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
+#else
+ covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+#endif
+ covariance_options.num_threads = 8;
+ covariance_options.apply_loss_function = false;
+ //covariance_options.null_space_rank = -1;
+ covariance_ = new 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_ = new ceres::Problem(problem_options);
}
CeresManager::~CeresManager()
{
-// std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl;
-// std::cout << "ceres parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
- while (!ctr_2_residual_idx_.empty())
- removeConstraint(ctr_2_residual_idx_.begin()->first);
-// std::cout << "all residuals removed! \n";
-
- delete covariance_;
- //std::cout << "covariance deleted! \n";
- delete ceres_problem_;
- //std::cout << "ceres problem deleted! \n";
+ // std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl;
+ // std::cout << "ceres parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
+ while (!ctr_2_residual_idx_.empty())
+ removeConstraint(ctr_2_residual_idx_.begin()->first);
+ // std::cout << "all residuals removed! \n";
+
+ delete covariance_;
+ //std::cout << "covariance deleted! \n";
+ delete ceres_problem_;
+ //std::cout << "ceres problem deleted! \n";
}
std::string CeresManager::solve(const unsigned int& _report_level)
{
- //std::cout << "Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
-
- // update problem
- update();
-
- //std::cout << "After Update: Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
-
- // run Ceres Solver
- ceres::Solve(ceres_options_, ceres_problem_, &summary_);
- //std::cout << "solved" << std::endl;
-
- //return report
- if (_report_level == 0)
- return std::string();
- else if (_report_level == 1)
- return summary_.BriefReport();
- else if (_report_level == 2)
- return summary_.FullReport();
- else
- throw std::invalid_argument( "Report level should be 0, 1 or 2!" );
+ //std::cout << "Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
+
+ // update problem
+ update();
+
+ //std::cout << "After Update: Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
+
+ // run Ceres Solver
+ ceres::Solve(ceres_options_, ceres_problem_, &summary_);
+ //std::cout << "solved" << std::endl;
+
+ //return report
+ if (_report_level == 0)
+ return std::string();
+ else if (_report_level == 1)
+ return summary_.BriefReport();
+ else if (_report_level == 2)
+ return summary_.FullReport();
+ else
+ throw std::invalid_argument( "Report level should be 0, 1 or 2!" );
}
void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
{
- //std::cout << "CeresManager: computing covariances..." << std::endl;
-
- // 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)
+ //std::cout << "CeresManager: computing covariances..." << std::endl;
+
+ // 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 ALL:
+ {
+ // first create a vector containing all state blocks
+ std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks, Frame_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())
{
- case ALL:
- {
- // first create a vector containing all state blocks
- std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks, Frame_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(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr());
- }
- break;
- }
- case ALL_MARGINALS:
+ 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(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr());
+ }
+ break;
+ }
+ case 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(sb->getPtr(), sb2->getPtr());
+ 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())
{
- // 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(sb->getPtr(), sb2->getPtr());
- 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(sb->getPtr(), sb2->getPtr());
- 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;
+ state_block_pairs.emplace_back(sb, sb2);
+ double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
+ if (sb == sb2) break;
}
- case ROBOT_LANDMARKS:
+ // // 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 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(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr());
+ double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
+ double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
+
+ // 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(last_key_frame->getPPtr()->getPtr(), (*state_it)->getPtr());
+ double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), (*state_it)->getPtr());
+
+ // landmark marginal
+ for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
{
- //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(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr());
- double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
- double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
-
- // 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(last_key_frame->getPPtr()->getPtr(), (*state_it)->getPtr());
- double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), (*state_it)->getPtr());
-
- // 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((*state_it)->getPtr(), (*next_state_it)->getPtr());
- }
- }
- }
- break;
+ state_block_pairs.emplace_back(*state_it, *next_state_it);
+ double_pairs.emplace_back((*state_it)->getPtr(), (*next_state_it)->getPtr());
}
+ }
}
- //std::cout << "pairs... " << double_pairs.size() << std::endl;
-
- // COMPUTE DESIRED COVARIANCES
- if (covariance_->Compute(double_pairs, ceres_problem_))
+ break;
+ }
+ }
+ //std::cout << "pairs... " << double_pairs.size() << std::endl;
+
+ // COMPUTE DESIRED COVARIANCES
+ if (covariance_->Compute(double_pairs, ceres_problem_))
+ {
+ // STORE DESIRED COVARIANCES
+ for (unsigned int i = 0; i < double_pairs.size(); i++)
{
- // 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;
- }
+ 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.push_back(std::pair<StateBlockPtr, StateBlockPtr>(*st_it1,*st_it2));
- double_pairs.push_back(std::pair<const double*, const double*>((*st_it1)->getPtr(),(*st_it2)->getPtr()));
- }
+ // 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.push_back(std::pair<StateBlockPtr, StateBlockPtr>(*st_it1,*st_it2));
+ double_pairs.push_back(std::pair<const double*, const double*>((*st_it1)->getPtr(),(*st_it2)->getPtr()));
+ }
- //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_))
- // 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_))
+ // 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(ConstraintBasePtr _ctr_ptr)
{
- ctr_2_costfunction_[_ctr_ptr] = createCostFunction(_ctr_ptr);
+ ctr_2_costfunction_[_ctr_ptr] = createCostFunction(_ctr_ptr);
-// std::cout << "adding constraint " << _ctr_ptr->id() << std::endl;
-// std::cout << "constraint pointer " << _ctr_ptr << std::endl;
+ // std::cout << "adding constraint " << _ctr_ptr->id() << std::endl;
+ // std::cout << "constraint pointer " << _ctr_ptr << std::endl;
- if (_ctr_ptr->getApplyLossFunction())
- ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), new ceres::CauchyLoss(0.5), _ctr_ptr->getStateScalarPtrVector());
- else
- ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), NULL, _ctr_ptr->getStateScalarPtrVector());
+ if (_ctr_ptr->getApplyLossFunction())
+ ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), new ceres::CauchyLoss(0.5), _ctr_ptr->getStateScalarPtrVector());
+ else
+ ctr_2_residual_idx_[_ctr_ptr] = ceres_problem_->AddResidualBlock(ctr_2_costfunction_[_ctr_ptr].get(), NULL, _ctr_ptr->getStateScalarPtrVector());
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
void CeresManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
{
-// std::cout << "removing constraint " << _ctr_ptr->id() << std::endl;
+ // std::cout << "removing constraint " << _ctr_ptr->id() << std::endl;
- assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end());
- ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
- ctr_2_residual_idx_.erase(_ctr_ptr);
- ctr_2_costfunction_.erase(_ctr_ptr);
+ assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end());
+ ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
+ ctr_2_residual_idx_.erase(_ctr_ptr);
+ ctr_2_costfunction_.erase(_ctr_ptr);
-// std::cout << "removingremoved!" << std::endl;
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ // std::cout << "removingremoved!" << std::endl;
+ assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
void CeresManager::addStateBlock(StateBlockPtr _st_ptr)
{
-// std::cout << "Adding State Block " << _st_ptr->getPtr() << std::endl;
-// std::cout << " size: " << _st_ptr->getSize() << std::endl;
-// std::cout << " vector: " << _st_ptr->getVector().transpose() << std::endl;
-
- if (_st_ptr->hasLocalParametrization())
- {
-// std::cout << "Local Parametrization to be added:" << _st_ptr->getLocalParametrizationPtr() << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), new LocalParametrizationWrapper(_st_ptr->getLocalParametrizationPtr()));
- }
- else
- {
-// std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), nullptr);
- }
+ // std::cout << "Adding State Block " << _st_ptr->getPtr() << std::endl;
+ // std::cout << " size: " << _st_ptr->getSize() << std::endl;
+ // std::cout << " vector: " << _st_ptr->getVector().transpose() << std::endl;
+
+ if (_st_ptr->hasLocalParametrization())
+ {
+ // std::cout << "Local Parametrization to be added:" << _st_ptr->getLocalParametrizationPtr() << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), new LocalParametrizationWrapper(_st_ptr->getLocalParametrizationPtr()));
+ }
+ else
+ {
+ // std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), nullptr);
+ }
}
void CeresManager::removeStateBlock(StateBlockPtr _st_ptr)
{
- //std::cout << "Removing State Block " << _st_ptr << std::endl;
- assert(_st_ptr);
- ceres_problem_->RemoveParameterBlock(_st_ptr->getPtr());
+ //std::cout << "Removing State Block " << _st_ptr << std::endl;
+ assert(_st_ptr);
+ ceres_problem_->RemoveParameterBlock(_st_ptr->getPtr());
}
void CeresManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
{
- assert(_st_ptr != nullptr);
- if (_st_ptr->isFixed())
- ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
- else
- ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
+ assert(_st_ptr != nullptr);
+ if (_st_ptr->isFixed())
+ ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
+ else
+ ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
}
ceres::CostFunctionPtr CeresManager::createCostFunction(ConstraintBasePtr _ctr_ptr)
{
- assert(_ctr_ptr != nullptr);
- //std::cout << "creating cost function for constraint " << _ctr_ptr->id() << std::endl;
+ assert(_ctr_ptr != nullptr);
+ //std::cout << "creating cost function for constraint " << _ctr_ptr->id() << std::endl;
- // 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( "Bad Jacobian Method!" );
+ else
+ throw std::invalid_argument( "Bad Jacobian Method!" );
}
} // namespace wolf
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index c36144c4ed60cb8f63e840475f3e90be138efd1e..0a41a4da218136457d5e99ecb092923df6b49bf7 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -13,7 +13,7 @@
#include "create_numeric_diff_cost_function.h"
namespace ceres {
- typedef std::shared_ptr<CostFunction> CostFunctionPtr;
+typedef std::shared_ptr<CostFunction> CostFunctionPtr;
}
namespace wolf {
@@ -26,52 +26,52 @@ WOLF_PTR_TYPEDEFS(CeresManager);
class CeresManager : public SolverManager
{
- protected:
- std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
- std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
- ceres::Problem* ceres_problem_;
- ceres::Solver::Options ceres_options_;
- ceres::Covariance* covariance_;
- ceres::Solver::Summary summary_;
+protected:
+ std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
+ std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
+ ceres::Problem* ceres_problem_;
+ ceres::Solver::Options ceres_options_;
+ ceres::Covariance* covariance_;
+ ceres::Solver::Summary summary_;
- public:
- CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options());
+public:
+ CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options());
- ~CeresManager();
+ ~CeresManager();
- virtual std::string solve(const unsigned int& _report_level);
+ virtual std::string solve(const unsigned int& _report_level);
- ceres::Solver::Summary getSummary();
+ ceres::Solver::Summary getSummary();
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+ virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
- virtual void computeCovariances(const StateBlockList& st_list);
+ virtual void computeCovariances(const StateBlockList& st_list);
- ceres::Solver::Options& getSolverOptions();
+ ceres::Solver::Options& getSolverOptions();
- private:
+private:
- virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
+ virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
- virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
+ virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
- virtual void addStateBlock(StateBlockPtr _st_ptr);
+ virtual void addStateBlock(StateBlockPtr _st_ptr);
- virtual void removeStateBlock(StateBlockPtr _st_ptr);
+ virtual void removeStateBlock(StateBlockPtr _st_ptr);
- virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
+ virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
- ceres::CostFunctionPtr createCostFunction(ConstraintBasePtr _ctr_ptr);
+ ceres::CostFunctionPtr createCostFunction(ConstraintBasePtr _ctr_ptr);
};
inline ceres::Solver::Summary CeresManager::getSummary()
{
- return summary_;
+ return summary_;
}
inline ceres::Solver::Options& CeresManager::getSolverOptions()
{
- return ceres_options_;
+ return ceres_options_;
}
} // namespace wolf