diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 8621a0eac7a781bb08abe419bc71fc517e886bce..72ed561c556275804b89b8bb9a54c876506d1926 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -152,7 +152,7 @@ inline MotionBuffer& CaptureMotion::getBuffer()
inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const
{
- return getBuffer().get().back().jacobian_calib_;
+ return getBuffer().back().jacobian_calib_;
}
inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
@@ -193,7 +193,7 @@ inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint)
inline VectorXd CaptureMotion::getDeltaPreint() const
{
- return getBuffer().get().back().delta_integr_;
+ return getBuffer().back().delta_integr_;
}
inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
@@ -203,7 +203,7 @@ inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
inline MatrixXd CaptureMotion::getDeltaPreintCov() const
{
- return getBuffer().get().back().delta_integr_cov_;
+ return getBuffer().back().delta_integr_cov_;
}
inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index a5b41ebfe3ebf77f269148d42781b7dd96cd36a2..548fcf6e1e4f99b9d020784b226f4f1df75e4b98 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -72,30 +72,16 @@ struct Motion
* - If the query time stamp does not match any time stamp in the buffer,
* the returned motion-integral or delta-integral is the one immediately before the query time stamp.
*/
-class MotionBuffer{
+class MotionBuffer : public std::list<Motion>
+{
public:
MotionBuffer() ;
- std::list<Motion>& get();
- const std::list<Motion>& get() const;
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);
-
- private:
- std::list<Motion> container_;
};
-inline std::list<Motion>& MotionBuffer::get()
-{
- return container_;
-}
-
-inline const std::list<Motion>& MotionBuffer::get() const
-{
- return container_;
-}
-
} // namespace wolf
#endif /* SRC_MOTIONBUFFER_H_ */
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index ab0e9b9cf222bbb16ab9e6713d273fbedadde8fe..4568c6fb3e04b9f3081e27666bc7e9cf3b22e806 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -171,7 +171,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
* \param _x the returned state vector
*/
virtual void getCurrentState(Eigen::VectorXd& _x) const override;
- virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; }
+ virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().back().ts_; }
using IsMotion::getCurrentState;
using IsMotion::getCurrentTimeStamp;
@@ -526,12 +526,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
{
- return getBuffer().get().back().delta_integr_cov_;
+ return getBuffer().back().delta_integr_cov_;
}
inline Motion ProcessorMotion::getMotion() const
{
- return getBuffer().get().back();
+ return getBuffer().back();
}
inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
@@ -544,7 +544,7 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
inline double ProcessorMotion::updateDt()
{
- return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
+ return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().back().ts_;
}
inline const MotionBuffer& ProcessorMotion::getBuffer() const
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index f9cda1f9ad32920b7c66c435f8fdffee1277d279..aa7d4f23a633a01090c9a0d042e46ff153613bd6 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -54,8 +54,8 @@ CaptureMotion::~CaptureMotion()
Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
{
VectorXd calib_preint = getCalibrationPreint();
- VectorXd delta_preint = getBuffer().get().back().delta_integr_;
- MatrixXd jac_calib = getBuffer().get().back().jacobian_calib_;
+ VectorXd delta_preint = getBuffer().back().delta_integr_;
+ MatrixXd jac_calib = getBuffer().back().jacobian_calib_;
VectorXd delta_error = jac_calib * (_calib_current - calib_preint);
VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
@@ -110,8 +110,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
}
}
- _stream << _tabs << " " << "buffer size : " << getBuffer().get().size() << std::endl;
- if ( _metric && ! getBuffer().get().empty())
+ _stream << _tabs << " " << "buffer size : " << getBuffer().size() << std::endl;
+ if ( _metric && ! getBuffer().empty())
{
_stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
if (hasCalibration())
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 5d0a595673da23a73efd69d6b1c12d0734f1172b..52383b14cbf0cb7c355f416ef96a20e726bd711b 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -64,17 +64,17 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
MotionBuffer::MotionBuffer()
{
- container_.clear();
+ this->clear();
}
const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
{
- //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+ //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->rend())
// The time stamp is older than the buffer's oldest data.
// We could do something here, and throw an error or something, but by now we'll return the first valid data
previous--;
@@ -84,12 +84,12 @@ const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
{
- //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+ //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->rend())
// The time stamp is older than the buffer's oldest data.
// We could do something here, but by now we'll return the last valid data
previous--;
@@ -99,25 +99,25 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
{
- assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
- assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
+ assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
+ assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->rend())
{
// The time stamp is more recent than the buffer's most recent data:
// return an empty buffer as the _oldest_buffer
- _buffer_part_before_ts.get().clear();
+ _buffer_part_before_ts.clear();
}
else
{
// Transfer part of the buffer
- _buffer_part_before_ts.container_.splice(_buffer_part_before_ts.container_.begin(),
- container_,
- container_.begin(),
+ _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(),
+ *this,
+ this->begin(),
(previous--).base());
}
}
@@ -130,15 +130,15 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
if (!show_data && !show_delta && !show_delta_int && !show_jacs)
{
- cout << "Buffer state [" << container_.size() << "] : <";
- for (Motion mot : container_)
+ cout << "Buffer state [" << this->size() << "] : <";
+ for (Motion mot : *this)
cout << " " << mot.ts_;
cout << " >" << endl;
}
else
{
print(0,0,0,0);
- for (Motion mot : container_)
+ for (Motion mot : *this)
{
cout << "-- Motion (" << mot.ts_ << ")" << endl;
if (show_data)
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 6993332c3e23dec79c7697abe6f56a1feb531ce7..55841ede92a1bb5dbf4d49bc190b53ab3392cb64 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -148,10 +148,10 @@ FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_moti
{
auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
"ProcessorDiffDrive",
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_,
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_,
_capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().get().back().jacobian_calib_);
+ _capture_motion->getBuffer().back().jacobian_calib_);
return key_feature_ptr;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c917f9b7887f6463912101d483f0886d768c81c5..b08d88dc77683f1018a0e077c58b4bd039e9bdd3 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -63,8 +63,8 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
_capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
// start with empty motion
- TimeStamp t_zero = _capture_target->getBuffer().get().back().ts_; // last tick of previous buffer is zero tick of current buffer
- _capture_source->getBuffer().get().push_front(motionZero(t_zero));
+ TimeStamp t_zero = _capture_target->getBuffer().back().ts_; // last tick of previous buffer is zero tick of current buffer
+ _capture_source->getBuffer().push_front(motionZero(t_zero));
// Update the existing capture
_capture_source->setOriginCapture(_capture_target);
@@ -204,8 +204,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
// Modify existing feature --------
- feature_existing->setMeasurement (capture_existing->getBuffer().get().back().delta_integr_);
- feature_existing->setMeasurementCovariance(capture_existing->getBuffer().get().back().delta_integr_cov_);
+ feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
+ feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
// Modify existing factor --------
// Instead of modifying, we remove one ctr, and create a new one.
@@ -362,7 +362,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->getCalibration(),
last_ptr_);
// reset the new buffer
- capture_new->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ;
+ capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
// reset derived things
resetDerived();
@@ -414,7 +414,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
_x.resize( state_0.size() );
// Compose on top of origin state using the buffered time stamp, not the query time stamp
- double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
+ double dt = motion.ts_ - capture_motion->getBuffer().front().ts_;
statePlusDelta(state_0, delta, dt, _x);
}
else
@@ -472,7 +472,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ptr_);
// clear and reset buffer
- getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
+ getBuffer().push_back(motionZero(_origin_frame->getTimeStamp()));
// Reset derived things
resetDerived();
@@ -497,7 +497,7 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_calib_);
// integrate the current delta to pre-integrated measurements, and get Jacobians
- deltaPlusDelta(getBuffer().get().back().delta_integr_,
+ deltaPlusDelta(getBuffer().back().delta_integr_,
delta_,
dt_,
delta_integrated_,
@@ -508,17 +508,17 @@ void ProcessorMotion::integrateOneStep()
if ( hasCalibration() )
{
jacobian_calib_.noalias()
- = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
+ = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
+ jacobian_delta_ * jacobian_delta_calib_;
}
// Integrate covariance
delta_integrated_cov_.noalias()
- = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
+ = jacobian_delta_preint_ * getBuffer().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
+ jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
// push all into buffer
- getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
+ getBuffer().emplace_back(incoming_ptr_->getTimeStamp(),
incoming_ptr_->getData(),
incoming_ptr_->getDataCovariance(),
delta_,
@@ -540,15 +540,15 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
jacobian_calib_ .setZero();
// check that first motion in buffer is zero!
- assert(_capture_ptr->getBuffer().get().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().get().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().get().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
// Iterate all the buffer
- auto motion_it = _capture_ptr->getBuffer().get().begin();
+ auto motion_it = _capture_ptr->getBuffer().begin();
auto prev_motion_it = motion_it;
motion_it++;
- while (motion_it != _capture_ptr->getBuffer().get().end())
+ while (motion_it != _capture_ptr->getBuffer().end())
{
// get dt
const double dt = motion_it->ts_ - prev_motion_it->ts_;
@@ -611,7 +611,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
- if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().get().empty() and _ts >= capture_motion->getBuffer().get().front().ts_)
+ if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().empty() and _ts >= capture_motion->getBuffer().front().ts_)
{
// Found time stamp satisfying rule 3 above !! ==> break for loop
break;
@@ -661,7 +661,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
// ignore "future" KF to avoid MotionBuffer::split() error
- if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().get().back().ts_)
+ if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
pack = nullptr;
if (pack)
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 04265e964c385bdbe591e84c650f8c7d99c36eeb..931efc2e3ea522f4afde3a1647e329d26507859d 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -98,24 +98,24 @@ void ProcessorOdom2d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::Vec
bool ProcessorOdom2d::voteForKeyFrame() const
{
// Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
+ if (getBuffer().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
return true;
}
- if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
+ if (getBuffer().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
return true;
}
// Uncertainty criterion
- if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
+ if (getBuffer().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance");
return true;
}
// Time criterion
- if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
+ if (getBuffer().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per time");
return true;
@@ -151,12 +151,12 @@ FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBas
FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- Eigen::MatrixXd covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
+ Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
makePosDef(covariance);
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
"ProcessorOdom2d",
- _capture_motion->getBuffer().get().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_,
covariance);
return key_feature_ptr;
}
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 457394c9213b39a69da4a01f74f2804db723a573..d8f2781f4667989b155ad65e5640a01e4aa52db3 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -161,20 +161,20 @@ void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::Vec
bool ProcessorOdom3d::voteForKeyFrame() const
{
- //WOLF_DEBUG( "Time span : " , getBuffer().get().back().ts_ - getBuffer().get().front().ts_ );
- //WOLF_DEBUG( " last ts : ", getBuffer().get().back().ts_);
- //WOLF_DEBUG( " first ts : ", getBuffer().get().front().ts_);
- //WOLF_DEBUG( "BufferLength: " , getBuffer().get().size() );
+ //WOLF_DEBUG( "Time span : " , getBuffer().back().ts_ - getBuffer().front().ts_ );
+ //WOLF_DEBUG( " last ts : ", getBuffer().back().ts_);
+ //WOLF_DEBUG( " first ts : ", getBuffer().front().ts_);
+ //WOLF_DEBUG( "BufferLength: " , getBuffer().size() );
//WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
//WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
// time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3d_->max_time_span)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_odom_3d_->max_time_span)
{
WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
- if (getBuffer().get().size() > params_odom_3d_->max_buff_length)
+ if (getBuffer().size() > params_odom_3d_->max_buff_length)
{
WOLF_DEBUG( "PM: vote: buffer size" );
return true;
@@ -217,8 +217,8 @@ FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
{
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
"ProcessorOdom3d",
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_);
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_);
return key_feature_ptr;
}
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 987c713a245df58dba2c32e9edde9505bcd15d56..3c74ede52ce7ec57a153cdcf416b9c84597cadd9 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -42,8 +42,8 @@ TEST(MotionBuffer, QueryTimeStamps)
{
MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
+ MB.push_back(m0);
+ MB.push_back(m1);
TimeStamp t;
t.set(-1); // t is older than m0.ts_ -> return m0
@@ -66,10 +66,10 @@ TEST(MotionBuffer, getMotion)
{
MotionBuffer MB;
- MB.get().push_back(m0);
+ MB.push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
- MB.get().push_back(m1);
+ MB.push_back(m1);
ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
ASSERT_EQ(MB.getMotion(t1).delta_, m1.delta_);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
@@ -80,11 +80,11 @@ TEST(MotionBuffer, getDelta)
{
MotionBuffer MB;
- MB.get().push_back(m0);
+ MB.push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
- MB.get().push_back(m1);
+ MB.push_back(m1);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_);
@@ -94,19 +94,19 @@ TEST(MotionBuffer, Split)
{
MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- MB.get().push_back(m2);
- MB.get().push_back(m3);
- MB.get().push_back(m4); // put 5 motions
+ MB.push_back(m0);
+ MB.push_back(m1);
+ MB.push_back(m2);
+ MB.push_back(m3);
+ MB.push_back(m4); // put 5 motions
MotionBuffer MB_old;
TimeStamp t = 1.5; // between m1 and m2
MB.split(t, MB_old);
- ASSERT_EQ(MB_old.get().size(), (unsigned int) 2);
- ASSERT_EQ(MB .get().size(), (unsigned int) 3);
+ ASSERT_EQ(MB_old.size(), (unsigned int) 2);
+ ASSERT_EQ(MB .size(), (unsigned int) 3);
ASSERT_EQ(MB_old.getMotion(t1).ts_, t1);
ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1
@@ -118,11 +118,11 @@ TEST(MotionBuffer, Split)
// {
// MotionBuffer MB;
//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
// Eigen::MatrixXd cov = MB.integrateCovariance();
// ASSERT_NEAR(cov(0), 0.04, 1e-8);
@@ -133,11 +133,11 @@ TEST(MotionBuffer, Split)
// {
// MotionBuffer MB;
//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
// Eigen::MatrixXd cov = MB.integrateCovariance(t2);
// ASSERT_NEAR(cov(0), 0.02, 1e-8);
@@ -147,11 +147,11 @@ TEST(MotionBuffer, Split)
// {
// MotionBuffer MB;
//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
// Eigen::MatrixXd cov = MB.integrateCovariance(t1, t3);
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
@@ -164,9 +164,9 @@ TEST(MotionBuffer, print)
{
MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- MB.get().push_back(m2);
+ MB.push_back(m0);
+ MB.push_back(m1);
+ MB.push_back(m2);
MB.print();
MB.print(0,0,0,0);