diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 8b260c88dbfb063a929c3d19c89573a213a50531..393eddb8a87c05347433e93de11cb17d05a57a06 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -102,13 +102,13 @@ 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, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h index ebbd06edd2712439652b2017604ad96615bcfef5..e023de9fe9a8f4c4901514f920203d841fa92205 100644 --- a/include/core/capture/capture_motion.h +++ b/include/core/capture/capture_motion.h @@ -101,7 +101,7 @@ class CaptureMotion : public CaptureBase CaptureBasePtr getOriginCapture() const; void setOriginCapture(CaptureBasePtr _capture_origin_ptr); - virtual void printHeader(int depth, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index 36725da3757104cadb68beace2f207562fa04e17..67134b3f1251e31fe17d889ead538aef4e33515d 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -206,13 +206,13 @@ 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, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h index 35d73460229b45e6f001c96ad354e22eaf51b401..b5d01e25605edb16e7fcf775a6321f8d58bc4624 100644 --- a/include/core/feature/feature_base.h +++ b/include/core/feature/feature_base.h @@ -106,12 +106,12 @@ 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, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 039d3482e3efeffbcaeae694bdc72744c927934b..9fc19c6ab21161dbd01897e60de1f591af3de87c 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -131,13 +131,13 @@ 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, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h index 201ec070b7d44d6d42c128cb5105c0299172c66a..240cf1fd1e9f89a6b13e0d4122a41cdc4d9628e0 100644 --- a/include/core/hardware/hardware_base.h +++ b/include/core/hardware/hardware_base.h @@ -27,12 +27,12 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa const SensorBasePtrList& getSensorList() const; - virtual void printHeader(int depth, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index d51312ab11b8b7f53ed23c10f32b8c0d4d70f0da..317c5c12759cfa77162cd18aa4ea3299c517f9a6 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -85,13 +85,13 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ */ static LandmarkBasePtr create(const YAML::Node& _node); - virtual void printHeader(int depth, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h index c2d3233ff83952e6a2d3f6907c4ec85e584d821c..6ed65e3de0e253a341a058d84d184744c4620971 100644 --- a/include/core/map/map_base.h +++ b/include/core/map/map_base.h @@ -38,12 +38,12 @@ 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, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index b65ce905de897f03a5cdb2b376b74a04ac6ccc38..41484d4ffde3f9d5ebda29e4bb8118788634a00d 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -362,13 +362,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all); void setVotingAuxActive(bool _voting_active = true); - virtual void printHeader(int depth, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 2128590306d81c1b3f6be6433256678b2ea9a328..c0ce83b758e7020f29b46c0db433cad07f8403f1 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -457,7 +457,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion void setDistTraveled(const double& _dist_traveled); void setAngleTurned(const double& _angle_turned); - void printHeader(int depth, // + void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index dbd1175d1e25bc855aec2a67ca59b319cf9da453..cc9aa3e4363468c8c451da9a59e9e51773a8d25a 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -246,7 +246,7 @@ class ProcessorTracker : public ProcessorBase return this->params_tracker_->print(); } - void printHeader(int depth, // + void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 2ceabb8f470fb7190c146035dfb34b8e6c4a8349..461ac56c60927a9570ec99e369de0129f9124a80 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -242,12 +242,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh Eigen::VectorXd getNoiseStd() const; Eigen::MatrixXd getNoiseCov() const; - virtual void printHeader(int depth, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 4c48df54cca17a88eebdefa099b4c154cc881deb..66122cf2f45246a6142507d5138a25bedfd81b00 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -47,12 +47,12 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj void sortFrame(FrameBasePtr _frm_ptr); void updateLastFrames(); - virtual void printHeader(int depth, // + virtual void printHeader(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // bool state_blocks) const; - void print(int depth, // + void print(std::string _tabs, int depth, // std::ostream& stream , bool constr_by, // bool metric, // diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 2175b4ac2f92efadeecf8074bf36c8f4a76f4b40..456c9451236b649d871782fea2d8f8716b6c71f2 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -277,9 +277,9 @@ 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 +void CaptureBase::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " Cap" << id() << " " << getType(); + _stream << _tabs << "Cap" << id() << " " << getType(); if(getSensor() != nullptr) { @@ -302,7 +302,7 @@ void CaptureBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by for (const auto& key : getStructure()) { auto sb = getStateBlock(key); - _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl; + _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl; } } else if (_metric) @@ -338,12 +338,11 @@ void CaptureBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by // } // } } -void CaptureBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const +void CaptureBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); if (_depth >= 3) for (auto f : getFeatureList()) - f->print(_depth, _stream, _constr_by, _metric, _state_blocks); + f->print(_tabs + " ", _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf - diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index 828d447b8a0c8880937e5168d3b5181ba7f1fa46..43b32071e18b6f31868fcdeb02f444c5371fd01c 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -71,9 +71,9 @@ 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 +void CaptureMotion::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " CapM" << id() << " " << getType(); + _stream << _tabs << "CapM" << id() << " " << getType(); if(getSensor() != nullptr) { @@ -102,7 +102,7 @@ void CaptureMotion::printHeader(int _depth, std::ostream& _stream, bool _constr_ { if(sb != nullptr) { - _stream << " sb: "; + _stream << _tabs << " " << "sb: "; _stream << (sb->isFixed() ? "Fix" : "Est"); if (_metric) _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )"; @@ -110,16 +110,16 @@ void CaptureMotion::printHeader(int _depth, std::ostream& _stream, bool _constr_ } } - _stream << " buffer size : " << getBuffer().get().size() << std::endl; + _stream << _tabs << " " << "buffer size : " << getBuffer().get().size() << std::endl; if ( _metric && ! getBuffer().get().empty()) { - _stream << " delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl; + _stream << _tabs << " " << "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; + _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl; + _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl; + _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl; + _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl; } } diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 67a1a21ea1f6ca0631d2aaf35b38ee5a18e51c1b..67c9d44f8d918df7a4af94297851a78ba880da2b 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -299,9 +299,9 @@ 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 +void FactorBase::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " Fac" << id() << " " << getType() << " -->"; + _stream << _tabs << "Fac" << id() << " " << getType() << " -->"; if ( getFrameOtherList() .empty() && getCaptureOtherList() .empty() && getFeatureOtherList() .empty() @@ -323,8 +323,8 @@ void FactorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, _stream << std::endl; } -void FactorBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const +void FactorBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index d89d3adb8119c9c64bdb244c6d209b04fe7e2367..d48101d9ef2db50ebca1ab217f2f4d6889816742 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -165,9 +165,9 @@ 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 +void FeatureBase::printHeader(std::string _tabs, 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 " : ""); + _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "c " : ""); if (_constr_by) { _stream << " <--\t"; @@ -176,16 +176,16 @@ void FeatureBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by } _stream << std::endl; if (_metric) - _stream << " m = ( " << std::setprecision(2) << getMeasurement().transpose() + _stream << _tabs << " " << "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 +void FeatureBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); if (_depth >= 4) for (auto c : getFactorList()) - c->print(_depth, _stream, _constr_by, _metric, _state_blocks); + c->print(_tabs + " ", _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 233ea4434a1739544af946c9680c61215e57eb72..f34fa3a2b9726ceb50dbea158797307a6eef1d38 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -359,9 +359,9 @@ 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 +void FrameBase::printHeader(std::string _tabs, 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 " : ""); + _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : ""); if (_constr_by) { _stream << " <-- "; @@ -379,14 +379,14 @@ void FrameBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, } else if (_metric) { - _stream << (isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5) + _stream << _tabs << " " << (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:"; + _stream << _tabs << " " << "sb:"; for (const auto& key : getStructure()) { const auto& sb = getStateBlock(key); @@ -396,11 +396,11 @@ void FrameBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, } } -void FrameBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const +void FrameBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); if (_depth >= 2) for (auto C : getCaptureList()) - C->print(_depth, _stream, _constr_by, _metric, _state_blocks); + C->print(_tabs + " ", _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp index 302809be3f82ae89b17fa8427e69ac543ce4cb3f..740aabe26da52ce5a6588b5ff53cc804f381ba15 100644 --- a/src/hardware/hardware_base.cpp +++ b/src/hardware/hardware_base.cpp @@ -20,16 +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 +void HardwareBase::printHeader(std::string _tabs, 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; + _stream << _tabs << "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 +void HardwareBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); if (_depth >= 1) for (auto S : getSensorList()) - S->print(_depth, _stream, _constr_by, _metric, _state_blocks); + S->print(_tabs + " ", _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 0750fa7aef8fb10d0b87325f0b4f3dbbc0212692..0941ece13d5ffceb48fce987d04da6b3d93ab89b 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -153,9 +153,9 @@ 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 +void LandmarkBase::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " Lmk" << id() << " " << getType(); + _stream << _tabs << "Lmk" << id() << " " << getType(); if (_constr_by) { _stream << "\t<-- "; @@ -173,13 +173,13 @@ void LandmarkBase::printHeader(int _depth, std::ostream& _stream, bool _constr_b } else if (_metric) { - _stream << (isFixed() ? " Fix" : " Est"); + _stream << _tabs << " " << (isFixed() ? "Fix" : "Est"); _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )"; _stream << std::endl; } else if (_state_blocks) { - _stream << " sb:"; + _stream << _tabs << " " << "sb:"; for (const auto& key : getStructure()) { const auto& sb = getStateBlock(key); @@ -189,9 +189,9 @@ void LandmarkBase::printHeader(int _depth, std::ostream& _stream, bool _constr_b } } -void LandmarkBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const +void LandmarkBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); } LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node) { diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp index 34c4ddf18946321e808f16b5550653449693a5a5..791438acf0aca6686d85ebe08272bc916256e099 100644 --- a/src/map/map_base.cpp +++ b/src/map/map_base.cpp @@ -90,15 +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 +void MapBase::printHeader(std::string _tabs, 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; + _stream << _tabs << "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 +void MapBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); if (_depth >= 1) for (auto L : getLandmarkList()) - L->print(_depth, _stream, _constr_by, _metric, _state_blocks); + L->print(_tabs + " ", _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index e01471f9775095545580db2d79eab400dde41f6e..49f59f9c89cbcfa2e12be380ec4fc73ee60dda5a 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -958,11 +958,11 @@ void Problem::print(int _depth, std::ostream& _stream, bool _constr_by, bool _me _stream << std::endl; _stream << "P: wolf tree status ---------------------" << std::endl; - getHardware()->print(_depth, _stream, _constr_by, _metric, _state_blocks); + getHardware()->print("", _depth, _stream, _constr_by, _metric, _state_blocks); - getTrajectory()->print(_depth, _stream, _constr_by, _metric, _state_blocks); + getTrajectory()->print("", _depth, _stream, _constr_by, _metric, _state_blocks); - getMap()->print(_depth, _stream, _constr_by, _metric, _state_blocks); + getMap()->print("", _depth, _stream, _constr_by, _metric, _state_blocks); _stream << "-----------------------------------------" << std::endl; _stream << std::endl; diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 791876e995f1d977f96e1bcbedd05a471201681f..4d3aa3a38c249b44babb7a4f1ddb32b0a1126710 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -196,14 +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 +void ProcessorBase::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; + _stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; } -void ProcessorBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const +void ProcessorBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index a3c0bc6cf0ec911ecc256e16fe67721cbef73fe5..9ebeebe91b9556416a02c7f033c2dadf93dd85d7 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -660,17 +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 +void ProcessorMotion::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; + _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; if (getOrigin()) - _stream << " o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm") + _stream << _tabs << " " << "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") + _stream << _tabs << " " << "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; + _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl; } } diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 52dcd5c02fa16c54b74d872de2b9ce12db0b7559..5de9a091d79012f82e24cf192f30a0b5d74c168b 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -322,17 +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 +void ProcessorTracker::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; + _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; if (getOrigin()) - _stream << " o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm") + _stream << _tabs << " " << "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") + _stream << _tabs << " " << "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; + _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl; } } // namespace wolf diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 525202369ffe0a711ad53606e77c760b447b4785..f3d2499065606900264040868643df255266ec43 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -467,16 +467,16 @@ void SensorBase::link(HardwareBasePtr _hw_ptr) } } -void SensorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const +void SensorBase::printHeader(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - _stream << " Sen" << id() << " " << getType() << " \"" << getName() << "\""; + _stream << _tabs << "Sen" << id() << " " << getType() << " \"" << getName() << "\""; if (_depth < 2) _stream << " -- " << getProcessorList().size() << "p"; _stream << std::endl; if (_metric && _state_blocks) { - _stream << " sb: "; + _stream << _tabs << " " << "sb: "; for (auto& _key : getStructure()) { auto key = std::string(1,_key); @@ -487,7 +487,7 @@ void SensorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, } else if (_metric) { - _stream << " ( "; + _stream << _tabs << " " << "( "; for (auto& _key : getStructure()) { auto key = std::string(1,_key); @@ -498,7 +498,7 @@ void SensorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, } else if (_state_blocks) { - _stream << " sb: "; + _stream << _tabs << " " << "sb: "; for (auto& _key : getStructure()) { auto key = std::string(1,_key); @@ -509,12 +509,12 @@ void SensorBase::printHeader(int _depth, std::ostream& _stream, bool _constr_by, } } -void SensorBase::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const +void SensorBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); if (_depth >= 2) for (auto p : getProcessorList()) - p->print(_depth, _stream, _constr_by, _metric, _state_blocks); + p->print(_tabs + " ", _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 96b4b43f7c0bc48fea75e0e655a19143d925b6dc..3fb3ff0a89aa45c7e8da6d0e2e4a55f09606d174 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -136,16 +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 +void TrajectoryBase::printHeader(std::string _tabs, 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; + _stream << _tabs << "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 +void TrajectoryBase::print(std::string _tabs, int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const { - printHeader(_depth, _stream, _constr_by, _metric, _state_blocks); + printHeader(_tabs, _depth, _stream, _constr_by, _metric, _state_blocks); if (_depth >= 1) for (auto F : getFrameList()) - F->print(_depth, _stream, _constr_by, _metric, _state_blocks); + F->print(_tabs + " ", _depth, _stream, _constr_by, _metric, _state_blocks); } } // namespace wolf