diff --git a/src/features/feature_diff_drive.cpp b/src/features/feature_diff_drive.cpp
index ae1f8a2b3a3298824ffd6703786d901b272a88c6..c33d5fdec4e487e4e1b8893625ba140b60b4f54e 100644
--- a/src/features/feature_diff_drive.cpp
+++ b/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_;
}
diff --git a/src/features/feature_diff_drive.h b/src/features/feature_diff_drive.h
index ac13c490717c66b54c652d9f93e49387672a1d1c..5c837469b577cf411eb8c830e2941823d1a0d91f 100644
--- a/src/features/feature_diff_drive.h
+++ b/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 */
diff --git a/src/sensors/sensor_diff_drive.cpp b/src/sensors/sensor_diff_drive.cpp
index 0931b700ce658b486823fa930e3f1c454f87be5e..4c57144c96375f6c077d90ea20fde5ad027e488d 100644
--- a/src/sensors/sensor_diff_drive.cpp
+++ b/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;
}
diff --git a/src/sensors/sensor_diff_drive.h b/src/sensors/sensor_diff_drive.h
index f4556a84a16c7a347f27a6f7f51ce913c02dc477..749772902dcf39e035b318705a4797028c896d5b 100644
--- a/src/sensors/sensor_diff_drive.h
+++ b/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_;
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/test/gtest_time_stamp.cpp b/src/test/gtest_time_stamp.cpp
index 73a69606e77a8dd45b33bf8789a742db06f062a3..a5feaf39b5e6467866ee89235ccd9579ffa6f568 100644
--- a/src/test/gtest_time_stamp.cpp
+++ b/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);
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
index 738ff922b19ba77cfefc2bc19117fa294011a2e2..3ec68441b044a87ea38725708ed3f5b424b0f326 100644
--- a/src/time_stamp.cpp
+++ b/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
diff --git a/src/time_stamp.h b/src/time_stamp.h
index d3902e82f2732bfc63c23a3b223a721435bdadb1..d38d25b5fa10b40d0374a5c518681802fac1ffea 100644
--- a/src/time_stamp.h
+++ b/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);
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;