diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h index 4763d0bc45d513611d34775754898da9290b7aec..7e0a1203e454c7b2ed02c9e056499ddb4900aee7 100644 --- a/include/core/processor/motion_buffer.h +++ b/include/core/processor/motion_buffer.h @@ -100,7 +100,7 @@ class MotionBuffer : public std::list<Motion> const Motion& getMotion(const TimeStamp& _ts) const; void getMotion(const TimeStamp& _ts, Motion& _motion) const; void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer); - void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0); + void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0, bool show_covs = 0); }; } // namespace wolf diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp index f61c1b1f9ef7ba4d4f3609fed41fedef53e3cbb8..6afd1c5c76c9c838e3072711b69ca199da0a48b9 100644 --- a/src/processor/motion_buffer.cpp +++ b/src/processor/motion_buffer.cpp @@ -144,14 +144,14 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before } -void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs) +void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs, bool show_covs) { using std::cout; using std::endl; - if (!show_data && !show_delta && !show_delta_int && !show_jacs) + if (!show_data && !show_delta && !show_delta_int && !show_jacs && !show_covs) { - cout << "Buffer state [" << this->size() << "] : <"; + cout << "Buffer size: " << this->size() << " ; time stamps: <"; for (Motion mot : *this) cout << " " << mot.ts_; cout << " >" << endl; @@ -165,17 +165,20 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b if (show_data) { cout << " data: " << mot.data_.transpose() << endl; - cout << " data cov: \n" << mot.data_cov_ << endl; + if (show_covs) + cout << " data cov: \n" << mot.data_cov_ << endl; } if (show_delta) { cout << " delta: " << mot.delta_.transpose() << endl; - cout << " delta cov: \n" << mot.delta_cov_ << endl; + if (show_covs) + cout << " delta cov: \n" << mot.delta_cov_ << endl; } if (show_delta_int) { cout << " delta integrated: " << mot.delta_integr_.transpose() << endl; - cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl; + if (show_covs) + cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl; } if (show_jacs) {