diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp index 2f3da744717fa0c9a8d99e6d163a09f0a16ea77c..ef833a48a77679f658627edc3be1a7a68f972769 100644 --- a/src/test/gtest_odom_2D.cpp +++ b/src/test/gtest_odom_2D.cpp @@ -354,12 +354,16 @@ TEST(Odom2D, KF_callback) // Capture to use as container for all incoming data CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); + std::cout << "t: " << t << std::endl; for (int n=1; n<=N; n++) { t += dt; // re-use capture with updated timestamp capture->setTimeStamp(t); + std::cout << "capture ts: " << capture->getTimeStamp() << " - " << capture->getTimeStamp().get(); + std::cout << "nsec: " << capture->getTimeStamp().getNanoSeconds() << std::endl; + std::cout << "filled nsec: " << std::setfill('0') << std::setw(9) << std::right << capture->getTimeStamp().getNanoSeconds() << std::endl; // Processor sensor_odom2d->process(capture); @@ -388,9 +392,9 @@ TEST(Odom2D, KF_callback) //////////////////////////////////////////////////////////////// // Split after the last keyframe, exact timestamp int n_split = 8; - TimeStamp t_split (t0 + n_split*dt); + TimeStamp t_split = t0 + n_split*dt; -// std::cout << "-----------------------------\nSplit after last KF; time: " << t_split - t0 << std::endl; + std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl; Vector3s x_split = processor_odom2d->getState(t_split); FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); @@ -418,7 +422,7 @@ TEST(Odom2D, KF_callback) // Split between keyframes, exact timestamp int m_split = 4; t_split = t0 + m_split*dt; -// std::cout << "-----------------------------\nSplit between KFs; time: " << t_split - t0 << std::endl; + std::cout << "-----------------------------\nSplit between KFs; time: " << t_split << std::endl; problem->print(4,1,1,1); @@ -445,6 +449,8 @@ TEST(Odom2D, KF_callback) report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + problem->print(4,1,1,1); + // check the split KF ASSERT_POSE2D_APPROX(keyframe_1->getState() , integrated_pose_vector[m_split], 1e-6); ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6); // FIXME test does not pass diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp index 643c1de6c20deb495455719103fb190794b241e5..861104832b76751b1034ce36c4c794efae2d0454 100644 --- a/src/time_stamp.cpp +++ b/src/time_stamp.cpp @@ -14,7 +14,7 @@ std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts) // std::streamsize nn; // nn = os.precision(TimeStamp::TIME_STAMP_DIGITS_); - os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) <<_ts.getNanoSeconds(); // write obj to stream + os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream return os; } @@ -35,7 +35,7 @@ TimeStamp::TimeStamp(const TimeStamp& _ts) : TimeStamp::TimeStamp(const Scalar& _ts) : //time_stamp_(_ts) - time_stamp_nano_((unsigned long int)(_ts*1e9)) + time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0) { // } @@ -90,7 +90,7 @@ void TimeStamp::print(std::ostream & ost) const //ost.flags(fmtfl); //ost.precision(nn); - ost << this->getSeconds() << "." << std::setfill('0') << std::setw(9) << this->getNanoSeconds(); + ost << *this; } } // namespace wolf diff --git a/src/time_stamp.h b/src/time_stamp.h index fd5593404036e97b03b705c3953ad02fa18df69d..88c65a1cd4387ab049e9a78300932e893d3d5dc5 100644 --- a/src/time_stamp.h +++ b/src/time_stamp.h @@ -179,7 +179,7 @@ class TimeStamp inline void TimeStamp::set(const Scalar& ts) { //time_stamp_ = ts; - time_stamp_nano_ = (unsigned long int)(ts*NANOSECS); + time_stamp_nano_ = (ts > 0 ? (unsigned long int)(ts*NANOSECS) : 0); } inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec) @@ -267,13 +267,13 @@ inline bool TimeStamp::operator >=(const TimeStamp& ts) const inline void TimeStamp::operator +=(const Scalar& dt) { //time_stamp_ += dt; - time_stamp_nano_ += (unsigned long int)(dt*NANOSECS); + time_stamp_nano_ += (unsigned long int)(dt*1e9); } inline void TimeStamp::operator -=(const Scalar& dt) { //time_stamp_ -= dt; - time_stamp_nano_ -= (unsigned long int)(dt*NANOSECS); + time_stamp_nano_ -= (unsigned long int)(dt*1e9); } inline Scalar TimeStamp::operator -(const TimeStamp& ts) const diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp index 577291e4f4621d95e1c42c7c66b7ffa83a4735b5..df3a9f5f7d932c5049ba093c59d3206ab19ddcc8 100644 --- a/src/trajectory_base.cpp +++ b/src/trajectory_base.cpp @@ -88,7 +88,8 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) if ((*frm_rit)->isKey()) { - Scalar dt = std::abs((*frm_rit)->getTimeStamp().get() - _ts.get()); + Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); + std::cout << "dt " << dt << std::endl; if (dt < min_dt) { min_dt = dt;