diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index 9037c4ddd743f72186f0b1d54714978d7accdb97..92846f85b76e7b67286f35c257ce32727de6ccb8 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -114,12 +114,17 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) } // double loop all against all (without repetitions) 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++) { + assert(isStateBlockRegisteredDerived(all_state_blocks[i])); + 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: @@ -128,26 +133,36 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) for(auto fr_ptr : *wolf_problem_->getTrajectory()) if (fr_ptr->isKey()) for (const auto& key1 : wolf_problem_->getFrameStructure()) + { + const auto& sb1 = fr_ptr->getStateBlock(key1); + assert(isStateBlockRegisteredDerived(sb1)); + for (const auto& key2 : wolf_problem_->getFrameStructure()) { - const auto& sb1 = fr_ptr->getStateBlock(key1); const auto& sb2 = fr_ptr->getStateBlock(key2); + assert(isStateBlockRegisteredDerived(sb2)); + state_block_pairs.emplace_back(sb1, sb2); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2)); if (sb1 == sb2) break; } + } // landmark state blocks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) for (auto sb : l_ptr->getUsedStateBlockVec()) + { + assert(isStateBlockRegisteredDerived(sb)); for(auto sb2 : l_ptr->getUsedStateBlockVec()) { + assert(isStateBlockRegisteredDerived(sb2)); 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++) // { @@ -166,6 +181,9 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) //robot-robot 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->getO()); state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO()); @@ -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++) { + assert(isStateBlockRegisteredDerived(*state_it)); + // robot - landmark state_block_pairs.emplace_back(last_key_frame->getP(), *state_it); state_block_pairs.emplace_back(last_key_frame->getO(), *state_it); @@ -197,8 +217,10 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) // 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)), + assert(isStateBlockRegisteredDerived(*next_state_it)); + + state_block_pairs.emplace_back(*state_it, *next_state_it); + double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)), getAssociatedMemBlockPtr((*next_state_it))); } } @@ -229,19 +251,19 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) // Error 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 isStateBlockRegistered(last_frame->getStateBlock('O')) or + not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')) 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_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(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(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; } @@ -253,12 +275,18 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) // double loop all against all (without repetitions) 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++) { + assert(isStateBlockRegisteredDerived(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]), getAssociatedMemBlockPtr(gauss_state_blocks[j])); } + } break; } @@ -301,15 +329,20 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) 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_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++) + { if (*st_it1 == nullptr){ continue; } + assert(isStateBlockRegisteredDerived(*st_it1)); + for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++) { if (*st_it2 == nullptr){ continue; } + assert(isStateBlockRegisteredDerived(*st_it2)); + state_block_pairs.emplace_back(*st_it1, *st_it2); double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)), getAssociatedMemBlockPtr((*st_it2))); @@ -341,7 +374,7 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) 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); fac_2_costfunction_[fac_ptr] = cost_func_ptr; @@ -350,6 +383,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size()); 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) ); } @@ -381,7 +415,7 @@ void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr) void SolverCeres::addStateBlockDerived(const StateBlockPtr& 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"); ceres::LocalParameterization* local_parametrization_ptr = nullptr; @@ -408,7 +442,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr) { //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl; 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)); state_blocks_local_param_.erase(state_ptr); } @@ -416,6 +451,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr) void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) { assert(state_ptr != nullptr); + assert(isStateBlockRegisteredDerived(state_ptr) && "updating status of a state block that is not in ceres"); + if (state_ptr->isFixed()) ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr)); else