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 313629352513d48faad7757c514a7e466ca58203..36f3ce94e88681f32529f28b1bd02c93ee5316ab 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -274,7 +274,6 @@ class Problem : public std::enable_shared_from_this<Problem>
// Perturb state
void perturb(double amplitude = 0.01);
-
// Map branch -----------------------------------------
MapBasePtr getMap() const;
void loadMap(const std::string& _filename_dot_yaml);
@@ -330,6 +329,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 +349,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/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 7f0a7c9c3954bb1f1e1abe90c233ceeddaf65a2f..dc62abcac8375fc238f7c93617a33ccb8662d246 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -266,10 +266,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
unsigned int id() const;
protected:
- /**
- * \brief set the pointer to problem
- */
- virtual void setProblem(ProblemPtr) override;
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
@@ -359,6 +355,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setVotingActive(bool _voting_active = true);
+ int getDim() const;
+
void link(SensorBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
@@ -405,6 +403,10 @@ inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
{
params_->time_tolerance = _time_tolerance;
}
+inline int ProcessorBase::getDim() const
+{
+ return dim_;
+}
template<typename classType, typename... T>
std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index ba7f295749a84f9b7a401201d8759d2c394816a1..42a11b3782a5a669264471bcae49571ec1322840 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -86,7 +86,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
addStateBlock("V", v_ptr);
- this->setType("POV 3D");
+ this->setType("POV 3d");
}else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
// auto _x = Eigen::VectorXd::Zero(10);
assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
@@ -102,7 +102,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
addStateBlock("C", c_ptr);
addStateBlock("D", cd_ptr);
addStateBlock("L", Lc_ptr);
- this->setType("POVCDL 3D");
+ this->setType("POVCDL 3d");
}else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl;
throw std::runtime_error("Unknown frame structure");
@@ -156,7 +156,8 @@ void FrameBase::setNonEstimated()
{
// unregister if previously estimated
if (isKeyOrAux())
- removeStateBlocks(getProblem());
+ for (const auto& sb : getStateBlockVec())
+ getProblem()->notifyStateBlock(sb, REMOVE);
type_ = NON_ESTIMATED;
if (getTrajectory())
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 849283d67cb26b9f849ec664cc98c27d3c67e28c..b81f9c8752bb047ae702a092c8f20458a29f3153 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -224,6 +224,12 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
}
ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params);
+
+ //Dimension check
+ int prc_dim = prc_ptr->getDim();
+ auto prb = this;
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
+
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
@@ -262,9 +268,16 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server);
+
+ //Dimension check
+ int prc_dim = prc_ptr->getDim();
+ auto prb = this;
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
+
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
+
// setting the origin in all processor motion if origin already setted
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
@@ -912,55 +925,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)
{
@@ -969,67 +980,66 @@ 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)
- << F->getTimeStamp().get();
- cout << ",\t x = ( " << std::setprecision(2) << endl;
- for (auto sb_name: F->getStructure()){
- auto sb = F->getStateBlock(sb_name);
- cout << " " << sb_name;
- if (state_blocks){
- cout << "," << (sb->isFixed() ? "Fix" : "Est");
- }
- cout << ": " << sb->getState().transpose() << "\n";
+ stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ << F->getTimeStamp();
+ stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ stream << std::endl;
+ }
+ if (state_blocks)
+ {
+ stream << " sb:";
+ for (const auto& sb : F->getStateBlockVec())
+ {
+ stream << " " << (sb->isFixed() ? "Fix" : "Est");
}
- cout << " )" << endl;
- // cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- // cout << endl;
+ stream << std::endl;
}
// if (state_blocks)
// {
@@ -1045,60 +1055,60 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
// 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;
}
}
}
@@ -1108,34 +1118,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
@@ -1144,339 +1154,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 << " 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;
- }
- } // for L
- }
- cout << "-----------------------------------------" << endl;
- cout << 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;
+ stream << std::endl;
}
} // for L
}
- result << "-----------------------------------------" << endl;
- result << endl;
- return result.str();
+ stream << "-----------------------------------------" << std::endl;
+ stream << std::endl;
}
-void Problem::perturb(double amplitude)
+bool Problem::check(bool verbose, std::ostream& stream) const
{
- // 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;
+ CheckLog log(true, "");
+ log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
- CheckLog log(true, "WOLF problem inconsistencies\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 << "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);
@@ -1484,28 +1217,29 @@ bool Problem::check(int verbose_level) const
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()));
+ 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
@@ -1526,11 +1260,11 @@ bool Problem::check(int verbose_level) const
// 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
@@ -1555,9 +1289,9 @@ bool Problem::check(int verbose_level) const
// 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);
@@ -1572,28 +1306,30 @@ bool Problem::check(int verbose_level) const
// 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;
+ 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 &sb : F->getStateBlockVec()) {
- inconsistency_explanation << "Frame's " << F.get()
+ 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_level > 0) {
- cout << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ 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;
}
- cout << endl;
- }
}
// check problem and trajectory pointers
@@ -1624,9 +1360,9 @@ bool Problem::check(int verbose_level) const
// Clear inconsistency_explanation
std::stringstream().swap(inconsistency_explanation);
}
- if (verbose_level > 0) {
- cout << " <- c" << cby->id() << " -> F"
- << 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);
@@ -1640,14 +1376,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;
}
}
}
@@ -1655,26 +1391,27 @@ bool Problem::check(int verbose_level) const
// 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;
+ 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 sb : C->getStateBlockVec())
+ for (auto pair: C->getStateBlockMap())
{
- if (verbose_level > 0)
+ auto sb = pair.second;
+ if (verbose)
{
- cout << " 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;
}
}
@@ -1698,12 +1435,12 @@ bool Problem::check(int verbose_level) const
// 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);
@@ -1724,9 +1461,9 @@ bool Problem::check(int verbose_level) const
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);
@@ -1741,8 +1478,8 @@ bool Problem::check(int verbose_level) const
// 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();
@@ -1751,24 +1488,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 << " ( --> 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;
@@ -1783,17 +1520,17 @@ bool Problem::check(int verbose_level) const
// 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;
@@ -1808,17 +1545,17 @@ bool Problem::check(int verbose_level) const
// 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;
@@ -1832,17 +1569,17 @@ bool Problem::check(int verbose_level) const
// 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;
@@ -1852,13 +1589,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 << " -> 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);
@@ -1882,119 +1619,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!)";
@@ -2018,8 +1755,8 @@ 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);
@@ -2031,21 +1768,22 @@ bool Problem::check(int verbose_level) const
// 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
@@ -2069,9 +1807,9 @@ bool Problem::check(int verbose_level) const
std::stringstream().swap(inconsistency_explanation);
for (auto cby : L->getConstrainedByList()) {
- if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L"
- << 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() &&
@@ -2086,26 +1824,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)
@@ -2122,4 +1868,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
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index ba27c6cd69a8e45b1a25a543beacec7fa085a5b5..f61f861b021e51a08dd92db301aa1bd8de159272 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -22,13 +22,6 @@ ProcessorBase::~ProcessorBase()
// WOLF_DEBUG("destructed -p" , id());
}
-void ProcessorBase::setProblem(ProblemPtr _problem)
-{
- std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
- assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
-
- NodeBase::setProblem(_problem);
-}
bool ProcessorBase::permittedKeyFrame()
{
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index f85a46cc3ade3fcb1a493d58b2e06a5fc43ad2db..aded1532cc17fc6a0f2e0d48edbbca857a1ab28b 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -370,7 +370,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
// Get delta and correct it with new calibration params
VectorXd calib_preint = capture_motion->getCalibrationPreint();
Motion motion = capture_motion->getBuffer().getMotion(_ts);
-
+
VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint);
VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
@@ -628,13 +628,10 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
void ProcessorMotion::setProblem(ProblemPtr _problem)
{
- if (_problem == nullptr)
- return;
-
std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
- if (_problem == this->getProblem())
+ if (_problem == nullptr or _problem == this->getProblem())
return;
NodeBase::setProblem(_problem);
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 68c069b16861376f3db6226c3bd7b0de02805a8f..929c1ed7ceb825fdaa265a1af592fa8125832789 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -5,7 +5,6 @@
namespace wolf
{
-
ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
params_odom_2d_(_params)