diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 8cb2ea566e8682395cd20dbc278ac93ad41c2060..e418b30c14377b15dff3eb606911a05fd7118cfa 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -260,7 +260,7 @@ int main()
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
- problem->print(4,1,1,1);
+ problem->print(4,0,1,0);
/*
* ============= FIRST COMMENT ON THE RESULTS ==================
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 2491d5a6a5afbdc4d3b52a3e86003c3794b6cd81..7eb1a10b19fe6ffcc169468c6feb69dd33aed69c 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,20 @@ 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, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
protected:
virtual SizeEigen computeCalibSize() const;
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index d78cd5169f3dfc523c9435c11dc65b41ef6b3591..8621a0eac7a781bb08abe419bc71fc517e886bce 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -98,8 +98,16 @@ class CaptureMotion : public CaptureBase
// Origin frame and capture
CaptureBasePtr getOriginCapture();
+ CaptureBasePtr getOriginCapture() const;
void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
+
// member data:
private:
Eigen::VectorXd data_; ///< Motion data in form of vector mandatory
@@ -163,6 +171,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..aa0164605fc80ddd0b48915a3b75e6c69c6a1b8b 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -206,6 +206,20 @@ 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, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") 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..83e343c0c3f25aaeb0219289e143bfa8f7733fae 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -106,6 +106,18 @@ 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, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") 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..32b582b04f15da321b6dd7710fda60ac9e882cac 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -131,6 +131,19 @@ 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, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") 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..3c29221acb417b3d0e13fb764207e2ae05e67190 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -27,6 +27,19 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
const SensorBasePtrList& getSensorList() const;
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") 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..15b423c3b398230697f066fead211569ee1a36d2 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -85,6 +85,20 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
*/
static LandmarkBasePtr create(const YAML::Node& _node);
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") 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..75a00a97c1a7026e51604ecca877f5b0e639f056 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -38,6 +38,18 @@ 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, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
private:
std::string dateTimeNow();
};
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 7dc2caca9fdbe5fc8527cab6814a03665b0ac89e..42dc859ad9753c44b184c3a8b1e77548fcc135c0 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -355,11 +355,6 @@ 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 )
@@ -370,7 +365,8 @@ class Problem : public std::enable_shared_from_this<Problem>
void print(int depth = 4, //
bool constr_by = false, //
bool metric = true, //
- bool state_blocks = false) const;
+ bool state_blocks = false,
+ std::ostream& stream = std::cout) const;
void print(const std::string& depth, //
bool constr_by = false, //
bool metric = true, //
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 0d0e54eef5217c037dabbf00eb4f3b87f100e283..00d5698016447ede93473c94fc36b2a57179beeb 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -361,6 +361,20 @@ 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, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
};
inline bool ProcessorBase::isVotingActive() const
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 63d55436c2c2f0ccd02f8a33cbbc378900718922..ab0e9b9cf222bbb16ab9e6713d273fbedadde8fe 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -460,6 +460,13 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
void setDistTraveled(const double& _dist_traveled);
void setAngleTurned(const double& _angle_turned);
+ void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") 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 efe8ef33837df852499491c5f524bcfed0765129..43552172e62520aef2b49962e531d1a50eea3388 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -247,6 +247,13 @@ class ProcessorTracker : public ProcessorBase
return this->params_tracker_->print();
}
+ void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
+
protected:
void computeProcessingStep();
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 1c099cb9be4827705bd43d229714ea1dabf24ed0..2bf9d2bbc902622e984a88471b62782115088fec 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -242,6 +242,20 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
Eigen::VectorXd getNoiseStd() const;
Eigen::MatrixXd getNoiseCov() const;
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") 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..8d6e69e41d741bd70bdc6e212df2490ecbc4b62c 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -47,6 +47,18 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
void sortFrame(FrameBasePtr _frm_ptr);
void updateLastFrames();
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") 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..9d4b96c3b9a096089b9ad051b4cf8a48510860c1 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -277,5 +277,74 @@ void CaptureBase::setProblem(ProblemPtr _problem)
ft->setProblem(_problem);
}
-} // namespace wolf
+void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "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(getStateBlockMap().size() > 0)
+ {
+ if (_metric && _state_blocks){
+ for (const auto& key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _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:";
+ for (const auto& key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 3)
+ for (auto f : getFeatureList())
+ f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index a736c242e7fb7047498d061d9121ce8fb56f817c..f9cda1f9ad32920b7c66c435f8fdffee1277d279 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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "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 << _tabs << " " << "sb: ";
+ _stream << (sb->isFixed() ? "Fix" : "Est");
+ if (_metric)
+ _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ }
+
+ _stream << _tabs << " " << "buffer size : " << getBuffer().get().size() << std::endl;
+ if ( _metric && ! getBuffer().get().empty())
+ {
+ _stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
+ if (hasCalibration())
+ {
+ _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 2b6fb54a6710fed79e8c608dafab1f7caaaab3af..0f65ddb4dcd0587fff4cc9cae0d0f6491c10f437 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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
} // namespace wolf
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 8dc3801efa5f93e5bd0df593442e4860cb75135e..d3aa372b7ecdae23e6d09382f12e7fd22ff7df58 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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "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 << _tabs << " " << "m = ( " << std::setprecision(2) << getMeasurement().transpose()
+ << " )" << std::endl;
+
+}
+
+void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 4)
+ for (auto c : getFactorList())
+ c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 217bfb8d4232a40af88670fac91a52b161377d32..a36417929bab661538736d837b016683c9597792 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -359,4 +359,48 @@ void FrameBase::setProblem(ProblemPtr _problem)
cap->setProblem(_problem);
}
+void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << (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 << _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 << _tabs << " " << "sb:";
+ for (const auto& key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+}
+
+void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 2)
+ for (auto C : getCaptureList())
+ C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
} // namespace wolf
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 8995fc7830b1b540c055ee1bc6aa4ca2c01c33ed..788f644c00298bfb7d7108271a2159475bc70334 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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "") << std::endl;
+
+}
+void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 1)
+ for (auto S : getSensorList())
+ S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 88626f26614c699802c891d5d2b399469641d662..56cf45857c880907415709525784ba50b85118fe 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -153,6 +153,46 @@ bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
return false;
}
+void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Lmk" << id() << " " << getType();
+ if (_constr_by)
+ {
+ _stream << "\t<-- ";
+ 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 << _tabs << " " << (isFixed() ? "Fix" : "Est");
+ _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (const auto& key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+}
+
+void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
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 e75e1147addab9c221bc80f28aa6d56bfeda8568..73efc29d7497f9e7c3fa434cc62cfef611ff9277 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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "") << std::endl;
+}
+void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 1)
+ for (auto L : getLandmarkList())
+ L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 762d98aab2fe101ac6d47eaab93d03a4809d82aa..c7b35c78394971965bf2f15476209dad3d2206c1 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1043,266 +1043,18 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+void Problem::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream) const
{
_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)
- {
- _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");
- }
- _stream << std::endl;
- }
- if (_depth >= 2)
- {
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- _stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
- 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();
- }
- }
+ getHardware()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
- _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;
+ getTrajectory()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
- 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;
- }
- }
+ getMap()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
- 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;
}
@@ -1452,7 +1204,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
{
if (_verbose)
{
- _stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- Fac" << cby->id() << " -> ";
for (const auto& Fow : cby->getFrameOtherList())
_stream << " F" << Fow.lock()->id() << std::endl;
}
@@ -1526,9 +1278,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
{
if (_verbose)
{
- _stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- Fac" << cby->id() << " -> ";
for (const auto& Cow : cby->getCaptureOtherList())
- _stream << " C" << Cow.lock()->id();
+ _stream << " Cap" << Cow.lock()->id();
_stream << std::endl;
}
@@ -1581,9 +1333,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
{
if (_verbose)
{
- _stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- Fac" << cby->id() << " -> ";
for (const auto& fow : cby->getFeatureOtherList())
- _stream << " f" << fow.lock()->id();
+ _stream << " Ftr" << fow.lock()->id();
_stream << std::endl;
}
@@ -1617,9 +1369,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
const auto& Fo = Fow.lock();
if (_verbose)
{
- _stream << " ( --> F" << Fo->id() << " <- ";
+ _stream << " ( --> Frm" << Fo->id() << " <- ";
for (auto cby : Fo->getConstrainedByList())
- _stream << " c" << cby->id();
+ _stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained frame
@@ -1642,9 +1394,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
if (_verbose)
{
- _stream << " ( --> C" << Co->id() << " <- ";
+ _stream << " ( --> Cap" << Co->id() << " <- ";
for (auto cby : Co->getConstrainedByList())
- _stream << " c" << cby->id();
+ _stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained frame
@@ -1666,9 +1418,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
const auto& fo = fow.lock();
if (_verbose)
{
- _stream << " ( --> f" << fo->id() << " <- ";
+ _stream << " ( --> Ftr" << fo->id() << " <- ";
for (auto cby : fo->getConstrainedByList())
- _stream << " c" << cby->id();
+ _stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained feature
@@ -1690,9 +1442,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
if (_verbose)
{
- _stream << " ( --> L" << Lo->id() << " <- ";
+ _stream << " ( --> Lmk" << Lo->id() << " <- ";
for (auto cby : Lo->getConstrainedByList())
- _stream << " c" << cby->id();
+ _stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained landmark
@@ -1745,12 +1497,12 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// find in own Frame
found_here = F->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " F" << F->id();
+ if (found_here && _verbose) _stream << " Frm" << F->id();
found = found || found_here;
// find in own Capture
found_here = C->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " C" << C->id();
+ if (found_here && _verbose) _stream << " Cap" << C->id();
found = found || found_here;
// Find in other Captures of the own Frame
@@ -1758,7 +1510,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
for (auto FC : F->getCaptureList())
{
found_here = FC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " F" << F->id() << ".C" << FC->id();
+ if (found_here && _verbose) _stream << " Frm" << F->id() << ".Cap" << FC->id();
found = found || found_here;
}
@@ -1766,7 +1518,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
if (S)
{
found_here = S->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " S" << S->id();
+ if (found_here && _verbose) _stream << " Sen" << S->id();
found = found || found_here;
}
@@ -1778,14 +1530,14 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
{
const auto& Fo = Fow.lock();
found_here = Fo->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Fo" << Fo->id();
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id();
found = found || found_here;
// find in feature other's captures
for (auto FoC : Fo->getCaptureList())
{
found_here = FoC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Fo" << Fo->id() << ".C" << FoC->id();
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id() << ".C" << FoC->id();
found = found || found_here;
}
@@ -1799,7 +1551,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
{
const auto& Co = Cow.lock();
found_here = Co->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Co" << Co->id();
+ if (found_here && _verbose) _stream << " CapO" << Co->id();
found = found || found_here;
}
}
@@ -1813,19 +1565,19 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// find in constrained feature's Frame
auto foF = fo->getFrame();
found_here = foF->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " foF" << foF->id();
+ if (found_here && _verbose) _stream << " FtrOF" << foF->id();
found = found || found_here;
// find in constrained feature's Capture
auto foC = fo->getCapture();
found_here = foC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " foC" << foC->id();
+ if (found_here && _verbose) _stream << " FtrOC" << foC->id();
found = found || found_here;
// find in constrained feature's Sensor
auto foS = fo->getCapture()->getSensor();
found_here = foS->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " foS" << foS->id();
+ if (found_here && _verbose) _stream << " FtrOS" << foS->id();
found = found || found_here;
}
}
@@ -1837,7 +1589,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
{
const auto& Lo = Low.lock();
found_here = Lo->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Lo" << Lo->id();
+ if (found_here && _verbose) _stream << " LmkO" << Lo->id();
found = found || found_here;
}
}
@@ -1954,10 +1706,6 @@ 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)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 364dc699740595f3f36a8af86e4d8ea290af585c..101bd5abb500ff95feec86e3c8969ab4f82301d5 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -200,4 +200,14 @@ void BufferPackKeyFrame::print(void) const
std::cout << "]" << std::endl;
}
+void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+
+}
+
+void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
} // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index bc11469c5c8aa5a5bb4c26c5a41f632c3f89312f..c917f9b7887f6463912101d483f0886d768c81c5 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -718,4 +718,17 @@ bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr)
return false;
}
+void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+ if (getOrigin())
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
+
+}
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 9750daa633b6a12a95ddf8e4034a792b816104f3..2696310df9b49f0cd621a7e3218cbc1bbaf72998 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -321,5 +321,17 @@ bool ProcessorTracker::storeCapture(CaptureBasePtr _cap_ptr)
return false;
}
+void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+ if (getOrigin())
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _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 07cd3697f6a7b3e1ae085b3a51cc158298e982b6..f40e6a4e5ca10d9d5369f06ce6e683e97c767ccc 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -467,4 +467,54 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
}
}
+void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Sen" << id() << " " << getType() << " \"" << getName() << "\"";
+ if (_depth < 2)
+ _stream << " -- " << getProcessorList().size() << "p";
+ _stream << std::endl;
+
+ if (_metric && _state_blocks)
+ {
+ _stream << _tabs << " " << "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 << _tabs << " " << "( ";
+ 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 << _tabs << " " << "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, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+
+ if (_depth >= 2)
+ for (auto p : getProcessorList())
+ p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
} // namespace wolf
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 7620c9fa29132561cb0324d60399da97355f7f67..84007a6cdbd9afece5ff51b271db25f92d4f41ff 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, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameList().size()) + "F") : "") << std::endl;
+}
+void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+ if (_depth >= 1)
+ for (auto F : getFrameList())
+ F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+
+}
} // namespace wolf
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index f0c1cbfcb2cee9f676700846dbb93fe0cd9e3521..4c004cd72e14e9e69bb343e288ab86e3a5b9354e 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -82,6 +82,15 @@ TEST(CaptureBase, addFeature)
ASSERT_EQ(C->getFeatureList().front(), f);
}
+TEST(CaptureBase, print)
+{
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
+ // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2d::Zero(), Matrix2d::Identity()));
+ auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
+
+ C->print(4, 1, 1, 1);
+}
+
TEST(CaptureBase, process)
{
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));