diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 56f51d0662d9feb82fce50c7429eb0d72477d1c5..db11e336bff04cd2ad1e22255a693cd8b46300b9 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -1280,14 +1280,15 @@ bool Problem::check(bool verbose, std::ostream& stream) const stream << " -> Trj @ " << F->getTrajectory().get() << std::endl; } for (const auto &pair: F->getStateBlockMap()) { + auto sb = pair.second; inconsistency_explanation << "Frame's " << F.get() << " has State block pointer " << sb.get() << " null! \n"; - log.compose( - CheckLog((sb.get() != 0), inconsistency_explanation.str())); + log.compose(CheckLog((sb.get() != 0), inconsistency_explanation.str())); // Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); + if (verbose) { stream << " "<< pair.first << " sb @ " << sb.get(); if (sb) { @@ -1325,9 +1326,8 @@ bool Problem::check(bool verbose, std::ostream& stream) const } // check constrained_by pointer to this frame - // is_consistent = is_consistent && (cby->hasFrameOther(F)); - inconsistency_explanation << "constrained-by frame pointer is " << cby->getFrameOther() - << " but frame pointer is" << F << "\n"; + inconsistency_explanation << "constrained-by frame pointer " << F + << " not found among constrained-by factors\n"; log.compose(CheckLog((cby->hasFrameOther(F)), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); @@ -1359,32 +1359,37 @@ bool Problem::check(bool verbose, std::ostream& stream) const stream << " -> Prb @ " << C->getProblem().get() << std::endl; stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl; } - for (auto pair: C->getStateBlockMap()) + for (auto pair: C->getStateBlockMap()) + { + auto sb = pair.second; + + // check for valid state block + inconsistency_explanation << "Frame's " << F.get() + << " has State block pointer " << sb.get() + << " null! \n"; + log.compose(CheckLog((sb.get() != 0), inconsistency_explanation.str())); + // Clear inconsistency_explanation + std::stringstream().swap(inconsistency_explanation); + + if (verbose) { - auto sb = pair.second; - if (verbose) - { stream << " " << pair.first << " sb @ " << sb.get(); if (sb) { - auto lp = sb->getLocalParametrization(); - if (lp) - stream << " (lp @ " << lp.get() << ")"; + auto lp = sb->getLocalParametrization(); + if (lp) + stream << " (lp @ " << lp.get() << ")"; } stream << std::endl; - } } + } // check problem and frame pointers - // is_consistent = is_consistent && (C->getProblem().get() == P_raw); - inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get() << " but problem pointer is" << P_raw << "\n"; log.compose(CheckLog((C->getProblem().get() == P_raw), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); - // is_consistent = is_consistent && (C->getFrame() == F); - inconsistency_explanation << "Capture frame pointer is " << C->getFrame() << " but frame pointer is" << F << "\n"; log.compose(CheckLog((C->getFrame() == F), inconsistency_explanation.str())); @@ -1430,17 +1435,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get() << std::endl; } - // check problem and capture pointers - // is_consistent = is_consistent && (f->getProblem().get() == P_raw); + // check problem and capture pointers inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get() << " but problem pointer is" << P_raw << "\n"; log.compose(CheckLog((f->getProblem().get() == P_raw), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); - // is_consistent = is_consistent && (f->getCapture() == C); - inconsistency_explanation << "Feature capture pointer is " << f->getCapture() << " but capture pointer is" << C << "\n"; log.compose(CheckLog((f->getCapture() == C), inconsistency_explanation.str())); @@ -1452,31 +1454,18 @@ bool Problem::check(bool verbose, std::ostream& stream) const { if (verbose) { -//<<<<<<< HEAD stream << " <- c" << cby->id() << " -> "; for (const auto& fow : cby->getFeatureOtherList()) stream << " f" << fow.lock()->id(); stream << endl; } + // check constrained_by pointer to this feature -// is_consistent = is_consistent && (cby->hasFeatureOther(f)); - inconsistency_explanation << "constrained by Feature pointer is " << cby->getFeatureOther() - << " but feature pointer is" << f << "\n"; + inconsistency_explanation << "constrained by Feature pointer is " << f + << " not found among constrained-by factors\n"; log.compose(CheckLog((cby->hasFeatureOther(f)), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); -//======= -// stream << " <- c" << cby->id() << " -> Ftr" << cby->getFeatureOther()->id() << std::endl; -// } -// // check constrained_by pointer to this feature -// // is_consistent = is_consistent && (cby->getFeatureOther() == f); -// -// inconsistency_explanation << "constrained by Feature pointer is " << cby->getFeatureOther() -// << " but feature pointer is" << f << "\n"; -// log.compose(CheckLog((cby->getFeatureOther() == f), inconsistency_explanation.str())); -// //Clear inconsistency_explanation -// std::stringstream().swap(inconsistency_explanation); -//>>>>>>> origin/devel } // Factors ======================================================================================= @@ -1497,7 +1486,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const // find constrained_by pointer in constrained frame for (const auto& Fow : c->getFrameOtherList()) { -//<<<<<<< HEAD if (!Fow.expired()) { const auto& Fo = Fow.lock(); @@ -1507,37 +1495,15 @@ bool Problem::check(bool verbose, std::ostream& stream) const for (auto cby : Fo->getConstrainedByList()) stream << " c" << cby->id(); } + // check constrained_by pointer in constrained frame bool found = Fo->isConstrainedBy(c); -// is_consistent = is_consistent && found; inconsistency_explanation << "The constrained Feature " << Fo << " does not close the constrained-by loop start = " << c << "\n"; log.compose(CheckLog((found), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); } -//======= -// if (verbose) -// stream << " ( --> Frm" << Fo->id() << " <- "; -// bool found = false; -// for (auto cby : Fo->getConstrainedByList()) -// { -// if (verbose) -// stream << " Fac" << cby->id(); -// found = found || (c == cby); -// } -// if (verbose) -// stream << ")"; -// // check constrained_by pointer in constrained frame -// // is_consistent = is_consistent && found; -// -// inconsistency_explanation << "The constrained Feature " << Fo -// << " does not close the constrained-by loop start = " << c << "\n"; -// log.compose(CheckLog((found), inconsistency_explanation.str())); -// //Clear inconsistency_explanation -// std::stringstream().swap(inconsistency_explanation); -// -//>>>>>>> origin/devel } if (verbose && !c->getFrameOtherList().empty()) cout << ")"; @@ -1545,7 +1511,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const // find constrained_by pointer in constrained capture for (const auto& Cow : c->getCaptureOtherList()) { -//<<<<<<< HEAD if (!Cow.expired()) { const auto& Co = Cow.lock(); @@ -1556,37 +1521,15 @@ bool Problem::check(bool verbose, std::ostream& stream) const for (auto cby : Co->getConstrainedByList()) stream << " c" << cby->id(); } + // check constrained_by pointer in constrained frame bool found = Co->isConstrainedBy(c); -// is_consistent = is_consistent && found; inconsistency_explanation << "The constrained capture " << Co << " does not close the constrained-by loop start = " << c << "\n"; log.compose(CheckLog((found), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); } -//======= -// if (verbose) -// stream << " ( --> Cap" << Co->id() << " <- "; -// bool found = false; -// for (auto cby : Co->getConstrainedByList()) -// { -// if (verbose) -// stream << " Fac" << cby->id(); -// found = found || (c == cby); -// } -// if (verbose) -// stream << ")"; -// // check constrained_by pointer in constrained frame -// // is_consistent = is_consistent && found; -// -// inconsistency_explanation << "The constrained capture " << Co -// << " does not close the constrained-by loop start = " << c << "\n"; -// log.compose(CheckLog((found), inconsistency_explanation.str())); -// //Clear inconsistency_explanation -// std::stringstream().swap(inconsistency_explanation); -// -//>>>>>>> origin/devel } if (verbose && !c->getCaptureOtherList().empty()) cout << ")"; @@ -1594,7 +1537,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const // find constrained_by pointer in constrained feature for (const auto& fow : c->getFeatureOtherList()) { -//<<<<<<< HEAD if (!fow.expired()) { const auto& fo = fow.lock(); @@ -1605,36 +1547,15 @@ bool Problem::check(bool verbose, std::ostream& stream) const for (auto cby : fo->getConstrainedByList()) stream << " c" << cby->id(); } + // check constrained_by pointer in constrained feature bool found = fo->isConstrainedBy(c); -// is_consistent = is_consistent && found; inconsistency_explanation << "The constrained feature" << fo << " does not close the constrained-by loop start = " << c << "\n"; log.compose(CheckLog((found), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); } -//======= -// if (verbose) -// stream << " ( --> Ftr" << fo->id() << " <- "; -// bool found = false; -// for (auto cby : fo->getConstrainedByList()) -// { -// if (verbose) -// stream << " Fac" << cby->id(); -// found = found || (c == cby); -// } -// if (verbose) -// stream << ")"; -// // check constrained_by pointer in constrained feature -// // is_consistent = is_consistent && found; -// -// inconsistency_explanation << "The constrained feature" << fo -// << " does not close the constrained-by loop start = " << c << "\n"; -// log.compose(CheckLog((found), inconsistency_explanation.str())); -// //Clear inconsistency_explanation -// std::stringstream().swap(inconsistency_explanation); -//>>>>>>> origin/devel } if (verbose && !c->getFeatureOtherList().empty()) cout << ")"; @@ -1642,7 +1563,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const // find constrained_by pointer in constrained landmark for (const auto& Low : c->getLandmarkOtherList()) { -//<<<<<<< HEAD if (Low.expired()) { const auto& Lo = Low.lock(); @@ -1653,9 +1573,9 @@ bool Problem::check(bool verbose, std::ostream& stream) const for (auto cby : Lo->getConstrainedByList()) stream << " c" << cby->id(); } + // check constrained_by pointer in constrained landmark bool found = Lo->isConstrainedBy(c); -// is_consistent = is_consistent && found; inconsistency_explanation << "The constrained landmark " << Lo << " does not close the constrained-by loop start = " << c << "\n"; log.compose(CheckLog((found), inconsistency_explanation.str())); @@ -1668,47 +1588,20 @@ bool Problem::check(bool verbose, std::ostream& stream) const if (verbose) stream << endl; -//======= -// if (verbose) -// stream << " ( --> Lmk" << Lo->id() << " <- "; -// bool found = false; -// for (auto cby : Lo->getConstrainedByList()) -// { -// if (verbose) -// stream << " Fac" << cby->id(); -// found = found || (c == cby); -// } -// if (verbose) -// stream << ")"; -// // check constrained_by pointer in constrained landmark -// // is_consistent = is_consistent && found; -// -// inconsistency_explanation << "The constrained landmark " << Lo -// << " does not close the constrained-by loop start = " << c << "\n"; -// log.compose(CheckLog((found), inconsistency_explanation.str())); -// //Clear inconsistency_explanation -// std::stringstream().swap(inconsistency_explanation); -// } -// if (verbose) -// stream << std::endl; -//>>>>>>> origin/devel if (verbose) { stream << " -> Prb @ " << c->getProblem().get() << std::endl; stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl; } - // check problem and feature pointers - // is_consistent = is_consistent && (c->getProblem().get() == P_raw); + // check problem and feature pointers inconsistency_explanation << "The factor " << c << " has problem ptr " << c->getProblem().get() << " but problem ptr is " << P_raw << "\n"; log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); - // is_consistent = is_consistent && (c->getFeature() == f); - inconsistency_explanation << "The factor " << c << " has feature ptr " << c->getFeature() << " but it should have " << f << "\n"; log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str())); @@ -1733,7 +1626,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const bool found_here; // find in own Frame -//<<<<<<< HEAD found_here = F->hasStateBlock(sb); if (found_here && verbose) stream << " F" << F->id(); found = found || found_here; @@ -1751,28 +1643,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const if (found_here && verbose) stream << " F" << F->id() << ".C" << FC->id(); found = found || found_here; } -//======= -// sb_vec = F->getStateBlockVec(); -// found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end()); -// if (found_here) stream << " F" << F->id(); -// found = found || found_here; -// -// // find in own Capture -// sb_vec = C->getStateBlockVec(); -// found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end()); -// if (found_here) stream << " C" << C->id(); -// found = found || found_here; -// -// // Find in other Captures of the own Frame -// if (!found_here) -// for (auto FC : F->getCaptureList()) -// { -// sb_vec = FC->getStateBlockVec(); -// found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end()); -// if (found_here) stream << " F" << F->id() << ".C" << FC->id(); -// found = found || found_here; -// } -//>>>>>>> origin/devel // find in own Sensor if (S) @@ -1872,9 +1742,6 @@ bool Problem::check(bool verbose, std::ostream& stream) const log.compose(CheckLog((sb.get() != nullptr), inconsistency_explanation.str())); //Clear inconsistency_explanation std::stringstream().swap(inconsistency_explanation); - - // check that all state block pointers were found - // is_consistent = is_consistent && found; } } }