diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 2491d5a6a5afbdc4d3b52a3e86003c3794b6cd81..8b260c88dbfb063a929c3d19c89573a213a50531 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -93,7 +93,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
virtual void fix() override;
virtual void unfix() override;
- bool hasCalibration() {return calib_size_ > 0;}
+ bool hasCalibration() const {return calib_size_ > 0;}
SizeEigen getCalibSize() const;
virtual Eigen::VectorXd getCalibration() const;
void setCalibration(const Eigen::VectorXd& _calib);
@@ -102,6 +102,18 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
protected:
virtual SizeEigen computeCalibSize() const;
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index d78cd5169f3dfc523c9435c11dc65b41ef6b3591..ebbd06edd2712439652b2017604ad96615bcfef5 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -98,8 +98,15 @@ class CaptureMotion : public CaptureBase
// Origin frame and capture
CaptureBasePtr getOriginCapture();
+ CaptureBasePtr getOriginCapture() const;
void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const override;
+
// member data:
private:
Eigen::VectorXd data_; ///< Motion data in form of vector mandatory
@@ -163,6 +170,11 @@ inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
return capture_origin_ptr_.lock();
}
+inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture() const
+{
+ return capture_origin_ptr_.lock();
+}
+
inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr)
{
capture_origin_ptr_ = _capture_origin_ptr;
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index cd1439f96e437dc39dde137dd1014f37abee10db..36725da3757104cadb68beace2f207562fa04e17 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -206,6 +206,18 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_ptr, T&&... all);
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
private:
void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index a2ba3bc60f5d0db16d6c486099ac5e3e9b3116ec..35d73460229b45e6f001c96ad354e22eaf51b401 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -106,6 +106,16 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index eea8873b3e804743f4cd224bffd6442d4c17febf..039d3482e3efeffbcaeae694bdc72744c927934b 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -131,6 +131,17 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
private:
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index a309e7a03c8a5e27f459b772ce6bff5b0994f714..201ec070b7d44d6d42c128cb5105c0299172c66a 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -27,6 +27,17 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
const SensorBasePtrList& getSensorList() const;
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
};
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 254e03e480f8471029c38c8f59626b230dc962f9..d51312ab11b8b7f53ed23c10f32b8c0d4d70f0da 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -85,6 +85,18 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
*/
static LandmarkBasePtr create(const YAML::Node& _node);
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
private:
void setMap(const MapBasePtr _map_ptr);
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 6d09191e54619d5a00fce39e1ecf002aace66d3a..c2d3233ff83952e6a2d3f6907c4ec85e584d821c 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -38,6 +38,16 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
private:
std::string dateTimeNow();
};
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index dc62abcac8375fc238f7c93617a33ccb8662d246..b65ce905de897f03a5cdb2b376b74a04ac6ccc38 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -361,6 +361,18 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
void setVotingAuxActive(bool _voting_active = true);
+
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
};
inline bool ProcessorBase::isVotingActive() const
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 81a9184ad4798ccbaac1ebd61a194f6e287331b4..2128590306d81c1b3f6be6433256678b2ea9a328 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -457,6 +457,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
void setDistTraveled(const double& _dist_traveled);
void setAngleTurned(const double& _angle_turned);
+ void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const override;
+
protected:
// Attributes
SizeEigen x_size_; ///< The size of the state vector
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index d7ab951643fbc2760b8d81110a9f2338a96dad36..dbd1175d1e25bc855aec2a67ca59b319cf9da453 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -246,6 +246,12 @@ class ProcessorTracker : public ProcessorBase
return this->params_tracker_->print();
}
+ void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const override;
+
protected:
void computeProcessingStep();
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 1c099cb9be4827705bd43d229714ea1dabf24ed0..2ceabb8f470fb7190c146035dfb34b8e6c4a8349 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -242,6 +242,18 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
Eigen::VectorXd getNoiseStd() const;
Eigen::MatrixXd getNoiseCov() const;
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+
+
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 60f81c58afcb881fe8be35aae225c2ee55ac2c69..4c48df54cca17a88eebdefa099b4c154cc881deb 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -47,6 +47,16 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
void sortFrame(FrameBasePtr _frm_ptr);
void updateLastFrames();
+ virtual void printHeader(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index d07b9b055d8197593dbdde99d05f6a4eddd46bfe..62e602c549182f522ddc89c2f321e44421d11b21 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -277,5 +277,45 @@ void CaptureBase::setProblem(ProblemPtr _problem)
ft->setProblem(_problem);
}
+void CaptureBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " Cap" << id() << " " << getType();
+
+ if(getSensor() != nullptr)
+ {
+ _stream << " -> Sen" << getSensor()->id();
+ }
+ else
+ _stream << " -> Sen-";
+
+ _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ if (_constr_by)
+ {
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ if (_state_blocks)
+ for (const auto& sb : getStateBlockVec())
+ {
+ if(sb != nullptr)
+ {
+ _stream << " sb: ";
+ _stream << (sb->isFixed() ? "Fix" : "Est");
+ if (_metric)
+ _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ }
+}
+void CaptureBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+ if (_depth >= 3)
+ for (auto f : getFeatureList())
+ f->print(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index a736c242e7fb7047498d061d9121ce8fb56f817c..828d447b8a0c8880937e5168d3b5181ba7f1fa46 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -71,4 +71,57 @@ Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current,
return delta_corrected;
}
+void CaptureMotion::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " CapM" << id() << " " << getType();
+
+ if(getSensor() != nullptr)
+ {
+ _stream << " -> Sen" << getSensor()->id();
+ }
+ else
+ _stream << " -> Sen-";
+ if (auto OC = getOriginCapture())
+ {
+ _stream << " -> OCap" << OC->id();
+ if (auto OF = OC->getFrame())
+ _stream << " ; OFrm" << OF->id();
+ }
+
+ _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ if (_constr_by)
+ {
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ if (_state_blocks)
+ for (const auto& sb : getStateBlockVec())
+ {
+ if(sb != nullptr)
+ {
+ _stream << " sb: ";
+ _stream << (sb->isFixed() ? "Fix" : "Est");
+ if (_metric)
+ _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ }
+
+ _stream << " buffer size : " << getBuffer().get().size() << std::endl;
+ if ( _metric && ! getBuffer().get().empty())
+ {
+ _stream << " delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
+ if (hasCalibration())
+ {
+ _stream << " calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
+ _stream << " jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
+ _stream << " calib current: (" << getCalibration().transpose() << ")" << std::endl;
+ _stream << " delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+ }
+ }
+
+}
}
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 72b36c519a0d8d0ce2cb7ac4748e0dfc110026d0..67a1a21ea1f6ca0631d2aaf35b38ee5a18e51c1b 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -299,4 +299,32 @@ void FactorBase::setProblem(ProblemPtr _problem)
this->getProblem()->notifyFactor(shared_from_this(),ADD);
}
+void FactorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " Fac" << id() << " " << getType() << " -->";
+ if ( getFrameOtherList() .empty()
+ && getCaptureOtherList() .empty()
+ && getFeatureOtherList() .empty()
+ && getLandmarkOtherList().empty())
+ _stream << " Abs";
+
+ for (const auto& Fow : getFrameOtherList())
+ if (!Fow.expired())
+ _stream << " Frm" << Fow.lock()->id();
+ for (const auto& Cow : getCaptureOtherList())
+ if (!Cow.expired())
+ _stream << " Cap" << Cow.lock()->id();
+ for (const auto& fow : getFeatureOtherList())
+ if (!fow.expired())
+ _stream << " Ftr" << fow.lock()->id();
+ for (const auto& Low : getLandmarkOtherList())
+ if (!Low.expired())
+ _stream << " Lmk" << Low.lock()->id();
+ _stream << std::endl;
+}
+
+void FactorBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 8dc3801efa5f93e5bd0df593442e4860cb75135e..d89d3adb8119c9c64bdb244c6d209b04fe7e2367 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -165,4 +165,27 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
WOLF_WARN("Linking with nullptr");
}
}
+void FeatureBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "c " : "");
+ if (_constr_by)
+ {
+ _stream << " <--\t";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+ if (_metric)
+ _stream << " m = ( " << std::setprecision(2) << getMeasurement().transpose()
+ << " )" << std::endl;
+
+}
+
+void FeatureBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+ if (_depth >= 4)
+ for (auto c : getFactorList())
+ c->print(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index c0f4d310e1062c9a68ea94a0ee0b70f912cd9d15..a341335d12c7a6d1b4dd4519901070805ef2492e 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -359,4 +359,47 @@ void FrameBase::setProblem(ProblemPtr _problem)
cap->setProblem(_problem);
}
+void FrameBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << (isKeyOrAux() ? (isKey() ? " KFrm" : " AFrm") : " Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
+ if (_constr_by)
+ {
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ if (_metric && _state_blocks){
+ for (const auto& key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ _stream << " " << key << "," << (sb->isFixed() ? "Fix: " : "Est: ") << sb->getState().transpose() << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << (isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ << getTimeStamp();
+ _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << " sb:";
+ for (const auto& sb : getStateBlockVec())
+ {
+ _stream << " " << (sb->isFixed() ? "Fix" : "Est");
+ }
+ _stream << std::endl;
+ }
+}
+
+void FrameBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+ if (_depth >= 2)
+ for (auto C : getCaptureList())
+ C->print(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index adda7984748ebfb2fa6e40517f0d1bedc331efd2..302809be3f82ae89b17fa8427e69ac543ce4cb3f 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -20,4 +20,16 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
return _sensor_ptr;
}
+void HardwareBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "") << std::endl;
+
+}
+void HardwareBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+ if (_depth >= 1)
+ for (auto S : getSensorList())
+ S->print(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index bc57f27294e6a0b05d67cbcf44725dd2b819f63b..20df53bd0f9e2e5a1e58b974edd1988c55464e89 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -153,6 +153,38 @@ bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
return false;
}
+void LandmarkBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " Lmk" << id() << " " << getType();
+ if (_constr_by)
+ {
+ _stream << "\t<-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+ if (_metric)
+ {
+ _stream << (isFixed() ? " Fix" : " Est");
+ _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ if (_state_blocks)
+ {
+ _stream << " sb:";
+ for (const auto& sb : getStateBlockVec())
+ {
+ if (sb != nullptr)
+ _stream << (sb->isFixed() ? " Fix" : " Est");
+ }
+ _stream << std::endl;
+ }
+}
+
+void LandmarkBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 3a3d32f8ac073ba88160995f69af8e1c150426bb..34c4ddf18946321e808f16b5550653449693a5a5 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -90,4 +90,15 @@ std::string MapBase::dateTimeNow()
return date_time;
}
+void MapBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "") << std::endl;
+}
+void MapBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+ if (_depth >= 1)
+ for (auto L : getLandmarkList())
+ L->print(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6326970fc3b4db3a5b825590c27367787ff457e2..e01471f9775095545580db2d79eab400dde41f6e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -957,270 +957,13 @@ void Problem::print(int _depth, std::ostream& _stream, bool _constr_by, bool _me
_stream << std::endl;
_stream << "P: wolf tree status ---------------------" << std::endl;
- _stream << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
- if (_depth >= 1)
- {
- // Sensors =======================================================================================
- for (auto S : getHardware()->getSensorList())
- {
- _stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (_depth < 2)
- _stream << " -- " << S->getProcessorList().size() << "p";
- _stream << std::endl;
- if (_metric && _state_blocks)
- {
- _stream << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
- }
- _stream << std::endl;
- }
- else if (_metric)
- {
- _stream << " ( ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- _stream << sb->getState().transpose() << " ";
- }
- _stream << ")" << std::endl;
- }
- else if (_state_blocks)
- {
- _stream << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
- }
- _stream << std::endl;
- }
- if (_depth >= 2)
- {
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (p->isMotion())
- {
- _stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
- ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOrigin())
- _stream << " o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
- << pm->getOrigin()->getFrame()->id() << std::endl;
- if (pm->getLast())
- _stream << " l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pm->getLast()->getFrame()->id() << std::endl;
- if (pm->getIncoming())
- _stream << " i: Cap" << pm->getIncoming()->id() << std::endl;
- }
- else
- {
- _stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
- ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
- if (pt)
- {
- if (pt->getOrigin())
- _stream << " o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pt->getOrigin()->getFrame()->id() << std::endl;
- if (pt->getLast())
- _stream << " l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pt->getLast()->getFrame()->id() << std::endl;
- if (pt->getIncoming())
- _stream << " i: Cap" << pt->getIncoming()->id() << std::endl;
- }
- }
- } // for p
- }
- } // for S
- }
- _stream << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
- if (_depth >= 1)
- {
- // Frames =======================================================================================
- for (auto F : getTrajectory()->getFrameList())
- {
- _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((_depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (_constr_by)
- {
- _stream << " <-- ";
- for (auto cby : F->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
-
- if (_metric && _state_blocks){
- for (const auto& key : F->getStructure())
- {
- auto sb = F->getStateBlock(key);
- _stream << " " << key << "," << (sb->isFixed() ? "Fix: " : "Est: ") << sb->getState().transpose() << std::endl;
- }
- }
- else if (_metric)
- {
- _stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp();
- _stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- _stream << std::endl;
- }
- else if (_state_blocks)
- {
- _stream << " sb:";
- for (const auto& sb : F->getStateBlockVec())
- {
- _stream << " " << (sb->isFixed() ? "Fix" : "Est");
- }
- _stream << std::endl;
- }
- if (_depth >= 2)
- {
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- _stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+ getHardware()->print(_depth, _stream, _constr_by, _metric, _state_blocks);
- if(C->getSensor() != nullptr)
- {
- _stream << " -> Sen" << C->getSensor()->id();
- }
- else
- _stream << " -> Sen-";
- if (C->isMotion())
- {
- auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (auto OC = CM->getOriginCapture())
- {
- _stream << " -> OCap" << OC->id();
- if (auto OF = OC->getFrame())
- _stream << " ; OFrm" << OF->id();
- }
- }
+ getTrajectory()->print(_depth, _stream, _constr_by, _metric, _state_blocks);
- _stream << ((_depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (_constr_by)
- {
- _stream << " <-- ";
- for (auto cby : C->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
+ getMap()->print(_depth, _stream, _constr_by, _metric, _state_blocks);
- if (_state_blocks)
- for (const auto& sb : C->getStateBlockVec())
- {
- if(sb != nullptr)
- {
- _stream << " sb: ";
- _stream << (sb->isFixed() ? "Fix" : "Est");
- if (_metric)
- _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- _stream << std::endl;
- }
- }
-
- if (C->isMotion() )
- {
- CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- _stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
- if ( _metric && ! CM->getBuffer().get().empty())
- {
- _stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
- if (CM->hasCalibration())
- {
- _stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
- _stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
- _stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
- _stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
- }
- }
- }
-
- if (_depth >= 3)
- {
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- _stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((_depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (_constr_by)
- {
- _stream << " <--\t";
- for (auto cby : f->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
- if (_metric)
- _stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << std::endl;
- if (_depth >= 4)
- {
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- _stream << " Fac" << c->id() << " " << c->getType() << " -->";
- if ( c->getFrameOtherList() .empty()
- && c->getCaptureOtherList() .empty()
- && c->getFeatureOtherList() .empty()
- && c->getLandmarkOtherList().empty())
- _stream << " Abs";
-
- for (const auto& Fow : c->getFrameOtherList())
- if (!Fow.expired())
- _stream << " Frm" << Fow.lock()->id();
- for (const auto& Cow : c->getCaptureOtherList())
- if (!Cow.expired())
- _stream << " Cap" << Cow.lock()->id();
- for (const auto& fow : c->getFeatureOtherList())
- if (!fow.expired())
- _stream << " Ftr" << fow.lock()->id();
- for (const auto& Low : c->getLandmarkOtherList())
- if (!Low.expired())
- _stream << " Lmk" << Low.lock()->id();
- _stream << std::endl;
- } // for c
- }
- } // for f
- }
- } // for C
- }
- } // for F
- }
- _stream << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
- if (_depth >= 1)
- {
- // Landmarks =======================================================================================
- for (auto L : getMap()->getLandmarkList())
- {
- _stream << " Lmk" << L->id() << " " << L->getType();
- if (_constr_by)
- {
- _stream << "\t<-- ";
- for (auto cby : L->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
- if (_metric)
- {
- _stream << (L->isFixed() ? " Fix" : " Est");
- _stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- _stream << std::endl;
- }
- if (_state_blocks)
- {
- _stream << " sb:";
- for (const auto& sb : L->getStateBlockVec())
- {
- if (sb != nullptr)
- _stream << (sb->isFixed() ? " Fix" : " Est");
- }
- _stream << std::endl;
- }
- } // for L
- }
_stream << "-----------------------------------------" << std::endl;
_stream << std::endl;
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index f61f861b021e51a08dd92db301aa1bd8de159272..791876e995f1d977f96e1bcbedd05a471201681f 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -196,4 +196,14 @@ void BufferPackKeyFrame::print(void) const
std::cout << "]" << std::endl;
}
+void ProcessorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+
+}
+
+void ProcessorBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 11db736b56699ff204f8ff262d63236a625297a7..a3c0bc6cf0ec911ecc256e16fe67721cbef73fe5 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -660,4 +660,17 @@ bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr)
return false;
}
+void ProcessorMotion::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+ if (getOrigin())
+ _stream << " o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << " l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _stream << " i: Cap" << getIncoming()->id() << std::endl;
+
+}
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 56d5ae4ae45ed40962274292328076087da7fb9f..52dcd5c02fa16c54b74d872de2b9ce12db0b7559 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -322,5 +322,17 @@ bool ProcessorTracker::storeCapture(CaptureBasePtr _cap_ptr)
return false;
}
+void ProcessorTracker::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+ if (getOrigin())
+ _stream << " o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << " l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _stream << " i: Cap" << getIncoming()->id() << std::endl;
+}
} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 09afa700e79e66b058df609ea004334bd0830e4b..6ed95da19ade0a8fefd9d90609bc7111fcccce8d 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -467,4 +467,53 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
}
}
+void SensorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << " Sen" << id() << " " << getType() << " \"" << getName() << "\"";
+ if (_depth < 2)
+ _stream << " -- " << getProcessorList().size() << "p";
+ _stream << std::endl;
+ if (_metric && _state_blocks)
+ {
+ _stream << " sb: ";
+ for (auto& _key : getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = getStateBlock(key);
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
+ }
+ _stream << std::endl;
+ }
+ else if (_metric)
+ {
+ _stream << " ( ";
+ for (auto& _key : getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = getStateBlock(key);
+ _stream << sb->getState().transpose() << " ";
+ }
+ _stream << ")" << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << " sb: ";
+ for (auto& _key : getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = getStateBlock(key);
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+}
+
+void SensorBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+
+ if (_depth >= 2)
+ for (auto p : getProcessorList())
+ p->print(_depth, _stream, _constr_by, _metric, _state_blocks);
+}
} // namespace wolf
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 428faf954d89ffc8f0d7a88b182cd04a92850c4a..96b4b43f7c0bc48fea75e0e655a19143d925b6dc 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -136,4 +136,16 @@ FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _t
return closest_kf;
}
+void TrajectoryBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ _stream << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameList().size()) + "F") : "") << std::endl;
+}
+void TrajectoryBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+{
+ printHeader(_depth, _stream, _constr_by, _metric, _state_blocks);
+ if (_depth >= 1)
+ for (auto F : getFrameList())
+ F->print(_depth, _stream, _constr_by, _metric, _state_blocks);
+
+}
} // namespace wolf