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

asserts previous to associatedMemBlock assert

parent e5efaf4f
No related branches found
No related tags found
No related merge requests found
Pipeline #5934 passed
...@@ -114,12 +114,17 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -114,12 +114,17 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
} }
// double loop all against all (without repetitions) // double loop all against all (without repetitions)
for (unsigned int i = 0; i < all_state_blocks.size(); i++) for (unsigned int i = 0; i < all_state_blocks.size(); i++)
{
assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
for (unsigned int j = i; j < all_state_blocks.size(); j++) for (unsigned int j = i; j < all_state_blocks.size(); j++)
{ {
assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]); state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]), double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
getAssociatedMemBlockPtr(all_state_blocks[j])); getAssociatedMemBlockPtr(all_state_blocks[j]));
} }
}
break; break;
} }
case CovarianceBlocksToBeComputed::ALL_MARGINALS: case CovarianceBlocksToBeComputed::ALL_MARGINALS:
...@@ -128,26 +133,36 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -128,26 +133,36 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
for(auto fr_ptr : *wolf_problem_->getTrajectory()) for(auto fr_ptr : *wolf_problem_->getTrajectory())
if (fr_ptr->isKey()) if (fr_ptr->isKey())
for (const auto& key1 : wolf_problem_->getFrameStructure()) for (const auto& key1 : wolf_problem_->getFrameStructure())
{
const auto& sb1 = fr_ptr->getStateBlock(key1);
assert(isStateBlockRegisteredDerived(sb1));
for (const auto& key2 : wolf_problem_->getFrameStructure()) for (const auto& key2 : wolf_problem_->getFrameStructure())
{ {
const auto& sb1 = fr_ptr->getStateBlock(key1);
const auto& sb2 = fr_ptr->getStateBlock(key2); const auto& sb2 = fr_ptr->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb1, sb2); state_block_pairs.emplace_back(sb1, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2)); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
if (sb1 == sb2) if (sb1 == sb2)
break; break;
} }
}
// landmark state blocks // landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto sb : l_ptr->getUsedStateBlockVec()) for (auto sb : l_ptr->getUsedStateBlockVec())
{
assert(isStateBlockRegisteredDerived(sb));
for(auto sb2 : l_ptr->getUsedStateBlockVec()) for(auto sb2 : l_ptr->getUsedStateBlockVec())
{ {
assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb, sb2); state_block_pairs.emplace_back(sb, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2)); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
if (sb == sb2) if (sb == sb2)
break; break;
} }
}
// // loop all marginals (PO marginals) // // loop all marginals (PO marginals)
// for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++) // for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
// { // {
...@@ -166,6 +181,9 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -166,6 +181,9 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
//robot-robot //robot-robot
auto last_key_frame = wolf_problem_->getLastFrame(); auto last_key_frame = wolf_problem_->getLastFrame();
assert(isStateBlockRegisteredDerived(last_key_frame->getP()));
assert(isStateBlockRegisteredDerived(last_key_frame->getO()));
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP()); state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO()); state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO()); state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO());
...@@ -186,6 +204,8 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -186,6 +204,8 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++) for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
{ {
assert(isStateBlockRegisteredDerived(*state_it));
// robot - landmark // robot - landmark
state_block_pairs.emplace_back(last_key_frame->getP(), *state_it); state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
state_block_pairs.emplace_back(last_key_frame->getO(), *state_it); state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
...@@ -197,8 +217,10 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -197,8 +217,10 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
// landmark marginal // landmark marginal
for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++) 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); assert(isStateBlockRegisteredDerived(*next_state_it));
double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
state_block_pairs.emplace_back(*state_it, *next_state_it);
double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
getAssociatedMemBlockPtr((*next_state_it))); getAssociatedMemBlockPtr((*next_state_it)));
} }
} }
...@@ -229,19 +251,19 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -229,19 +251,19 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
// Error // Error
if (not last_frame->hasStateBlock('P') or if (not last_frame->hasStateBlock('P') or
not isStateBlockRegistered(last_frame->getStateBlock('P')) or not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or
not last_frame->hasStateBlock('O') or not last_frame->hasStateBlock('O') or
not isStateBlockRegistered(last_frame->getStateBlock('O')) or not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')) or
not sb_gnss_T or not sb_gnss_T or
not isStateBlockRegistered(sb_gnss_T)) not isStateBlockRegisteredDerived(sb_gnss_T))
{ {
WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P O or T, returning..."); WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P O or T, returning...");
WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found"); WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found");
WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegistered(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P')); WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
WOLF_WARN_COND(not last_frame->getStateBlock('O'), "SolverCeres::computeCovariances: KF state block 'O' not found"); WOLF_WARN_COND(not last_frame->getStateBlock('O'), "SolverCeres::computeCovariances: KF state block 'O' not found");
WOLF_WARN_COND(last_frame->getStateBlock('O') and not isStateBlockRegistered(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P')); WOLF_WARN_COND(last_frame->getStateBlock('O') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'O' not registered ", last_frame->getStateBlock('O'));
WOLF_WARN_COND(not sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found"); WOLF_WARN_COND(not sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found");
WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegistered(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T); WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegisteredDerived(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T);
return false; return false;
} }
...@@ -253,12 +275,18 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -253,12 +275,18 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
// double loop all against all (without repetitions) // double loop all against all (without repetitions)
for (unsigned int i = 0; i < gauss_state_blocks.size(); i++) for (unsigned int i = 0; i < gauss_state_blocks.size(); i++)
{
assert(isStateBlockRegisteredDerived(gauss_state_blocks[i]));
for (unsigned int j = i; j < gauss_state_blocks.size(); j++) for (unsigned int j = i; j < gauss_state_blocks.size(); j++)
{ {
assert(isStateBlockRegisteredDerived(gauss_state_blocks[j]));
state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[j]); state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[j]);
double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]), double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]),
getAssociatedMemBlockPtr(gauss_state_blocks[j])); getAssociatedMemBlockPtr(gauss_state_blocks[j]));
} }
}
break; break;
} }
...@@ -301,15 +329,20 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) ...@@ -301,15 +329,20 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
std::vector<std::pair<const double*, const double*>> double_pairs; std::vector<std::pair<const double*, const double*>> double_pairs;
// double loop all against all (without repetitions) // double loop all against all (without repetitions)
for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++){ for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
{
if (*st_it1 == nullptr){ if (*st_it1 == nullptr){
continue; continue;
} }
assert(isStateBlockRegisteredDerived(*st_it1));
for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++) for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
{ {
if (*st_it2 == nullptr){ if (*st_it2 == nullptr){
continue; continue;
} }
assert(isStateBlockRegisteredDerived(*st_it2));
state_block_pairs.emplace_back(*st_it1, *st_it2); state_block_pairs.emplace_back(*st_it1, *st_it2);
double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)), double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
getAssociatedMemBlockPtr((*st_it2))); getAssociatedMemBlockPtr((*st_it2)));
...@@ -341,7 +374,7 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) ...@@ -341,7 +374,7 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
{ {
assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map"); assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map");
auto cost_func_ptr = createCostFunction(fac_ptr); auto cost_func_ptr = createCostFunction(fac_ptr);
fac_2_costfunction_[fac_ptr] = cost_func_ptr; fac_2_costfunction_[fac_ptr] = cost_func_ptr;
...@@ -350,6 +383,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) ...@@ -350,6 +383,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size()); res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector()) for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
{ {
assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb");
res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) ); res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
} }
...@@ -381,7 +415,7 @@ void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr) ...@@ -381,7 +415,7 @@ void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr)
void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr) void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
{ {
assert(state_ptr); assert(state_ptr);
assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added"); assert(!isStateBlockRegisteredDerived(state_ptr) && "adding a state block already added");
assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block"); assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block");
ceres::LocalParameterization* local_parametrization_ptr = nullptr; ceres::LocalParameterization* local_parametrization_ptr = nullptr;
...@@ -408,7 +442,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr) ...@@ -408,7 +442,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
{ {
//std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
assert(state_ptr); assert(state_ptr);
assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres"); assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres");
ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr)); ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
state_blocks_local_param_.erase(state_ptr); state_blocks_local_param_.erase(state_ptr);
} }
...@@ -416,6 +451,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr) ...@@ -416,6 +451,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
{ {
assert(state_ptr != nullptr); assert(state_ptr != nullptr);
assert(isStateBlockRegisteredDerived(state_ptr) && "updating status of a state block that is not in ceres");
if (state_ptr->isFixed()) if (state_ptr->isFixed())
ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr)); ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
else else
......
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