Skip to content
Snippets Groups Projects
Commit 9ad03798 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch 'timeStamp' into 'master'

Time stamp

See merge request mobile_robotics/wolf!209
parents ea7a749d 0cde075b
No related branches found
No related tags found
1 merge request!209Time stamp
...@@ -13,7 +13,7 @@ FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated, ...@@ -13,7 +13,7 @@ FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
// //
} }
const Eigen::VectorXs& FeatureDiffDrive::getJacobianFactor() const const Eigen::MatrixXs& FeatureDiffDrive::getJacobianFactor() const
{ {
return jacobian_diff_drive_factors_; return jacobian_diff_drive_factors_;
} }
......
...@@ -26,12 +26,12 @@ public: ...@@ -26,12 +26,12 @@ public:
virtual ~FeatureDiffDrive() = default; virtual ~FeatureDiffDrive() = default;
const Eigen::VectorXs& getJacobianFactor() const; const Eigen::MatrixXs& getJacobianFactor() const;
protected: protected:
Eigen::VectorXs diff_drive_factors_; Eigen::VectorXs diff_drive_factors_;
Eigen::VectorXs jacobian_diff_drive_factors_; Eigen::MatrixXs jacobian_diff_drive_factors_;
}; };
} /* namespace wolf */ } /* namespace wolf */
......
...@@ -33,7 +33,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, ...@@ -33,7 +33,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true); StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true); StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, true); StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, false);
SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params); SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params);
...@@ -41,7 +41,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, ...@@ -41,7 +41,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
/// @todo make calibration optional at creation /// @todo make calibration optional at creation
//if (calibrate) //if (calibrate)
odo->unfixIntrinsics(); // odo->unfixIntrinsics();
return odo; return odo;
} }
......
...@@ -14,6 +14,9 @@ ...@@ -14,6 +14,9 @@
namespace wolf { namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive);
WOLF_STRUCT_PTR_TYPEDEFS(SensorDiffDrive);
struct IntrinsicsDiffDrive : public IntrinsicsBase struct IntrinsicsDiffDrive : public IntrinsicsBase
{ {
Scalar left_radius_; Scalar left_radius_;
......
...@@ -354,12 +354,16 @@ TEST(Odom2D, KF_callback) ...@@ -354,12 +354,16 @@ TEST(Odom2D, KF_callback)
// Capture to use as container for all incoming data // 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); 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++) for (int n=1; n<=N; n++)
{ {
t += dt; t += dt;
// re-use capture with updated timestamp // re-use capture with updated timestamp
capture->setTimeStamp(t); 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 // Processor
sensor_odom2d->process(capture); sensor_odom2d->process(capture);
...@@ -388,9 +392,9 @@ TEST(Odom2D, KF_callback) ...@@ -388,9 +392,9 @@ TEST(Odom2D, KF_callback)
//////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////
// Split after the last keyframe, exact timestamp // Split after the last keyframe, exact timestamp
int n_split = 8; 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); Vector3s x_split = processor_odom2d->getState(t_split);
FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
...@@ -418,7 +422,7 @@ TEST(Odom2D, KF_callback) ...@@ -418,7 +422,7 @@ TEST(Odom2D, KF_callback)
// Split between keyframes, exact timestamp // Split between keyframes, exact timestamp
int m_split = 4; int m_split = 4;
t_split = t0 + m_split*dt; 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); problem->print(4,1,1,1);
...@@ -445,6 +449,8 @@ TEST(Odom2D, KF_callback) ...@@ -445,6 +449,8 @@ TEST(Odom2D, KF_callback)
report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(4,1,1,1);
// check the split KF // check the split KF
ASSERT_POSE2D_APPROX(keyframe_1->getState() , integrated_pose_vector[m_split], 1e-6); 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 ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6); // FIXME test does not pass
......
...@@ -39,7 +39,7 @@ TEST(WolfTestTimeStamp, TimeStampInitScalar) ...@@ -39,7 +39,7 @@ TEST(WolfTestTimeStamp, TimeStampInitScalar)
std::stringstream ss; std::stringstream ss;
start.print(ss); start.print(ss);
ASSERT_STREQ("101010.0000000000", ss.str().c_str()); ASSERT_STREQ("101010.000000000", ss.str().c_str());
// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n"); // PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n");
} }
...@@ -61,11 +61,58 @@ TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano) ...@@ -61,11 +61,58 @@ TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano)
std::stringstream ss; std::stringstream ss;
start.print(ss); start.print(ss);
ASSERT_STREQ("101010.0002020200", ss.str().c_str()); ASSERT_STREQ("101010.000202020", ss.str().c_str());
// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n"); // PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n");
} }
TEST(WolfTestTimeStamp, TimeStampSetNow)
{
wolf::TimeStamp t1;
wolf::TimeStamp t2(t1);
ASSERT_EQ(t1,t2);
// If we don't sleep, start == time_stamp sometimes.
// And sometimes start <= time_stamp ...
std::this_thread::sleep_for(std::chrono::microseconds(1));
t2.setToNow();
ASSERT_LT(t1,t2);
}
TEST(WolfTestTimeStamp, TimeStampSetScalar)
{
wolf::Scalar val(101010.000202020);
wolf::TimeStamp start;
start.set(val);
ASSERT_EQ(start.get(), val);
ASSERT_EQ(start.getSeconds(), 101010);
ASSERT_EQ(start.getNanoSeconds(), 202020);
std::stringstream ss;
start.print(ss);
ASSERT_STREQ("101010.000202020", ss.str().c_str());
}
TEST(WolfTestTimeStamp, TimeStampSetSecNano)
{
unsigned long int sec(101010);
unsigned long int nano(202020);
wolf::TimeStamp start;
start.set(sec,nano);
// start.get -> 101010.000202020004508
ASSERT_EQ(start.getSeconds(), sec);
ASSERT_EQ(start.getNanoSeconds(), nano);
}
TEST(WolfTestTimeStamp, TimeStampEquality) TEST(WolfTestTimeStamp, TimeStampEquality)
{ {
wolf::TimeStamp start; wolf::TimeStamp start;
...@@ -111,14 +158,40 @@ TEST(WolfTestTimeStamp, TimeStampInequality) ...@@ -111,14 +158,40 @@ TEST(WolfTestTimeStamp, TimeStampInequality)
// PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n"); // PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n");
} }
TEST(WolfTestTimeStamp, TimeStampSubstraction)
{
wolf::TimeStamp t1;
wolf::TimeStamp t2(t1);
wolf::Scalar dt(1e-5);
t2+=dt;
ASSERT_LT(t1, t2);
ASSERT_EQ(t2-t1, dt);
ASSERT_EQ(t1-t2, -dt);
}
TEST(WolfTestTimeStamp, TimeStampAdding)
{
wolf::TimeStamp t1,t3;
wolf::TimeStamp t2(t1);
wolf::Scalar dt(1e-5);
t2 +=dt;
t3 = t1+dt;
ASSERT_EQ(t2, t3);
}
TEST(WolfTestTimeStamp, TimeStampOperatorOstream) TEST(WolfTestTimeStamp, TimeStampOperatorOstream)
{ {
wolf::TimeStamp t(5); wolf::TimeStamp t(5);
wolf::Scalar dt = 1e-4; wolf::Scalar dt = 1e-4;
t+=dt; t+=dt;
std::ostringstream ss1, ss2; std::ostringstream ss1, ss2;
t.print(ss1);
ss1 << t.get();
ss2 << t; ss2 << t;
ASSERT_EQ(ss1.str(), ss2.str()); ASSERT_EQ(ss1.str(), ss2.str());
...@@ -126,6 +199,19 @@ TEST(WolfTestTimeStamp, TimeStampOperatorOstream) ...@@ -126,6 +199,19 @@ TEST(WolfTestTimeStamp, TimeStampOperatorOstream)
// PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n"); // PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n");
} }
TEST(WolfTestTimeStamp, TimeStampSecNanoSec)
{
unsigned long int sec = 5;
unsigned long int nano = 1e5;
wolf::TimeStamp t1(wolf::Scalar(sec)+wolf::Scalar(nano)*1e-9);
wolf::TimeStamp t2(sec,nano);
ASSERT_EQ(t1.getSeconds(),sec);
ASSERT_EQ(t2.getSeconds(),sec);
ASSERT_EQ(t1.getNanoSeconds(),nano);
ASSERT_EQ(t2.getNanoSeconds(),nano);
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
......
...@@ -5,35 +5,33 @@ namespace wolf { ...@@ -5,35 +5,33 @@ namespace wolf {
std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts) std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
{ {
// std::ios_base::fmtflags fmtfl; os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream
//
// //get/set ostream flags and precision digits
// fmtfl = os.flags(std::ios::left);
// os.setf(std::ios::fixed, std::ios::floatfield);
// std::streamsize nn;
// nn = os.precision(TimeStamp::TIME_STAMP_DIGITS_);
os << _ts.getSeconds() << "." << _ts.getNanoSeconds(); // write obj to stream
return os; return os;
} }
TimeStamp::TimeStamp() : TimeStamp::TimeStamp() :
time_stamp_(0) //time_stamp_(0)
time_stamp_nano_(0)
{ {
setToNow(); setToNow();
} }
TimeStamp::TimeStamp(const Scalar _ts) : TimeStamp::TimeStamp(const TimeStamp& _ts) :
time_stamp_(_ts) time_stamp_nano_(_ts.time_stamp_nano_)
{
//
}
TimeStamp::TimeStamp(const Scalar& _ts) :
time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0)
{ {
// //
} }
TimeStamp::TimeStamp(const unsigned long int _sec, const unsigned long int _nsec) : TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) :
time_stamp_((Scalar)_sec + (Scalar)_nsec/(Scalar)1e9) time_stamp_nano_(_sec*NANOSECS+_nsec)
{ {
// //
} }
...@@ -47,39 +45,32 @@ void TimeStamp::setToNow() ...@@ -47,39 +45,32 @@ void TimeStamp::setToNow()
{ {
timeval ts; timeval ts;
gettimeofday(&ts, NULL); gettimeofday(&ts, NULL);
time_stamp_ = (Scalar)((ts.tv_sec)) + (Scalar)((ts.tv_usec)) / 1e6; set(ts);
} }
unsigned long int TimeStamp::getSeconds() const TimeStamp TimeStamp::operator +(const Scalar& dt) const
{ {
unsigned long int ts; TimeStamp ts(*this);
ts = (unsigned long int)(((floor(time_stamp_)))); ts += dt;
return ts; return ts;
} }
unsigned long int TimeStamp::getNanoSeconds() const TimeStamp TimeStamp::operator -(const Scalar& dt) const
{ {
Scalar ts; TimeStamp ts(*this);
ts = (Scalar)(((floor(time_stamp_)))); ts -= dt;
return (unsigned long int)(((time_stamp_ - ts) * 1e9)); return ts;
} }
void TimeStamp::print(std::ostream & ost) const inline void TimeStamp::operator -=(const Scalar& dt)
{ {
std::streamsize nn; unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
std::ios_base::fmtflags fmtfl; time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
}
//get/set ostream flags and precision digits
fmtfl = ost.flags(std::ios::left);
ost.setf(std::ios::fixed, std::ios::floatfield);
nn = ost.precision(TIME_STAMP_DIGITS_);
//send to ostream
ost << this->time_stamp_;
//restore flags and precision void TimeStamp::print(std::ostream & ost) const
ost.flags(fmtfl); {
ost.precision(nn); ost << *this;
} }
} // namespace wolf } // namespace wolf
...@@ -8,7 +8,9 @@ ...@@ -8,7 +8,9 @@
//C, std //C, std
#include <sys/time.h> #include <sys/time.h>
#include <iostream> #include <iostream>
#include <iomanip>
static const unsigned int NANOSECS = 1000000000;
namespace wolf { namespace wolf {
...@@ -19,9 +21,8 @@ namespace wolf { ...@@ -19,9 +21,8 @@ namespace wolf {
*/ */
class TimeStamp class TimeStamp
{ {
private: protected:
Scalar time_stamp_; ///< Time stamp. Expressed in seconds from 1th jan 1970. unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
static const unsigned int TIME_STAMP_DIGITS_ = 10; ///< Number of digits to print time stamp values
public: public:
/** \brief Constructor /** \brief Constructor
...@@ -31,19 +32,26 @@ class TimeStamp ...@@ -31,19 +32,26 @@ class TimeStamp
*/ */
TimeStamp(); TimeStamp();
/** \brief Copy constructor
*
* Copy constructor
*
*/
TimeStamp(const TimeStamp& _ts);
/** \brief Constructor with argument /** \brief Constructor with argument
* *
* Constructor with arguments * Constructor with arguments
* *
*/ */
TimeStamp(const Scalar _ts); TimeStamp(const Scalar& _ts);
/** \brief Constructor from sec and nsec /** \brief Constructor from sec and nsec
* *
* Constructor from sec and nsec * Constructor from sec and nsec
* *
*/ */
TimeStamp(const unsigned long int _sec, const unsigned long int _nsec); TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec);
/** \brief Destructor /** \brief Destructor
* *
...@@ -67,14 +75,14 @@ class TimeStamp ...@@ -67,14 +75,14 @@ class TimeStamp
* Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds) * Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds)
* *
*/ */
void set(const unsigned long int sec, const unsigned long int nanosec); void set(const unsigned long int& sec, const unsigned long int& nanosec);
/** \brief Set time stamp /** \brief Set time stamp
* *
* Sets time stamp to a given value passed as a scalar_t (seconds) * Sets time stamp to a given value passed as a scalar_t (seconds)
* *
*/ */
void set(const Scalar ts); void set(const Scalar& ts);
/** \brief Get time stamp /** \brief Get time stamp
* *
...@@ -129,13 +137,6 @@ class TimeStamp ...@@ -129,13 +137,6 @@ class TimeStamp
bool operator <=(const TimeStamp& ts) const; bool operator <=(const TimeStamp& ts) const;
bool operator >=(const TimeStamp& ts) const; bool operator >=(const TimeStamp& ts) const;
/** \brief difference operator
*
* difference operator that returns a scalar_t (seconds)
*
*/
Scalar operator -(const TimeStamp& ts) const;
/** \brief Add-assign operator given a scalar_t (seconds) /** \brief Add-assign operator given a scalar_t (seconds)
*/ */
void operator +=(const Scalar& dt); void operator +=(const Scalar& dt);
...@@ -144,6 +145,24 @@ class TimeStamp ...@@ -144,6 +145,24 @@ class TimeStamp
*/ */
TimeStamp operator +(const Scalar& dt) const; TimeStamp operator +(const Scalar& dt) const;
/** \brief Substraction-assign operator given a scalar_t (seconds)
*/
void operator -=(const Scalar& dt);
/** \brief difference operator
*
* difference operator that returns a scalar_t (seconds)
*
*/
TimeStamp operator -(const Scalar& ts) const;
/** \brief difference operator
*
* difference operator that returns a Timestamp (seconds)
*
*/
Scalar operator -(const TimeStamp& ts) const;
/** \brief Prints time stamp to a given ostream /** \brief Prints time stamp to a given ostream
* *
* Prints time stamp to a given ostream * Prints time stamp to a given ostream
...@@ -155,79 +174,84 @@ class TimeStamp ...@@ -155,79 +174,84 @@ class TimeStamp
}; };
inline void TimeStamp::set(const Scalar ts) inline void TimeStamp::set(const Scalar& ts)
{ {
time_stamp_ = ts; 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) inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec)
{ {
time_stamp_ = (Scalar)(sec) + (Scalar)(nanosec) / (Scalar)(1e9); time_stamp_nano_ = sec*NANOSECS+nanosec;
} }
inline void TimeStamp::set(const timeval& ts) inline void TimeStamp::set(const timeval& ts)
{ {
time_stamp_ = (Scalar)(ts.tv_sec) + (Scalar)(ts.tv_usec) / 1e6; time_stamp_nano_ = (unsigned long int)(ts.tv_sec*NANOSECS) + (unsigned long int)(ts.tv_usec*1000);
} }
inline Scalar TimeStamp::get() const inline Scalar TimeStamp::get() const
{ {
return time_stamp_; return ((Scalar)( time_stamp_nano_))*1e-9;
}
inline unsigned long int TimeStamp::getSeconds() const
{
return time_stamp_nano_ / NANOSECS;
}
inline unsigned long int TimeStamp::getNanoSeconds() const
{
return time_stamp_nano_ % NANOSECS;
} }
inline void TimeStamp::operator =(const TimeStamp& ts) inline void TimeStamp::operator =(const TimeStamp& ts)
{ {
time_stamp_ = ts.get(); time_stamp_nano_ = ts.time_stamp_nano_;
} }
inline void TimeStamp::operator =(const Scalar& ts) inline void TimeStamp::operator =(const Scalar& ts)
{ {
time_stamp_ = ts; time_stamp_nano_ = (unsigned long int)(ts*NANOSECS);
} }
inline bool TimeStamp::operator ==(const TimeStamp& ts) const inline bool TimeStamp::operator ==(const TimeStamp& ts) const
{ {
return (time_stamp_ == ts.get()); return (time_stamp_nano_ == ts.time_stamp_nano_);
} }
inline bool TimeStamp::operator !=(const TimeStamp& ts) const inline bool TimeStamp::operator !=(const TimeStamp& ts) const
{ {
return (time_stamp_ != ts.get()); return (time_stamp_nano_ != ts.time_stamp_nano_);
} }
inline bool TimeStamp::operator <(const TimeStamp& ts) const inline bool TimeStamp::operator <(const TimeStamp& ts) const
{ {
return (time_stamp_ < ts.get()); return (time_stamp_nano_ < ts.time_stamp_nano_);
} }
inline bool TimeStamp::operator >(const TimeStamp& ts) const inline bool TimeStamp::operator >(const TimeStamp& ts) const
{ {
return (time_stamp_ > ts.get()); return (time_stamp_nano_ > ts.time_stamp_nano_);
} }
inline bool TimeStamp::operator <=(const TimeStamp& ts) const inline bool TimeStamp::operator <=(const TimeStamp& ts) const
{ {
return (time_stamp_ <= ts.get()); return (time_stamp_nano_ <= ts.time_stamp_nano_);
} }
inline bool TimeStamp::operator >=(const TimeStamp& ts) const inline bool TimeStamp::operator >=(const TimeStamp& ts) const
{ {
return (time_stamp_ >= ts.get()); return (time_stamp_nano_ >= ts.time_stamp_nano_);
}
inline Scalar TimeStamp::operator -(const TimeStamp& ts) const
{
return (time_stamp_ - ts.get());
} }
inline void TimeStamp::operator +=(const Scalar& dt) inline void TimeStamp::operator +=(const Scalar& dt)
{ {
time_stamp_ += dt; time_stamp_nano_ += (unsigned long int)(dt*NANOSECS);
} }
inline TimeStamp TimeStamp::operator +(const Scalar& dt) const inline Scalar TimeStamp::operator -(const TimeStamp& ts) const
{ {
return TimeStamp(time_stamp_ + dt); return Scalar((long int)(time_stamp_nano_ - ts.time_stamp_nano_))*1e-9; // long int cast fix overflow in case of negative substraction result
} }
static const TimeStamp InvalidStamp(-1,-1); static const TimeStamp InvalidStamp(-1,-1);
......
...@@ -88,7 +88,8 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) ...@@ -88,7 +88,8 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
if ((*frm_rit)->isKey()) 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) if (dt < min_dt)
{ {
min_dt = dt; min_dt = dt;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment