diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index b9befc35a036142b87eb48ccdf9973fc16d818b9..d7133cddc09f99aad05d0ada7c35ffbe88513c25 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -325,6 +325,11 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
// Print and check ---------------------------------------
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
@@ -340,11 +345,8 @@ class Problem : public std::enable_shared_from_this<Problem>
bool constr_by = false, //
bool metric = true, //
bool state_blocks = false) const;
- std::string printToString(int depth = 4, //
- bool constr_by = false, //
- bool metric = true, //
- bool state_blocks = false) const;
- bool check(int verbose_level = 0) const;
+ bool check(int verbose_level) const;
+ bool check(bool verbose, std::ostream& stream) const;
};
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 8ba9bd9acab0ef571d8859b361b369428ddeba4f..8a694beb4c8145d5d7253f0c8687ce5f6e45d82c 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -872,55 +872,53 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int depth, 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
{
- using std::cout;
- using std::endl;
- cout << endl;
- cout << "P: wolf tree status ---------------------" << endl;
- cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
+ 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())
{
- cout << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
+ stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
if (depth < 2)
- cout << " -- " << S->getProcessorList().size() << "p";
- cout << endl;
+ stream << " -- " << S->getProcessorList().size() << "p";
+ stream << std::endl;
if (metric && state_blocks)
{
- cout << " sb: ";
+ stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << 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() << " ); ";
}
- cout << endl;
+ stream << std::endl;
}
else if (metric)
{
- cout << " ( ";
+ stream << " ( ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << sb->getState().transpose() << " ";
+ stream << sb->getState().transpose() << " ";
}
- cout << ")" << endl;
+ stream << ")" << std::endl;
}
else if (state_blocks)
{
- cout << " sb: ";
+ stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
- cout << endl;
+ stream << std::endl;
}
if (depth >= 2)
{
@@ -929,126 +927,126 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
{
if (p->isMotion())
{
- std::cout << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin())
- cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
- << pm->getOrigin()->getFrame()->id() << endl;
+ stream << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ << pm->getOrigin()->getFrame()->id() << std::endl;
if (pm->getLast())
- cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pm->getLast()->getFrame()->id() << endl;
+ stream << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pm->getLast()->getFrame()->id() << std::endl;
if (pm->getIncoming())
- cout << " i: C" << pm->getIncoming()->id() << endl;
+ stream << " i: C" << pm->getIncoming()->id() << std::endl;
}
else
{
- cout << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
if (pt)
{
if (pt->getOrigin())
- cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pt->getOrigin()->getFrame()->id() << endl;
+ stream << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pt->getOrigin()->getFrame()->id() << std::endl;
if (pt->getLast())
- cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pt->getLast()->getFrame()->id() << endl;
+ stream << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pt->getLast()->getFrame()->id() << std::endl;
if (pt->getIncoming())
- cout << " i: C" << pt->getIncoming()->id() << endl;
+ stream << " i: C" << pt->getIncoming()->id() << std::endl;
}
}
} // for p
}
} // for S
}
- cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
+ stream << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
if (depth >= 1)
{
// Frames =======================================================================================
for (auto F : getTrajectory()->getFrameList())
{
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
+ stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
if (constr_by)
{
- cout << " <-- ";
+ stream << " <-- ";
for (auto cby : F->getConstrainedByList())
- cout << "Fac" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (metric)
{
- cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
<< F->getTimeStamp();
- cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- cout << endl;
+ stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ stream << std::endl;
}
if (state_blocks)
{
- cout << " sb:";
+ stream << " sb:";
for (const auto& sb : F->getStateBlockVec())
{
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ stream << " " << (sb->isFixed() ? "Fix" : "Est");
}
- cout << endl;
+ stream << std::endl;
}
if (depth >= 2)
{
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- cout << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+ stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if(C->getSensor() != nullptr)
{
- cout << " -> Sen" << C->getSensor()->id();
+ stream << " -> Sen" << C->getSensor()->id();
}
else
- cout << " -> S-";
+ stream << " -> S-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
if (auto OC = CM->getOriginCapture())
{
- cout << " -> OCap" << OC->id();
+ stream << " -> OCap" << OC->id();
if (auto OF = OC->getFrame())
- cout << " ; OFrm" << OF->id();
+ stream << " ; OFrm" << OF->id();
}
}
- cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
+ stream << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
if (constr_by)
{
- cout << " <-- ";
+ stream << " <-- ";
for (auto cby : C->getConstrainedByList())
- cout << "Fac" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (state_blocks)
for (const auto& sb : C->getStateBlockVec())
{
if(sb != nullptr)
{
- cout << " sb: ";
- cout << (sb->isFixed() ? "Fix" : "Est");
+ stream << " sb: ";
+ stream << (sb->isFixed() ? "Fix" : "Est");
if (metric)
- cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- cout << endl;
+ stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ stream << std::endl;
}
}
if (C->isMotion() )
{
CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- cout << " buffer size : " << CM->getBuffer().get().size() << endl;
+ stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
if ( metric && ! CM->getBuffer().get().empty())
{
- cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
+ stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
if (CM->hasCalibration())
{
- cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
- cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
- cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
- cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << 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;
}
}
}
@@ -1058,34 +1056,34 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- cout << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
+ stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
if (constr_by)
{
- cout << " <--\t";
+ stream << " <--\t";
for (auto cby : f->getConstrainedByList())
- cout << "Fac" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (metric)
- cout << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << endl;
+ stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
+ << " )" << std::endl;
if (depth >= 4)
{
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- cout << " Fac" << c->id() << " " << c->getType() << " -->";
+ stream << " Fac" << c->id() << " " << c->getType() << " -->";
if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- cout << " Abs";
+ stream << " Abs";
if (c->getFrameOther() != nullptr)
- cout << " Frm" << c->getFrameOther()->id();
+ stream << " Frm" << c->getFrameOther()->id();
if (c->getCaptureOther() != nullptr)
- cout << " Cap" << c->getCaptureOther()->id();
+ stream << " Cap" << c->getCaptureOther()->id();
if (c->getFeatureOther() != nullptr)
- cout << " Fac" << c->getFeatureOther()->id();
+ stream << " Fac" << c->getFeatureOther()->id();
if (c->getLandmarkOther() != nullptr)
- cout << " Lmk" << c->getLandmarkOther()->id();
- cout << endl;
+ stream << " Lmk" << c->getLandmarkOther()->id();
+ stream << std::endl;
} // for c
}
} // for f
@@ -1094,320 +1092,62 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
}
} // for F
}
- cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
+ stream << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
if (depth >= 1)
{
// Landmarks =======================================================================================
for (auto L : getMap()->getLandmarkList())
{
- cout << " Lmk" << L->id() << " " << L->getType();
+ stream << " Lmk" << L->id() << " " << L->getType();
if (constr_by)
{
- cout << "\t<-- ";
+ stream << "\t<-- ";
for (auto cby : L->getConstrainedByList())
- cout << "Fac" << cby->id() << " \t";
+ stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ stream << std::endl;
if (metric)
{
- cout << (L->isFixed() ? " Fix" : " Est");
- cout << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- cout << endl;
+ stream << (L->isFixed() ? " Fix" : " Est");
+ stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
+ stream << std::endl;
}
if (state_blocks)
{
- cout << " sb:";
+ stream << " sb:";
for (const auto& sb : L->getStateBlockVec())
{
if (sb != nullptr)
- cout << (sb->isFixed() ? " Fix" : " Est");
+ stream << (sb->isFixed() ? " Fix" : " Est");
}
- cout << endl;
+ stream << std::endl;
}
} // for L
}
- cout << "-----------------------------------------" << endl;
- cout << endl;
+ stream << "-----------------------------------------" << std::endl;
+ stream << std::endl;
}
-std::string Problem::printToString(int depth, bool constr_by, bool metric, bool state_blocks) const
-{
- using std::cout;
- using std::endl;
- std::stringstream result;
-
- result << endl;
- result << "P: wolf tree status ---------------------" << endl;
- result << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
- if (depth >= 1)
- {
- // Sensors =======================================================================================
- for (auto S : getHardware()->getSensorList())
- {
- result << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (depth < 2)
- result << " -- " << S->getProcessorList().size() << "p";
- result << endl;
- if (metric && state_blocks)
- {
- result << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
- }
- result << endl;
- }
- else if (metric)
- {
- result << " ( ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << sb->getState().transpose() << " ";
- }
- result << ")" << endl;
- }
- else if (state_blocks)
- {
- result << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
- }
- result << endl;
- }
-
-
- if (depth >= 2)
- {
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (p->isMotion())
- {
- result << " pm" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
- ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOrigin())
- result << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
- << pm->getOrigin()->getFrame()->id() << endl;
- if (pm->getLast())
- result << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pm->getLast()->getFrame()->id() << endl;
- if (pm->getIncoming())
- result << " i: C" << pm->getIncoming()->id() << endl;
- }
- else
- {
- result << " pt" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
- ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
- if (pt)
- {
- if (pt->getOrigin())
- result << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getOrigin()->getFrame()->id() << endl;
- if (pt->getLast())
- result << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getLast()->getFrame()->id() << endl;
- if (pt->getIncoming())
- result << " i: C" << pt->getIncoming()->id() << endl;
- }
- }
- } // for p
- }
- } // for S
- }
- result << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
- if (depth >= 1)
- {
- // Frames =======================================================================================
- for (auto F : getTrajectory()->getFrameList())
- {
- result << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (constr_by)
- {
- result << " <-- ";
- for (auto cby : F->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- {
- result << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp().get();
- result << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- result << endl;
- }
- if (state_blocks)
- {
- result << " sb:";
- for (auto sb : F->getStateBlockVec())
- if (sb != nullptr)
- result << " " << (sb->isFixed() ? "Fix" : "Est");
- result << endl;
- }
- if (depth >= 2)
- {
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- result << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
-
- if(C->getSensor() != nullptr)
- {
- result << " -> S" << C->getSensor()->id();
- }
- else
- result << " -> S-";
- if (C->isMotion())
- {
- auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (auto OC = CM->getOriginCapture())
- {
- result << " -> OC" << OC->id();
- if (auto OF = OC->getFrame())
- result << " ; OF" << OF->id();
- }
- }
-
- result << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (constr_by)
- {
- result << " <-- ";
- for (auto cby : C->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
-
- if (state_blocks)
- for(auto sb : C->getStateBlockVec())
- if(sb != nullptr)
- {
- result << " sb: ";
- result << (sb->isFixed() ? "Fix" : "Est");
- if (metric)
- result << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- result << endl;
- }
-
- if (C->isMotion() )
- {
- CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- result << " buffer size : " << CM->getBuffer().get().size() << endl;
- if ( metric && ! CM->getBuffer().get().empty())
- {
- result << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
- if (CM->hasCalibration())
- {
- result << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
- result << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
- result << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
- result << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
- }
- }
- }
-
- if (depth >= 3)
- {
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- result << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (constr_by)
- {
- result << " <--\t";
- for (auto cby : f->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- result << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << endl;
- if (depth >= 4)
- {
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- result << " c" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- result << " A";
- if (c->getFrameOther() != nullptr)
- result << " F" << c->getFrameOther()->id();
- if (c->getCaptureOther() != nullptr)
- result << " C" << c->getCaptureOther()->id();
- if (c->getFeatureOther() != nullptr)
- result << " f" << c->getFeatureOther()->id();
- if (c->getLandmarkOther() != nullptr)
- result << " L" << c->getLandmarkOther()->id();
- result << endl;
- } // for c
- }
- } // for f
- }
- } // for C
- }
- } // for F
- }
- result << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
- if (depth >= 1)
- {
- // Landmarks =======================================================================================
- for (auto L : getMap()->getLandmarkList())
- {
- result << " L" << L->id() << " " << L->getType();
- if (constr_by)
- {
- result << "\t<-- ";
- for (auto cby : L->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- {
- result << (L->isFixed() ? " Fix" : " Est");
- result << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- result << endl;
- }
- if (state_blocks)
- {
- result << " sb:";
- for (auto sb : L->getStateBlockVec())
- if (sb != nullptr)
- result << (sb->isFixed() ? " Fix" : " Est");
- result << endl;
- }
- } // for L
- }
- result << "-----------------------------------------" << endl;
- result << endl;
- return result.str();
-}
-bool Problem::check(int verbose_level) const
+bool Problem::check(bool verbose, std::ostream& stream) const
{
- using std::cout;
- using std::endl;
CheckLog log(true, "");
log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
- if (verbose_level) cout << endl;
- if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl;
+ if (verbose) stream << std::endl;
+ if (verbose) stream << "Wolf tree integrity ---------------------" << std::endl;
auto P_raw = this;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "Prb @ " << P_raw << endl;
+ stream << "Prb @ " << P_raw << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "Hrw @ " << H.get() << endl;
+ stream << "Hrw @ " << H.get() << std::endl;
}
// check pointer to Problem
// is_consistent = is_consistent && (H->getProblem().get() == P_raw);
@@ -1422,22 +1162,22 @@ bool Problem::check(int verbose_level) const
// Sensors =======================================================================================
for (auto S : H->getSensorList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " Sen" << S->id() << " @ " << S.get() << endl;
- cout << " -> Prb @ " << S->getProblem().get() << endl;
- cout << " -> Hrw @ " << S->getHardware().get() << 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;
- cout << " " << pair.first << " sb @ " << sb.get();
+ stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
// check problem and hardware pointers
@@ -1458,11 +1198,11 @@ bool Problem::check(int verbose_level) const
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << endl;
- cout << " -> Prb @ " << p->getProblem().get() << endl;
- cout << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << 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
@@ -1487,9 +1227,9 @@ bool Problem::check(int verbose_level) const
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "Trj @ " << T.get() << endl;
+ stream << "Trj @ " << T.get() << std::endl;
}
// check pointer to Problem
// is_consistent = is_consistent && (T->getProblem().get() == P_raw);
@@ -1504,11 +1244,11 @@ bool Problem::check(int verbose_level) const
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
- if (verbose_level > 0) {
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
- << F->id() << " @ " << F.get() << endl;
- cout << " -> Prb @ " << F->getProblem().get() << endl;
- cout << " -> Trj @ " << F->getTrajectory().get() << endl;
+ 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;
}
for (const auto &pair: F->getStateBlockMap()) {
auto sb = pair.second;
@@ -1519,14 +1259,14 @@ bool Problem::check(int verbose_level) const
CheckLog((sb.get() != 0), inconsistency_explanation.str()));
// Clear inconsistency_explanation
std::stringstream().swap(inconsistency_explanation);
- if (verbose_level > 0) {
- cout << " "<< pair.first << " sb @ " << sb.get();
+ if (verbose) {
+ stream << " "<< pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
@@ -1558,9 +1298,9 @@ bool Problem::check(int verbose_level) const
// Clear inconsistency_explanation
std::stringstream().swap(inconsistency_explanation);
}
- if (verbose_level > 0) {
- cout << " <- c" << cby->id() << " -> Frm"
- << cby->getFrameOther()->id() << endl;
+ if (verbose) {
+ stream << " <- c" << cby->id() << " -> Frm"
+ << cby->getFrameOther()->id() << std::endl;
}
// check constrained_by pointer to this frame
// is_consistent = is_consistent && (cby->getFrameOther() == F);
@@ -1574,14 +1314,14 @@ bool Problem::check(int verbose_level) const
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose_level > 0) {
- cout << " sb @ " << sb.get();
+ if (verbose) {
+ stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
}
@@ -1589,27 +1329,27 @@ bool Problem::check(int verbose_level) const
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
- if (C->getSensor()) cout << C->getSensor()->id();
- else cout << "-";
- cout << endl;
- cout << " -> Prb @ " << C->getProblem().get() << endl;
- cout << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << 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())
{
auto sb = pair.second;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " " << pair.first << " sb @ " << sb.get();
+ stream << " " << pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
@@ -1633,12 +1373,12 @@ bool Problem::check(int verbose_level) const
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " Ftr" << f->id() << " @ " << f.get() << endl;
- cout << " -> Prb @ " << f->getProblem().get() << endl;
- cout << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
- << endl;
+ 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;
}
// check problem and capture pointers
// is_consistent = is_consistent && (f->getProblem().get() == P_raw);
@@ -1659,9 +1399,9 @@ bool Problem::check(int verbose_level) const
for (auto cby : f->getConstrainedByList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " <- c" << cby->id() << " -> Ftr" << cby->getFeatureOther()->id() << endl;
+ stream << " <- c" << cby->id() << " -> Ftr" << cby->getFeatureOther()->id() << std::endl;
}
// check constrained_by pointer to this feature
// is_consistent = is_consistent && (cby->getFeatureOther() == f);
@@ -1676,8 +1416,8 @@ bool Problem::check(int verbose_level) const
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- if (verbose_level > 0)
- cout << " Fac" << c->id() << " @ " << c.get();
+ if (verbose)
+ stream << " Fac" << c->id() << " @ " << c.get();
auto Fo = c->getFrameOther();
auto Co = c->getCaptureOther();
@@ -1686,24 +1426,24 @@ bool Problem::check(int verbose_level) const
if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
{
- if (verbose_level > 0)
- cout << " --> Abs." << endl;
+ if (verbose)
+ stream << " --> Abs." << std::endl;
}
// find constrained_by pointer in constrained frame
if ( Fo ) // case FRAME:
{
- if (verbose_level > 0)
- cout << " ( --> Frm" << Fo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Frm" << Fo->id() << " <- ";
bool found = false;
for (auto cby : Fo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " Fac" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << ")";
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained frame
// is_consistent = is_consistent && found;
@@ -1718,17 +1458,17 @@ bool Problem::check(int verbose_level) const
// find constrained_by pointer in constrained capture
if ( Co ) // case CAPTURE:
{
- if (verbose_level > 0)
- cout << " ( --> Cap" << Co->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Cap" << Co->id() << " <- ";
bool found = false;
for (auto cby : Co->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " Fac" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << ")";
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained frame
// is_consistent = is_consistent && found;
@@ -1743,17 +1483,17 @@ bool Problem::check(int verbose_level) const
// find constrained_by pointer in constrained feature
if ( fo ) // case FEATURE:
{
- if (verbose_level > 0)
- cout << " ( --> Ftr" << fo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Ftr" << fo->id() << " <- ";
bool found = false;
for (auto cby : fo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " Fac" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << ")";
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained feature
// is_consistent = is_consistent && found;
@@ -1767,17 +1507,17 @@ bool Problem::check(int verbose_level) const
// find constrained_by pointer in constrained landmark
if ( Lo ) // case LANDMARK:
{
- if (verbose_level > 0)
- cout << " ( --> Lmk" << Lo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Lmk" << Lo->id() << " <- ";
bool found = false;
for (auto cby : Lo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " Fac" << cby->id();
+ if (verbose)
+ stream << " Fac" << cby->id();
found = found || (c == cby);
}
- if (verbose_level > 0)
- cout << ")";
+ if (verbose)
+ stream << ")";
// check constrained_by pointer in constrained landmark
// is_consistent = is_consistent && found;
@@ -1787,13 +1527,13 @@ bool Problem::check(int verbose_level) const
//Clear inconsistency_explanation
std::stringstream().swap(inconsistency_explanation);
}
- if (verbose_level > 0)
- cout << endl;
+ if (verbose)
+ stream << std::endl;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " -> Prb @ " << c->getProblem().get() << endl;
- cout << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl;
+ 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);
@@ -1817,119 +1557,119 @@ bool Problem::check(int verbose_level) const
for (auto sb : c->getStateBlockPtrVector())
{
bool found = false;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " sb @ " << sb.get();
+ stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
}
bool found_here;
std::vector<StateBlockPtr> sb_vec;
// find in own Frame
- sb_vec = F->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " F" << F->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) cout << " C" << C->id();
- found = found || found_here;
+ 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) cout << " F" << F->id() << ".C" << FC->id();
- found = found || 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;
+ }
// find in own Sensor
if (S)
{
- sb_vec = S->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " S" << S->id();
- found = found || found_here;
+ sb_vec = S->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " S" << S->id();
+ found = found || found_here;
}
// find in constrained Frame
if (Fo){
- sb_vec = Fo->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " Fo" << Fo->id();
- found = found || found_here;
+ sb_vec = Fo->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " Fo" << Fo->id();
+ found = found || found_here;
}
// find in constrained Capture
if (Co)
{
- sb_vec = Co->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " Co" << Co->id();
- found = found || found_here;
+ sb_vec = Co->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " Co" << Co->id();
+ found = found || found_here;
}
// Find in other Captures of the constrained Frame
if (!found_here && Fo)
for (auto FoC : Fo->getCaptureList())
{
- sb_vec = FoC->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " Fo" << Fo->id() << ".C" << FoC->id();
- found = found || found_here;
+ sb_vec = FoC->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " Fo" << Fo->id() << ".C" << FoC->id();
+ found = found || found_here;
}
// find in constrained Feature
if (fo)
{
// find in constrained feature's Frame
- auto foF = fo->getFrame();
- sb_vec = foF->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " foF" << foF->id();
- found = found || found_here;
+ auto foF = fo->getFrame();
+ sb_vec = foF->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " foF" << foF->id();
+ found = found || found_here;
// find in constrained feature's Capture
- auto foC = fo->getCapture();
- sb_vec = foC->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " foC" << foC->id();
- found = found || found_here;
+ auto foC = fo->getCapture();
+ sb_vec = foC->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " foC" << foC->id();
+ found = found || found_here;
// find in constrained feature's Sensor
- auto foS = fo->getCapture()->getSensor();
- sb_vec = foS->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " foS" << foS->id();
- found = found || found_here;
+ auto foS = fo->getCapture()->getSensor();
+ sb_vec = foS->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " foS" << foS->id();
+ found = found || found_here;
}
// find in constrained landmark
if (Lo)
{
- sb_vec = Lo->getStateBlockVec();
- found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
- if (found_here) cout << " Lo" << Lo->id();
- found = found || found_here;
+ sb_vec = Lo->getStateBlockVec();
+ found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ if (found_here) stream << " Lo" << Lo->id();
+ found = found || found_here;
}
- if (verbose_level > 0)
+ if (verbose)
{
if (found)
- cout << " found";
+ stream << " found";
else
- cout << " NOT FOUND !";
- cout << endl;
+ stream << " NOT FOUND !";
+ stream << std::endl;
}
inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
@@ -1953,8 +1693,8 @@ bool Problem::check(int verbose_level) const
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (verbose_level > 0)
- cout << "Map @ " << M.get() << endl;
+ if (verbose)
+ stream << "Map @ " << M.get() << std::endl;
// check pointer to Problem
// is_consistent = is_consistent && (M->getProblem().get() == P_raw);
@@ -1966,22 +1706,22 @@ bool Problem::check(int verbose_level) const
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " Lmk" << L->id() << " @ " << L.get() << endl;
- cout << " -> Prb @ " << L->getProblem().get() << endl;
- cout << " -> Map @ " << L->getMap().get() << 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;
- cout << " " << pair.first << " sb @ " << sb.get();
+ stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
// check problem and map pointers
@@ -2005,9 +1745,9 @@ bool Problem::check(int verbose_level) const
std::stringstream().swap(inconsistency_explanation);
for (auto cby : L->getConstrainedByList()) {
- if (verbose_level > 0)
- cout << " <- Fac" << cby->id() << " -> Lmk"
- << cby->getLandmarkOther()->id() << endl;
+ if (verbose)
+ stream << " <- Fac" << cby->id() << " -> Lmk"
+ << cby->getLandmarkOther()->id() << std::endl;
// check constrained_by pointer to this landmark
// is_consistent =
// is_consistent && (cby->getLandmarkOther() &&
@@ -2022,26 +1762,34 @@ bool Problem::check(int verbose_level) const
std::stringstream().swap(inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector()) {
- if (verbose_level > 0) {
- cout << " sb @ " << sb.get();
+ if (verbose) {
+ stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ stream << std::endl;
}
}
}
}
- if (verbose_level) cout << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << endl;
- if (verbose_level) cout << endl;
- if (verbose_level and not log.is_consistent_) cout << log.explanation_ << 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
+{
+ return check((verbose_level > 0), std::cout);
+}
+void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
+{
+ print(depth, std::cout, constr_by, metric, state_blocks);
+}
void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
{
if (depth.compare("T") == 0)