Merge branch 'timeStamp' into 'master'

Time stamp

See merge request mobile_robotics/wolf!209

src/features/feature_diff_drive.cpp

@@ -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_;
 }

src/features/feature_diff_drive.h

@@ -26,12 +26,12 @@ public:
 
   virtual ~FeatureDiffDrive() = default;
 
-  const Eigen::VectorXs& getJacobianFactor() const;
+  const Eigen::MatrixXs& getJacobianFactor() const;
 
 protected:
 
   Eigen::VectorXs diff_drive_factors_;
-  Eigen::VectorXs jacobian_diff_drive_factors_;
+  Eigen::MatrixXs jacobian_diff_drive_factors_;
 };
 
 } /* namespace wolf */

src/sensors/sensor_diff_drive.cpp

@@ -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 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);
 
@@ -41,7 +41,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
 
   /// @todo make calibration optional at creation
   //if (calibrate)
-    odo->unfixIntrinsics();
+  //  odo->unfixIntrinsics();
 
   return odo;
 }

src/sensors/sensor_diff_drive.h

@@ -14,6 +14,9 @@
 
 namespace wolf {
 
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive);
+WOLF_STRUCT_PTR_TYPEDEFS(SensorDiffDrive);
+
 struct IntrinsicsDiffDrive : public IntrinsicsBase
 {
   Scalar left_radius_;

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

src/test/gtest_time_stamp.cpp

@@ -39,7 +39,7 @@ TEST(WolfTestTimeStamp, TimeStampInitScalar)
   std::stringstream 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");
 }
@@ -61,11 +61,58 @@ TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano)
   std::stringstream 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");
 }
 
+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)
 {
   wolf::TimeStamp start;
@@ -111,14 +158,40 @@ TEST(WolfTestTimeStamp, TimeStampInequality)
 //  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)
 {
     wolf::TimeStamp t(5);
     wolf::Scalar dt = 1e-4;
     t+=dt;
+
     std::ostringstream ss1, ss2;
+    t.print(ss1);
 
-    ss1 << t.get();
     ss2 << t;
 
     ASSERT_EQ(ss1.str(), ss2.str());
@@ -126,6 +199,19 @@ TEST(WolfTestTimeStamp, TimeStampOperatorOstream)
 //    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)
 {
   testing::InitGoogleTest(&argc, argv);

src/time_stamp.cpp

@@ -5,35 +5,33 @@ namespace wolf {
 
 std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
 {
-    //    std::ios_base::fmtflags fmtfl;
-    //
-    //    //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
+    os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream
     return os;
 }
 
 
 
 TimeStamp::TimeStamp() :
-        time_stamp_(0)
+        //time_stamp_(0)
+        time_stamp_nano_(0)
 {
     setToNow();
 }
 
-TimeStamp::TimeStamp(const Scalar _ts) :
-        time_stamp_(_ts)
+TimeStamp::TimeStamp(const TimeStamp& _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) :
-        time_stamp_((Scalar)_sec + (Scalar)_nsec/(Scalar)1e9)
+TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) :
+        time_stamp_nano_(_sec*NANOSECS+_nsec)
 {
     //
 }
@@ -47,39 +45,32 @@ void TimeStamp::setToNow()
 {
     timeval ts;
     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;
-    ts = (unsigned long int)(((floor(time_stamp_))));
+    TimeStamp ts(*this);
+    ts += dt;
     return ts;
 }
 
-unsigned long int TimeStamp::getNanoSeconds() const
+TimeStamp TimeStamp::operator -(const Scalar& dt) const
 {
-    Scalar ts;
-    ts = (Scalar)(((floor(time_stamp_))));
-    return (unsigned long int)(((time_stamp_ - ts) * 1e9));
+    TimeStamp ts(*this);
+    ts -= dt;
+    return ts;
 }
 
-void TimeStamp::print(std::ostream & ost) const
+inline void TimeStamp::operator -=(const Scalar& dt)
 {
-    std::streamsize nn;
-    std::ios_base::fmtflags fmtfl;
-
-    //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_;
+    unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
+    time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
+}
 
-    //restore flags and precision
-    ost.flags(fmtfl);
-    ost.precision(nn);
+void TimeStamp::print(std::ostream & ost) const
+{
+    ost << *this;
 }
 
 } // namespace wolf

src/time_stamp.h

@@ -8,7 +8,9 @@
 //C, std
 #include <sys/time.h>
 #include <iostream>
+#include <iomanip>
 
+static const unsigned int NANOSECS = 1000000000;
 
 namespace wolf {
 
@@ -19,9 +21,8 @@ namespace wolf {
  */
 class TimeStamp
 {
-    private:
-        Scalar time_stamp_; ///< Time stamp. Expressed in seconds from 1th jan 1970.
-        static const unsigned int TIME_STAMP_DIGITS_ = 10; ///< Number of digits to print time stamp values        
+    protected:
+        unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
 
     public:
         /** \brief Constructor
@@ -31,19 +32,26 @@ class TimeStamp
          */
         TimeStamp();
 
+        /** \brief Copy constructor
+         *
+         * Copy constructor
+         *
+         */
+        TimeStamp(const TimeStamp& _ts);
+
         /** \brief Constructor with argument
          *
          * Constructor with arguments
          *
          */
-        TimeStamp(const Scalar _ts);
+        TimeStamp(const Scalar& _ts);
 
         /** \brief 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
          *
@@ -67,14 +75,14 @@ class TimeStamp
          * 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
          *
          * 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
          *
@@ -129,13 +137,6 @@ class TimeStamp
         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)
          */
         void operator +=(const Scalar& dt);
@@ -144,6 +145,24 @@ class TimeStamp
          */
         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
          *
          * Prints time stamp to a given ostream
@@ -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)
 {
-    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
 {
-    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)
 {
-    time_stamp_ = ts.get();
+    time_stamp_nano_ = ts.time_stamp_nano_;
 }
 
 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
 {
-    return (time_stamp_ == ts.get());
+    return (time_stamp_nano_ == ts.time_stamp_nano_);
 }
 
 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
 {
-    return (time_stamp_ < ts.get());
+    return (time_stamp_nano_ < ts.time_stamp_nano_);
 }
 
 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
 {
-    return (time_stamp_ <= ts.get());
+    return (time_stamp_nano_ <= ts.time_stamp_nano_);
 }
 
 inline bool TimeStamp::operator >=(const TimeStamp& ts) const
 {
-    return (time_stamp_ >= ts.get());
-}
-
-inline Scalar TimeStamp::operator -(const TimeStamp& ts) const
-{
-    return (time_stamp_ - ts.get());
+    return (time_stamp_nano_ >= ts.time_stamp_nano_);
 }
 
 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);

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;