diff --git a/include/core/utils/check_log.hpp b/include/core/utils/check_log.hpp
index b1905d7b7c1a0afdb8247ee8686467a104b2b330..1a1dd7505982e6406565935ee2f8b6bde5cf0e20 100644
--- a/include/core/utils/check_log.hpp
+++ b/include/core/utils/check_log.hpp
@@ -17,11 +17,11 @@ class CheckLog
is_consistent_ = true;
explanation_ = "";
}
- CheckLog(bool consistent, std::string explanation)
+ CheckLog(bool _consistent, std::string _explanation)
{
- is_consistent_ = consistent;
- if (not consistent)
- explanation_ = explanation;
+ is_consistent_ = _consistent;
+ if (not _consistent)
+ explanation_ = _explanation;
else
explanation_ = "";
}
@@ -31,12 +31,12 @@ class CheckLog
is_consistent_ = is_consistent_ and l.is_consistent_;
explanation_ = explanation_ + l.explanation_;
}
- void addAssertion(bool condition, std::stringstream& stream)
+ void addAssertion(bool _condition, std::stringstream& _stream)
{
- auto cl = CheckLog(condition, stream.str());
+ auto cl = CheckLog(_condition, _stream.str());
this->compose(cl);
// Clear inconsistency_explanation
- std::stringstream().swap(stream);
+ std::stringstream().swap(_stream);
}
};
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index dc3b90ef9abcc7e7a7818f8e2dbda5f0985c5134..da6c8cbd654e557a6bec6c7a8e5d5ffd53947c3f 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -887,226 +887,226 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric, bool state_blocks) const
+void Problem::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
{
- stream << std::endl;
- stream << "P: wolf tree status ---------------------" << std::endl;
- stream << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
- if (depth >= 1)
+ _stream << std::endl;
+ _stream << "P: wolf tree status ---------------------" << std::endl;
+ _stream << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
+ if (_depth >= 1)
{
// Sensors =======================================================================================
for (auto S : getHardware()->getSensorList())
{
- stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (depth < 2)
- stream << " -- " << S->getProcessorList().size() << "p";
- stream << std::endl;
- if (metric && state_blocks)
+ _stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
+ if (_depth < 2)
+ _stream << " -- " << S->getProcessorList().size() << "p";
+ _stream << std::endl;
+ if (_metric && _state_blocks)
{
- stream << " sb: ";
+ _stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
+ _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
- stream << std::endl;
+ _stream << std::endl;
}
- else if (metric)
+ else if (_metric)
{
- stream << " ( ";
+ _stream << " ( ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- stream << sb->getState().transpose() << " ";
+ _stream << sb->getState().transpose() << " ";
}
- stream << ")" << std::endl;
+ _stream << ")" << std::endl;
}
- else if (state_blocks)
+ else if (_state_blocks)
{
- stream << " sb: ";
+ _stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
- stream << std::endl;
+ _stream << std::endl;
}
- if (depth >= 2)
+ if (_depth >= 2)
{
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (p->isMotion())
{
- stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
+ _stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin())
- stream << " o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ _stream << " o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
<< pm->getOrigin()->getFrame()->id() << std::endl;
if (pm->getLast())
- stream << " l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << " l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pm->getLast()->getFrame()->id() << std::endl;
if (pm->getIncoming())
- stream << " i: Cap" << pm->getIncoming()->id() << std::endl;
+ _stream << " i: Cap" << pm->getIncoming()->id() << std::endl;
}
else
{
- stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
+ _stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
if (pt)
{
if (pt->getOrigin())
- stream << " o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << " o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pt->getOrigin()->getFrame()->id() << std::endl;
if (pt->getLast())
- stream << " l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << " l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pt->getLast()->getFrame()->id() << std::endl;
if (pt->getIncoming())
- stream << " i: Cap" << pt->getIncoming()->id() << std::endl;
+ _stream << " i: Cap" << pt->getIncoming()->id() << std::endl;
}
}
} // for p
}
} // for S
}
- stream << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
- if (depth >= 1)
+ _stream << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
+ if (_depth >= 1)
{
// Frames =======================================================================================
for (auto F : getTrajectory()->getFrameList())
{
- stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (constr_by)
+ _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((_depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
+ if (_constr_by)
{
- stream << " <-- ";
+ _stream << " <-- ";
for (auto cby : F->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
- if (metric)
+ _stream << std::endl;
+ if (_metric)
{
- stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ _stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
<< F->getTimeStamp();
- stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- stream << std::endl;
+ _stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ _stream << std::endl;
}
- if (state_blocks)
+ if (_state_blocks)
{
- stream << " sb:";
+ _stream << " sb:";
for (const auto& sb : F->getStateBlockVec())
{
- stream << " " << (sb->isFixed() ? "Fix" : "Est");
+ _stream << " " << (sb->isFixed() ? "Fix" : "Est");
}
- stream << std::endl;
+ _stream << std::endl;
}
- if (depth >= 2)
+ if (_depth >= 2)
{
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+ _stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if(C->getSensor() != nullptr)
{
- stream << " -> Sen" << C->getSensor()->id();
+ _stream << " -> Sen" << C->getSensor()->id();
}
else
- stream << " -> Sen-";
+ _stream << " -> Sen-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
if (auto OC = CM->getOriginCapture())
{
- stream << " -> OCap" << OC->id();
+ _stream << " -> OCap" << OC->id();
if (auto OF = OC->getFrame())
- stream << " ; OFrm" << OF->id();
+ _stream << " ; OFrm" << OF->id();
}
}
- stream << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (constr_by)
+ _stream << ((_depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
+ if (_constr_by)
{
- stream << " <-- ";
+ _stream << " <-- ";
for (auto cby : C->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
+ _stream << std::endl;
- if (state_blocks)
+ if (_state_blocks)
for (const auto& sb : C->getStateBlockVec())
{
if(sb != nullptr)
{
- stream << " sb: ";
- stream << (sb->isFixed() ? "Fix" : "Est");
- if (metric)
- stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- stream << std::endl;
+ _stream << " sb: ";
+ _stream << (sb->isFixed() ? "Fix" : "Est");
+ if (_metric)
+ _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ _stream << std::endl;
}
}
if (C->isMotion() )
{
CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
- if ( metric && ! CM->getBuffer().get().empty())
+ _stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
+ if ( _metric && ! CM->getBuffer().get().empty())
{
- stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
+ _stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
if (CM->hasCalibration())
{
- stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
- stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
- stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
- stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
+ _stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
+ _stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
+ _stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
+ _stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
}
}
}
- if (depth >= 3)
+ if (_depth >= 3)
{
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (constr_by)
+ _stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((_depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
+ if (_constr_by)
{
- stream << " <--\t";
+ _stream << " <--\t";
for (auto cby : f->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
- if (metric)
- stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
+ _stream << std::endl;
+ if (_metric)
+ _stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
<< " )" << std::endl;
- if (depth >= 4)
+ if (_depth >= 4)
{
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- stream << " Fac" << c->id() << " " << c->getType() << " -->";
+ _stream << " Fac" << c->id() << " " << c->getType() << " -->";
if ( c->getFrameOtherList() .empty()
&& c->getCaptureOtherList() .empty()
&& c->getFeatureOtherList() .empty()
&& c->getLandmarkOtherList().empty())
- stream << " Abs";
+ _stream << " Abs";
for (const auto& Fow : c->getFrameOtherList())
if (!Fow.expired())
- stream << " Frm" << Fow.lock()->id();
+ _stream << " Frm" << Fow.lock()->id();
for (const auto& Cow : c->getCaptureOtherList())
if (!Cow.expired())
- stream << " Cap" << Cow.lock()->id();
+ _stream << " Cap" << Cow.lock()->id();
for (const auto& fow : c->getFeatureOtherList())
if (!fow.expired())
- stream << " Ftr" << fow.lock()->id();
+ _stream << " Ftr" << fow.lock()->id();
for (const auto& Low : c->getLandmarkOtherList())
if (!Low.expired())
- stream << " Lmk" << Low.lock()->id();
- stream << std::endl;
+ _stream << " Lmk" << Low.lock()->id();
+ _stream << std::endl;
} // for c
}
} // for f
@@ -1115,62 +1115,62 @@ void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric
}
} // for F
}
- stream << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
- if (depth >= 1)
+ _stream << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
+ if (_depth >= 1)
{
// Landmarks =======================================================================================
for (auto L : getMap()->getLandmarkList())
{
- stream << " Lmk" << L->id() << " " << L->getType();
- if (constr_by)
+ _stream << " Lmk" << L->id() << " " << L->getType();
+ if (_constr_by)
{
- stream << "\t<-- ";
+ _stream << "\t<-- ";
for (auto cby : L->getConstrainedByList())
- stream << "Fac" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- stream << std::endl;
- if (metric)
+ _stream << std::endl;
+ if (_metric)
{
- stream << (L->isFixed() ? " Fix" : " Est");
- stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- stream << std::endl;
+ _stream << (L->isFixed() ? " Fix" : " Est");
+ _stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
+ _stream << std::endl;
}
- if (state_blocks)
+ if (_state_blocks)
{
- stream << " sb:";
+ _stream << " sb:";
for (const auto& sb : L->getStateBlockVec())
{
if (sb != nullptr)
- stream << (sb->isFixed() ? " Fix" : " Est");
+ _stream << (sb->isFixed() ? " Fix" : " Est");
}
- stream << std::endl;
+ _stream << std::endl;
}
} // for L
}
- stream << "-----------------------------------------" << std::endl;
- stream << std::endl;
+ _stream << "-----------------------------------------" << std::endl;
+ _stream << std::endl;
}
-bool Problem::check(bool verbose, std::ostream& stream) const
+bool Problem::check(bool _verbose, std::ostream& _stream) const
{
CheckLog log(true, "");
log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
- if (verbose) stream << std::endl;
- if (verbose) stream << "Wolf tree integrity ---------------------" << std::endl;
+ if (_verbose) _stream << std::endl;
+ if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
auto P_raw = this;
- if (verbose)
+ if (_verbose)
{
- stream << "Prb @ " << P_raw << std::endl;
+ _stream << "Prb @ " << P_raw << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (verbose)
+ if (_verbose)
{
- stream << "Hrw @ " << H.get() << std::endl;
+ _stream << "Hrw @ " << H.get() << std::endl;
}
// check pointer to Problem
@@ -1183,22 +1183,22 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Sensors =======================================================================================
for (auto S : H->getSensorList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
- stream << " -> Prb @ " << S->getProblem().get() << std::endl;
- stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
+ _stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
+ _stream << " -> Prb @ " << S->getProblem().get() << std::endl;
+ _stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
for (auto pair: S->getStateBlockMap())
{
auto sb = pair.second;
- stream << " " << pair.first << " sb @ " << sb.get();
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
@@ -1216,11 +1216,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
- stream << " -> Prb @ " << p->getProblem().get() << std::endl;
- stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
+ _stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
+ _stream << " -> Prb @ " << p->getProblem().get() << std::endl;
+ _stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
}
// check problem and sensor pointers
@@ -1239,9 +1239,9 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (verbose)
+ if (_verbose)
{
- stream << "Trj @ " << T.get() << std::endl;
+ _stream << "Trj @ " << T.get() << std::endl;
}
// check pointer to Problem
@@ -1253,11 +1253,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
- if (verbose) {
- stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
+ if (_verbose) {
+ _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
<< F->id() << " @ " << F.get() << std::endl;
- stream << " -> Prb @ " << F->getProblem().get() << std::endl;
- stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
+ _stream << " -> Prb @ " << F->getProblem().get() << std::endl;
+ _stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
}
for (const auto &pair: F->getStateBlockMap()) {
@@ -1269,14 +1269,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
<< " = 0 \n";
log.addAssertion((sb.get() != 0), inconsistency_explanation);
- if (verbose) {
- stream << " "<< pair.first << " sb @ " << sb.get();
+ if (_verbose) {
+ _stream << " "<< pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
@@ -1294,11 +1294,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check constrained_by
for (auto cby : F->getConstrainedByList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- c" << cby->id() << " -> ";
for (const auto& Fow : cby->getFrameOtherList())
- stream << " F" << Fow.lock()->id() << std::endl;
+ _stream << " F" << Fow.lock()->id() << std::endl;
}
// check constrained_by pointer to this frame
@@ -1309,14 +1309,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose) {
- stream << " sb @ " << sb.get();
+ if (_verbose) {
+ _stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
}
@@ -1324,14 +1324,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
- if (C->getSensor()) stream << C->getSensor()->id();
- else stream << "-";
- stream << std::endl;
- stream << " -> Prb @ " << C->getProblem().get() << std::endl;
- stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
+ _stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
+ if (C->getSensor()) _stream << C->getSensor()->id();
+ else _stream << "-";
+ _stream << std::endl;
+ _stream << " -> Prb @ " << C->getProblem().get() << std::endl;
+ _stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
}
for (auto pair: C->getStateBlockMap())
{
@@ -1342,15 +1342,15 @@ bool Problem::check(bool verbose, std::ostream& stream) const
<< sb.get() << " = 0 \n";
log.addAssertion((sb.get() != 0), inconsistency_explanation);
- if (verbose)
+ if (_verbose)
{
- stream << " " << pair.first << " sb @ " << sb.get();
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
@@ -1368,12 +1368,12 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check contrained_by
for (const auto& cby : C->getConstrainedByList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- c" << cby->id() << " -> ";
for (const auto& Cow : cby->getCaptureOtherList())
- stream << " C" << Cow.lock()->id();
- stream << std::endl;
+ _stream << " C" << Cow.lock()->id();
+ _stream << std::endl;
}
// check constrained_by pointer to this capture
@@ -1383,16 +1383,16 @@ bool Problem::check(bool verbose, std::ostream& stream) const
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose)
+ if (_verbose)
{
- stream << " sb @ " << sb.get();
+ _stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
@@ -1401,11 +1401,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
- stream << " -> Prb @ " << f->getProblem().get() << std::endl;
- stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
+ _stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
+ _stream << " -> Prb @ " << f->getProblem().get() << std::endl;
+ _stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
<< std::endl;
}
@@ -1423,12 +1423,12 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// check contrained_by
for (auto cby : f->getConstrainedByList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- c" << cby->id() << " -> ";
for (const auto& fow : cby->getFeatureOtherList())
- stream << " f" << fow.lock()->id();
- stream << std::endl;
+ _stream << " f" << fow.lock()->id();
+ _stream << std::endl;
}
// check constrained_by pointer to this feature
@@ -1441,16 +1441,16 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- if (verbose)
- stream << " Fac" << c->id() << " @ " << c.get();
+ if (_verbose)
+ _stream << " Fac" << c->id() << " @ " << c.get();
if ( c->getFrameOtherList() .empty()
&& c->getCaptureOtherList() .empty()
&& c->getFeatureOtherList() .empty()
&& c->getLandmarkOtherList().empty() ) // case ABSOLUTE:
{
- if (verbose)
- stream << " --> Abs.";
+ if (_verbose)
+ _stream << " --> Abs.";
}
// find constrained_by pointer in constrained frame
@@ -1459,11 +1459,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
if (!Fow.expired())
{
const auto& Fo = Fow.lock();
- if (verbose)
+ if (_verbose)
{
- stream << " ( --> F" << Fo->id() << " <- ";
+ _stream << " ( --> F" << Fo->id() << " <- ";
for (auto cby : Fo->getConstrainedByList())
- stream << " c" << cby->id();
+ _stream << " c" << cby->id();
}
// check constrained_by pointer in constrained frame
@@ -1474,8 +1474,8 @@ bool Problem::check(bool verbose, std::ostream& stream) const
//Clear inconsistency_explanation
}
}
- if (verbose && !c->getFrameOtherList().empty())
- stream << ")";
+ if (_verbose && !c->getFrameOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained capture
for (const auto& Cow : c->getCaptureOtherList())
@@ -1484,11 +1484,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
{
const auto& Co = Cow.lock();
- if (verbose)
+ if (_verbose)
{
- stream << " ( --> C" << Co->id() << " <- ";
+ _stream << " ( --> C" << Co->id() << " <- ";
for (auto cby : Co->getConstrainedByList())
- stream << " c" << cby->id();
+ _stream << " c" << cby->id();
}
// check constrained_by pointer in constrained frame
@@ -1499,8 +1499,8 @@ bool Problem::check(bool verbose, std::ostream& stream) const
//Clear inconsistency_explanation
}
}
- if (verbose && !c->getCaptureOtherList().empty())
- stream << ")";
+ if (_verbose && !c->getCaptureOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained feature
for (const auto& fow : c->getFeatureOtherList())
@@ -1508,11 +1508,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
if (!fow.expired())
{
const auto& fo = fow.lock();
- if (verbose)
+ if (_verbose)
{
- stream << " ( --> f" << fo->id() << " <- ";
+ _stream << " ( --> f" << fo->id() << " <- ";
for (auto cby : fo->getConstrainedByList())
- stream << " c" << cby->id();
+ _stream << " c" << cby->id();
}
// check constrained_by pointer in constrained feature
@@ -1522,8 +1522,8 @@ bool Problem::check(bool verbose, std::ostream& stream) const
log.addAssertion((found), inconsistency_explanation);
}
}
- if (verbose && !c->getFeatureOtherList().empty())
- stream << ")";
+ if (_verbose && !c->getFeatureOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained landmark
for (const auto& Low : c->getLandmarkOtherList())
@@ -1532,11 +1532,11 @@ bool Problem::check(bool verbose, std::ostream& stream) const
{
const auto& Lo = Low.lock();
- if (verbose)
+ if (_verbose)
{
- stream << " ( --> L" << Lo->id() << " <- ";
+ _stream << " ( --> L" << Lo->id() << " <- ";
for (auto cby : Lo->getConstrainedByList())
- stream << " c" << cby->id();
+ _stream << " c" << cby->id();
}
// check constrained_by pointer in constrained landmark
@@ -1547,16 +1547,16 @@ bool Problem::check(bool verbose, std::ostream& stream) const
//Clear inconsistency_explanation
}
}
- if (verbose && !c->getLandmarkOtherList().empty())
- stream << ")";
+ if (_verbose && !c->getLandmarkOtherList().empty())
+ _stream << ")";
- if (verbose)
- stream << std::endl;
+ if (_verbose)
+ _stream << std::endl;
- if (verbose)
+ if (_verbose)
{
- stream << " -> Prb @ " << c->getProblem().get() << std::endl;
- stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
+ _stream << " -> Prb @ " << c->getProblem().get() << std::endl;
+ _stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
}
// check problem and feature pointers
@@ -1575,26 +1575,26 @@ bool Problem::check(bool verbose, std::ostream& stream) const
for (auto sb : c->getStateBlockPtrVector())
{
bool found = false;
- if (verbose)
+ if (_verbose)
{
- stream << " sb @ " << sb.get();
+ _stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
}
bool found_here;
// find in own Frame
found_here = F->hasStateBlock(sb);
- if (found_here && verbose) stream << " F" << F->id();
+ if (found_here && _verbose) _stream << " F" << F->id();
found = found || found_here;
// find in own Capture
found_here = C->hasStateBlock(sb);
- if (found_here && verbose) stream << " C" << C->id();
+ if (found_here && _verbose) _stream << " C" << C->id();
found = found || found_here;
// Find in other Captures of the own Frame
@@ -1602,7 +1602,7 @@ bool Problem::check(bool verbose, std::ostream& stream) const
for (auto FC : F->getCaptureList())
{
found_here = FC->hasStateBlock(sb);
- if (found_here && verbose) stream << " F" << F->id() << ".C" << FC->id();
+ if (found_here && _verbose) _stream << " F" << F->id() << ".C" << FC->id();
found = found || found_here;
}
@@ -1610,7 +1610,7 @@ bool Problem::check(bool verbose, std::ostream& stream) const
if (S)
{
found_here = S->hasStateBlock(sb);
- if (found_here && verbose) stream << " S" << S->id();
+ if (found_here && _verbose) _stream << " S" << S->id();
found = found || found_here;
}
@@ -1622,14 +1622,14 @@ bool Problem::check(bool verbose, std::ostream& stream) const
{
const auto& Fo = Fow.lock();
found_here = Fo->hasStateBlock(sb);
- if (found_here && verbose) stream << " Fo" << Fo->id();
+ if (found_here && _verbose) _stream << " Fo" << Fo->id();
found = found || found_here;
// find in feature other's captures
for (auto FoC : Fo->getCaptureList())
{
found_here = FoC->hasStateBlock(sb);
- if (found_here && verbose) stream << " Fo" << Fo->id() << ".C" << FoC->id();
+ if (found_here && _verbose) _stream << " Fo" << Fo->id() << ".C" << FoC->id();
found = found || found_here;
}
@@ -1643,7 +1643,7 @@ bool Problem::check(bool verbose, std::ostream& stream) const
{
const auto& Co = Cow.lock();
found_here = Co->hasStateBlock(sb);
- if (found_here && verbose) stream << " Co" << Co->id();
+ if (found_here && _verbose) _stream << " Co" << Co->id();
found = found || found_here;
}
}
@@ -1657,19 +1657,19 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// find in constrained feature's Frame
auto foF = fo->getFrame();
found_here = foF->hasStateBlock(sb);
- if (found_here && verbose) stream << " foF" << foF->id();
+ if (found_here && _verbose) _stream << " foF" << foF->id();
found = found || found_here;
// find in constrained feature's Capture
auto foC = fo->getCapture();
found_here = foC->hasStateBlock(sb);
- if (found_here && verbose) stream << " foC" << foC->id();
+ if (found_here && _verbose) _stream << " foC" << foC->id();
found = found || found_here;
// find in constrained feature's Sensor
auto foS = fo->getCapture()->getSensor();
found_here = foS->hasStateBlock(sb);
- if (found_here && verbose) stream << " foS" << foS->id();
+ if (found_here && _verbose) _stream << " foS" << foS->id();
found = found || found_here;
}
}
@@ -1681,18 +1681,18 @@ bool Problem::check(bool verbose, std::ostream& stream) const
{
const auto& Lo = Low.lock();
found_here = Lo->hasStateBlock(sb);
- if (found_here && verbose) stream << " Lo" << Lo->id();
+ if (found_here && _verbose) _stream << " Lo" << Lo->id();
found = found || found_here;
}
}
- if (verbose)
+ if (_verbose)
{
if (found)
- stream << " found";
+ _stream << " found";
else
- stream << " NOT FOUND !";
- stream << std::endl;
+ _stream << " NOT FOUND !";
+ _stream << std::endl;
}
// check that the state block has been found somewhere
@@ -1710,8 +1710,8 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (verbose)
- stream << "Map @ " << M.get() << std::endl;
+ if (_verbose)
+ _stream << "Map @ " << M.get() << std::endl;
// check pointer to Problem
inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
@@ -1720,22 +1720,22 @@ bool Problem::check(bool verbose, std::ostream& stream) const
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
- stream << " -> Prb @ " << L->getProblem().get() << std::endl;
- stream << " -> Map @ " << L->getMap().get() << std::endl;
+ _stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
+ _stream << " -> Prb @ " << L->getProblem().get() << std::endl;
+ _stream << " -> Map @ " << L->getMap().get() << std::endl;
for (const auto& pair : L->getStateBlockMap())
{
auto sb = pair.second;
- stream << " " << pair.first << " sb @ " << sb.get();
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
@@ -1753,19 +1753,19 @@ bool Problem::check(bool verbose, std::ostream& stream) const
for (auto cby : L->getConstrainedByList())
{
- if (verbose)
+ if (_verbose)
{
- stream << " <- Fac" << cby->id() << " ->";
+ _stream << " <- Fac" << cby->id() << " ->";
for (const auto& Low : cby->getLandmarkOtherList())
{
if (!Low.expired())
{
const auto& Lo = Low.lock();
- stream << " Lmk" << Lo->id();
+ _stream << " Lmk" << Lo->id();
}
}
- stream << std::endl;
+ _stream << std::endl;
}
// check constrained-by factors
@@ -1774,29 +1774,29 @@ bool Problem::check(bool verbose, std::ostream& stream) const
log.addAssertion((cby->hasLandmarkOther(L)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector()) {
- if (verbose) {
- stream << " sb @ " << sb.get();
+ if (_verbose) {
+ _stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- stream << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- stream << std::endl;
+ _stream << std::endl;
}
}
}
}
- if (verbose) stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
- if (verbose) stream << std::endl;
- if (verbose and not log.is_consistent_) stream << log.explanation_ << std::endl;
+ if (_verbose) _stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
+ if (_verbose) _stream << std::endl;
+ if (_verbose and not log.is_consistent_) _stream << log.explanation_ << std::endl;
return log.is_consistent_;
}
-bool Problem::check(int verbose_level) const
+bool Problem::check(int _verbose_level) const
{
- return check((verbose_level > 0), std::cout);
+ return check((_verbose_level > 0), std::cout);
}
void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
{