diff --git a/CMakeLists.txt b/CMakeLists.txt
index 89416119d98d9251a6f2e1d4b8385a1f9044c932..fb83ccf5399baef48a2267f0657cf68d8bbd60db 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -181,6 +181,7 @@ SET(HDRS_MATH
include/core/math/covariance.h
)
SET(HDRS_UTILS
+ include/core/utils/check_log.hpp
include/core/utils/converter.h
include/core/utils/eigen_assert.h
include/core/utils/eigen_predicates.h
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 4f521c1155b33c1ab5afbf3aab67a3b38312dfd5..63674826d08a026767aeb19a62803b46625b0d2b 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -330,6 +330,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 )
@@ -345,11 +350,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;
};
@@ -394,7 +396,8 @@ inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
return factor_notification_map_.size();
}
-
} // namespace wolf
+
+
#endif // PROBLEM_H
diff --git a/include/core/utils/check_log.hpp b/include/core/utils/check_log.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..db3ab2dc9dd6e165c3541544bf7770022568f2fe
--- /dev/null
+++ b/include/core/utils/check_log.hpp
@@ -0,0 +1,32 @@
+#ifndef CHECK_LOG_HPP
+#define CHECK_LOG_HPP
+#include <iostream>
+#include <string>
+
+class CheckLog {
+
+public:
+
+ bool is_consistent_;
+ std::string explanation_;
+
+ CheckLog() {
+ is_consistent_ = true;
+ explanation_ = "";
+ }
+ CheckLog(bool consistent, std::string explanation) {
+ is_consistent_ = consistent;
+ if (not consistent)
+ explanation_ = explanation;
+ else
+ explanation_ = "";
+ }
+ ~CheckLog(){};
+ void compose(CheckLog l) {
+
+ CheckLog result_log;
+ is_consistent_ = is_consistent_ and l.is_consistent_;
+ explanation_ = explanation_ + l.explanation_;
+ }
+};
+#endif
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c21ffe9b1092b3bbf8bbc024b50342e9e92fca0c..51c3ccd8e40ddcc2df2bfa35ed22e7a9af27e973 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -14,6 +14,7 @@
#include "core/utils/logging.h"
#include "core/utils/params_server.hpp"
#include "core/utils/loader.hpp"
+#include "core/utils/check_log.hpp"
// IRI libs includes
@@ -183,7 +184,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXd& _extrinsics, //
+
+ const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename)
{
@@ -885,55 +887,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 << " S" << 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)
{
@@ -942,126 +942,126 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
{
if (p->isMotion())
{
- std::cout << " pm" << 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() ? " KF" : " AuxF" ) : " F")
- << 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() ? " KF" : " AuxF") : " F")
- << 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 << " pt" << 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() ? " KF" : " AuxF") : " F")
- << 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() ? " KF" : " AuxF") : " F")
- << 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() ? " KF" : " AuxF") : " F") << 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 << "c" << 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 << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+ stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if(C->getSensor() != nullptr)
{
- cout << " -> S" << 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 << " -> OC" << OC->id();
+ stream << " -> OCap" << OC->id();
if (auto OF = OC->getFrame())
- cout << " ; OF" << 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 << "c" << 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;
}
}
}
@@ -1071,34 +1071,34 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- cout << " f" << 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 << "c" << 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 << " c" << c->id() << " " << c->getType() << " -->";
+ stream << " Fac" << c->id() << " " << c->getType() << " -->";
if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- cout << " A";
+ stream << " Abs";
if (c->getFrameOther() != nullptr)
- cout << " F" << c->getFrameOther()->id();
+ stream << " Frm" << c->getFrameOther()->id();
if (c->getCaptureOther() != nullptr)
- cout << " C" << c->getCaptureOther()->id();
+ stream << " Cap" << c->getCaptureOther()->id();
if (c->getFeatureOther() != nullptr)
- cout << " f" << c->getFeatureOther()->id();
+ stream << " Fac" << c->getFeatureOther()->id();
if (c->getLandmarkOther() != nullptr)
- cout << " L" << c->getLandmarkOther()->id();
- cout << endl;
+ stream << " Lmk" << c->getLandmarkOther()->id();
+ stream << std::endl;
} // for c
}
} // for f
@@ -1107,495 +1107,332 @@ 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 << " L" << L->id() << " " << L->getType();
+ stream << " Lmk" << L->id() << " " << L->getType();
if (constr_by)
{
- cout << "\t<-- ";
+ stream << "\t<-- ";
for (auto cby : L->getConstrainedByList())
- cout << "c" << 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
+bool Problem::check(bool verbose, std::ostream& stream) 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();
- }
- }
+ CheckLog log(true, "");
+ log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
- 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();
-}
-
-void Problem::perturb(double amplitude)
-{
- // Sensors
- for (auto& S : getHardware()->getSensorList())
- S->perturb(amplitude);
-
- // Frames and Captures
- for (auto& F : getTrajectory()->getFrameList())
- {
- F->perturb(amplitude);
- for (auto& C : F->getCaptureList())
- C->perturb(amplitude);
- }
-
- // Landmarks
- for (auto& L : getMap()->getLandmarkList())
- L->perturb(amplitude);
-}
-
-bool Problem::check(int verbose_level) const
-{
- using std::cout;
- using std::endl;
-
- bool is_consistent = true; // true if all checks passed; false if any check fails.
-
- 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 << "P @ " << P_raw << endl;
+ stream << "Prb @ " << P_raw << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "H @ " << H.get() << endl;
+ stream << "Hrw @ " << H.get() << std::endl;
}
// check pointer to Problem
- is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+
+ std::stringstream inconsistency_explanation;
+ inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
+ << " but problem pointer is " << P_raw << "\n";
+ log.compose(CheckLog((H->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// Sensors =======================================================================================
for (auto S : H->getSensorList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " S" << S->id() << " @ " << S.get() << endl;
- cout << " -> P @ " << S->getProblem().get() << endl;
- cout << " -> H @ " << S->getHardware().get() << endl;
- for (auto sb : S->getStateBlockVec())
+ 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())
{
- cout << " sb @ " << sb.get();
+ auto sb = pair.second;
+ 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
- is_consistent = is_consistent && (S->getProblem().get() == P_raw);
- is_consistent = is_consistent && (S->getHardware() == H);
-
- // Processors =======================================================================================
+ // is_consistent = is_consistent && (S->getProblem().get() == P_raw);
+ inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
+ << " but problem pointer is " << P_raw << "\n";
+ log.compose(CheckLog((S->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (S->getHardware() == H);
+ inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
+ << " but hardware pointer is " << H << "\n";
+ log.compose(CheckLog((S->getHardware() == H), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // Processors =======================================================================================
for (auto p : S->getProcessorList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl;
- cout << " -> P @ " << p->getProblem().get() << endl;
- cout << " -> S" << 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
- is_consistent = is_consistent && (p->getProblem().get() == P_raw);
- is_consistent = is_consistent && (p->getSensor() == S);
+
+ // is_consistent = is_consistent && (p->getProblem().get() == P_raw);
+
+ inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
+ << " but problem pointer is " << P_raw << "\n";
+ log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (p->getSensor() == S);
+
+ inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
+ << " but sensor pointer is " << P_raw << "\n";
+ log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
}
}
// ------------------------
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << "T @ " << T.get() << endl;
+ stream << "Trj @ " << T.get() << std::endl;
}
// check pointer to Problem
- is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+
+
+ inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
+ << " but problem pointer is" << P_raw << "\n";
+ log.compose(CheckLog((T->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
- if (verbose_level > 0)
- {
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F") << F->id() << " @ " << F.get() << endl;
- cout << " -> P @ " << F->getProblem().get() << endl;
- cout << " -> T @ " << F->getTrajectory().get() << endl;
- for (const auto& sb : F->getStateBlockVec())
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << 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;
+ 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) {
+ stream << " "<< pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ stream << " (lp @ " << lp.get() << ")";
}
- }
+ stream << std::endl;
+ }
+ }
+
// check problem and trajectory pointers
- is_consistent = is_consistent && (F->getProblem().get() == P_raw);
- is_consistent = is_consistent && (F->getTrajectory() == T);
+ // is_consistent = is_consistent && (F->getProblem().get() == P_raw);
+
+ inconsistency_explanation << "Frame problem 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->getTrajectory() == T);
+
+ inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
+ << " but trajectory pointer is" << T << "\n";
+ log.compose(CheckLog((F->getTrajectory() == T), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
for (auto cby : F->getConstrainedByList())
{
- if (verbose_level > 0)
- {
- cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
- }
+ for (auto sb : cby->getStateBlockPtrVector()) {
+ inconsistency_explanation
+ << "Factor " << cby.get() << " constraining " << 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) {
+ stream << " <- c" << cby->id() << " -> Frm"
+ << cby->getFrameOther()->id() << std::endl;
+ }
// check constrained_by pointer to this frame
- is_consistent = is_consistent && (cby->getFrameOther() == F);
+ // is_consistent = is_consistent && (cby->getFrameOther() == F);
+
+ inconsistency_explanation << "constrained-by frame pointer is " << cby->getFrameOther()
+ << " but frame pointer is" << F << "\n";
+ log.compose(CheckLog((cby->getFrameOther() == F), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
+
+ if (verbose) {
+ stream << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ stream << " (lp @ " << lp.get() << ")";
}
+ stream << std::endl;
+ }
}
}
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " C" << C->id() << " @ " << C.get() << " -> S";
- if (C->getSensor()) cout << C->getSensor()->id();
- else cout << "-";
- cout << endl;
- cout << " -> P @ " << C->getProblem().get() << endl;
- cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
- for (auto sb : C->getStateBlockVec())
+ 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())
{
- cout << " sb @ " << sb.get();
- if (sb)
+ auto sb = pair.second;
+ if (verbose)
{
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ stream << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ stream << " (lp @ " << lp.get() << ")";
+ }
+ stream << std::endl;
}
- cout << endl;
}
- }
+
// check problem and frame pointers
- is_consistent = is_consistent && (C->getProblem().get() == P_raw);
- is_consistent = is_consistent && (C->getFrame() == F);
+ // 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " f" << f->id() << " @ " << f.get() << endl;
- cout << " -> P @ " << f->getProblem().get() << endl;
- cout << " -> C" << 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);
- is_consistent = is_consistent && (f->getCapture() == C);
+ // is_consistent = is_consistent && (f->getProblem().get() == P_raw);
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
for (auto cby : f->getConstrainedByList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " <- c" << cby->id() << " -> f" << 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);
+ // 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);
}
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- if (verbose_level > 0)
- cout << " c" << c->id() << " @ " << c.get();
+ if (verbose)
+ stream << " Fac" << c->id() << " @ " << c.get();
auto Fo = c->getFrameOther();
auto Co = c->getCaptureOther();
@@ -1604,214 +1441,264 @@ 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 << " ( --> F" << Fo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Frm" << Fo->id() << " <- ";
bool found = false;
for (auto cby : Fo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << 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;
+ // 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);
+
}
// find constrained_by pointer in constrained capture
if ( Co ) // case CAPTURE:
{
- if (verbose_level > 0)
- cout << " ( --> C" << Co->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Cap" << Co->id() << " <- ";
bool found = false;
for (auto cby : Co->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << 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;
+ // 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);
+
}
// find constrained_by pointer in constrained feature
if ( fo ) // case FEATURE:
{
- if (verbose_level > 0)
- cout << " ( --> f" << fo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Ftr" << fo->id() << " <- ";
bool found = false;
for (auto cby : fo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << 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;
+ // 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);
}
// find constrained_by pointer in constrained landmark
if ( Lo ) // case LANDMARK:
{
- if (verbose_level > 0)
- cout << " ( --> L" << Lo->id() << " <- ";
+ if (verbose)
+ stream << " ( --> Lmk" << Lo->id() << " <- ";
bool found = false;
for (auto cby : Lo->getConstrainedByList())
{
- if (verbose_level > 0)
- cout << " c" << 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;
+ // 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_level > 0)
- cout << endl;
+ if (verbose)
+ stream << std::endl;
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " -> P @ " << c->getProblem().get() << endl;
- cout << " -> f" << 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);
- is_consistent = is_consistent && (c->getFeature() == f);
+ // is_consistent = is_consistent && (c->getProblem().get() == P_raw);
+
+ 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()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// find state block pointers in all constrained nodes
SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
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!)";
+ log.compose(CheckLog((found), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ inconsistency_explanation << "The stateblock " << sb << " of factor " << c << " is null\n";
+ 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;
+ // is_consistent = is_consistent && found;
}
}
}
@@ -1821,63 +1708,103 @@ bool Problem::check(int verbose_level) const
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (verbose_level > 0)
- cout << "M @ " << M.get() << endl;
+ if (verbose)
+ stream << "Map @ " << M.get() << std::endl;
// check pointer to Problem
- is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+ // is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+
+ inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
+ log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ //Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
- if (verbose_level > 0)
+ if (verbose)
{
- cout << " L" << L->id() << " @ " << L.get() << endl;
- cout << " -> P @ " << L->getProblem().get() << endl;
- cout << " -> M @ " << L->getMap().get() << endl;
- for (const auto& sb : L->getStateBlockVec())
+ 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())
{
- cout << " sb @ " << sb.get();
+ auto sb = pair.second;
+ 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
- is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- is_consistent = is_consistent && (L->getMap() == M);
- for (auto cby : L->getConstrainedByList())
- {
- if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl;
- // check constrained_by pointer to this landmark
- is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id());
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
- }
+ // is_consistent = is_consistent && (L->getProblem().get() == P_raw);
+
+ inconsistency_explanation << "The landmarks problem ptr is "
+ << L->getProblem().get() << " but problem is "
+ << P_raw << "\n";
+
+ log.compose(CheckLog((L->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ // Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ // is_consistent = is_consistent && (L->getMap() == M);
+
+ inconsistency_explanation << "The landmarks map ptr is "
+ << L->getMap() << " but map is "
+ << M << "\n";
+ log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
+ // Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ for (auto cby : L->getConstrainedByList()) {
+ 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() &&
+ // L->id() == cby->getLandmarkOther()->id());
+
+ inconsistency_explanation << "The landmark constrained-by loop started at "
+ << cby
+ << " is not closed\n";
+ log.compose(CheckLog((cby->getLandmarkOther() &&
+ L->id() == cby->getLandmarkOther()->id()), inconsistency_explanation.str()));
+ // Clear inconsistency_explanation
+ std::stringstream().swap(inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector()) {
+ if (verbose) {
+ stream << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ stream << " (lp @ " << lp.get() << ")";
+ }
+ stream << std::endl;
}
+ }
}
}
- if (verbose_level) cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "Not OK !!") << endl;
- if (verbose_level) cout << 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 is_consistent;
+ 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)
@@ -1894,4 +1821,23 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool
print(0, constr_by, metric, state_blocks);
}
+void Problem::perturb(double amplitude)
+{
+ // Sensors
+ for (auto& S : getHardware()->getSensorList())
+ S->perturb(amplitude);
+
+ // Frames and Captures
+ for (auto& F : getTrajectory()->getFrameList())
+ {
+ F->perturb(amplitude);
+ for (auto& C : F->getCaptureList())
+ C->perturb(amplitude);
+ }
+
+ // Landmarks
+ for (auto& L : getMap()->getLandmarkList())
+ L->perturb(amplitude);
+}
+
} // namespace wolf