Skip to content
Snippets Groups Projects
Commit 8ca57aef authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

bug fixed, my fault

parent fcbed7de
No related branches found
No related tags found
No related merge requests found
......@@ -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()
......
......@@ -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
......
......@@ -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);
......
......@@ -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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment