diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index 6809c92eedd000c803961b3b8629920e17c5b449..9d7ae613ffa3d5e065c0fe96b319a78d1441c2ae 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -74,9 +74,7 @@ struct Motion
*/
class MotionBuffer{
public:
- SizeEigen data_size_, delta_size_, cov_size_;
- MotionBuffer() = delete;
- MotionBuffer(SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size);
+ MotionBuffer() ;
std::list<Motion>& get();
const std::list<Motion>& get() const;
const Motion& getMotion(const TimeStamp& _ts) const;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 2eb19c69cd77ebb4b249dad0cda83d9a388c450e..1cd51d36ee2526ee0072dcbbeb31081ab4c69793 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -466,7 +466,6 @@ class ProcessorMotion : public ProcessorBase
// helpers to avoid allocation
Scalar dt_; ///< Time step
Eigen::VectorXs x_; ///< current state
- Eigen::VectorXs data_; ///< current data
Eigen::VectorXs delta_; ///< current delta
Eigen::MatrixXs delta_cov_; ///< current delta covariance
Eigen::VectorXs delta_integrated_; ///< integrated delta
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 9f1e2216665806f7c7ac51b53928f4ea0cd19016..90b282f2a5e5fdba4bcfd9fc209b8ff9f2c11e7e 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -20,7 +20,7 @@ CaptureMotion::CaptureMotion(const std::string & _type,
CaptureBase(_type, _ts, _sensor_ptr),
data_(_data),
data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
- buffer_(_data.size(), _delta_size, _delta_cov_size),
+ buffer_(),
origin_frame_ptr_(_origin_frame_ptr)
{
//
@@ -40,7 +40,7 @@ CaptureMotion::CaptureMotion(const std::string & _type,
CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
data_(_data),
data_cov_(_data_cov),
- buffer_(_data.size(), _delta_size, _delta_cov_size),
+ buffer_(),
origin_frame_ptr_(_origin_frame_ptr)
{
//
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 635004edd7dfd33854acc33cdb7c69ba51263ad9..1a4aa63e0179e4bf859c890246b70d04c2e9fb27 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -62,12 +62,7 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
////////////////////////////////////////////////////////////////////////////////////////
-MotionBuffer::MotionBuffer(SizeEigen _data_size,
- SizeEigen _delta_size,
- SizeEigen _cov_size) :
- data_size_(_data_size),
- delta_size_(_delta_size),
- cov_size_(_cov_size)
+MotionBuffer::MotionBuffer()
{
container_.clear();
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 633a98022487b5f54cfc93d27e2ac3018ecaea6d..7d7ae85accebb0b627db06b3d133bcb7713d2b00 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -21,7 +21,6 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
last_ptr_(),
incoming_ptr_(),
dt_(0.0), x_(_state_size),
- data_(_data_size),
delta_(_delta_size),
delta_cov_(_delta_cov_size, _delta_cov_size),
delta_integrated_(_delta_size),
@@ -92,30 +91,55 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case RUNNING_WITH_PACK_BEFORE_ORIGIN :
{
+
+ /*
+ * Legend:
+ * * : any frame or keyframe
+ * x : any capture
+ * k : arriving keyframe
+ * o : origin capture
+ * l : last capture
+ * e : existing capture -> new KF splits its buffer in two parts
+ * n : new capture -> part of the split buffer will be given to this
+ * --- : buffer history
+ *
+ * Trajectory before the KF, and arriving KF 'k'
+ *
+ * k
+ * * ========= * === ... === * ========= *
+ * x ----------e ... ----o ----------l
+ *
+ * Trajectory after the KF, and arriving KF 'k'
+ *
+ * k
+ * * === * === * === ... === * ========= *
+ * x ----n ----e ... ----o ----------l
+ */
+
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
- auto existing_capture = findCaptureContainingTimeStamp(ts_from_callback); // k
+ auto existing_capture = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k
// Find the frame acting as the capture's origin
- auto keyframe_origin = existing_capture->getOriginFrame(); // i
+ auto keyframe_origin = existing_capture->getOriginFrame(); // i
- auto capture_origin = std::static_pointer_cast<CaptureMotion>(keyframe_origin->getCaptureOf(getSensor()));
+ auto capture_origin = std::static_pointer_cast<CaptureMotion>(keyframe_origin->getCaptureOf(getSensor()));
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXs calib_origin = capture_origin->getCalibration();
+ VectorXs calib_origin = capture_origin->getCalibration();
// emplace a new motion capture to the new keyframe
- auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
- getSensor(),
- ts_from_callback,
- Eigen::VectorXs::Zero(data_size_),
- capture_origin->getDataCovariance(),
- calib_origin,
- calib_origin,
- keyframe_origin);
+ TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
+ getSensor(),
+ ts_from_callback,
+ Eigen::VectorXs::Zero(data_size_),
+ getSensor()->getNoiseCov(),
+ calib_origin,
+ calib_origin,
+ keyframe_origin);
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
@@ -147,29 +171,62 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case RUNNING_WITH_PACK_AFTER_ORIGIN :
{
+ /*
+ * Legend:
+ * * : any frame or keyframe
+ * x : any capture
+ * k : arriving keyframe
+ * o : origin capture
+ * l : last capture -> new KF splits its buffer in two parts
+ * n : new capture -> part of the split buffer will be given to this
+ * --- : buffer history
+ *
+ * Trajectory before the KF, and arriving KF 'k'
+ *
+ * k
+ * * ========= * ========= *
+ * x ----------o ----------l
+ *
+ * Trajectory after the KF, and arriving KF 'k'
+ *
+ * k
+ * * ========= * === * === *
+ * x ----------o ----n ----l
+ *
+ * Trajectory after the KF, and after reset
+ *
+ * k
+ * * ========= * === * === *
+ * x ----------x ----o ----l
+ */
+
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+
+ auto & existing_capture = last_ptr_;
// Find the frame acting as the capture's origin
- auto keyframe_origin = last_ptr_->getOriginFrame();
+ auto keyframe_origin = existing_capture->getOriginFrame();
+
+ auto & capture_origin = origin_ptr_;
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXs calib_origin = origin_ptr_->getCalibration();
+ VectorXs calib_origin = capture_origin->getCalibration();
// emplace a new motion capture to the new keyframe
+ TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
getSensor(),
ts_from_callback,
Eigen::VectorXs::Zero(data_size_),
- origin_ptr_->getDataCovariance(),
+ getSensor()->getNoiseCov(),
calib_origin,
calib_origin,
keyframe_origin);
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(last_ptr_, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+ splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
@@ -200,48 +257,66 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
if (permittedKeyFrame() && voteForKeyFrame())
{
+ /*
+ * Legend:
+ * * : any keyframe
+ * + : last frame
+ * x : any capture
+ * o : origin capture
+ * l : last capture
+ * i : incoming capture
+ * e : existing capture -> new KF splits its buffer in two parts
+ * n : new capture -> part of the split buffer will be given to this
+ * --- : buffer history
+ *
+ * Trajectory before the KF
+ *
+ * * ========= * ========= +
+ * x ----------o ----------l i
+ *
+ * Trajectory after creating KF at last and reset
+ *
+ * * ========= * ========= * = +
+ * x ----------x ----------o --l
+ *
+ */
+
+
// Set the frame of last_ptr as key
- auto key_frame_ptr = last_ptr_->getFrame();
- key_frame_ptr->setKey();
+ auto key_frame = last_ptr_->getFrame();
+ key_frame ->setKey();
// create motion feature and add it to the key_capture
- auto key_feature_ptr = emplaceFeature(last_ptr_);
+ auto key_feature = emplaceFeature(last_ptr_);
// create motion factor and link it to parent feature and other frame (which is origin's frame)
- auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
+ auto factor = emplaceFactor(key_feature, origin_ptr_);
// create a new frame
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
+ auto new_frame = getProblem()->emplaceFrame(NON_ESTIMATED,
getCurrentState(),
getCurrentTimeStamp());
// create a new capture
- auto new_capture_ptr = emplaceCapture(new_frame_ptr,
- getSensor(),
- key_frame_ptr->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
- last_ptr_->getCalibration(),
- last_ptr_->getCalibration(),
- key_frame_ptr);
+ auto new_capture = emplaceCapture(new_frame,
+ getSensor(),
+ key_frame->getTimeStamp(),
+ Eigen::VectorXs::Zero(data_size_),
+ getSensor()->getNoiseCov(),
+ last_ptr_->getCalibration(),
+ last_ptr_->getCalibration(),
+ key_frame);
// reset the new buffer
- new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
-
- // reset integrals
- delta_ = deltaZero();
- delta_cov_ . setZero();
- delta_integrated_ = deltaZero();
- delta_integrated_cov_ . setZero();
- jacobian_calib_ . setZero();
+ new_capture->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ;
// reset derived things
resetDerived();
// Update pointers
origin_ptr_ = last_ptr_;
- last_ptr_ = new_capture_ptr;
+ last_ptr_ = new_capture;
// callback to other processors
- getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), params_motion_->time_tolerance);
+ getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
}
resetDerived();
@@ -311,22 +386,22 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
getSensor(),
_origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
+ getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
getSensor()->getCalibration(),
nullptr);
// ---------- LAST ----------
// Make non-key-frame for last Capture
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
- _origin_frame->getTimeStamp());
+ auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
+ _origin_frame->getState(),
+ _origin_frame->getTimeStamp());
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
_origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
+ getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
getSensor()->getCalibration(),
_origin_frame);
@@ -334,13 +409,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// clear and reset buffer
getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
- // Reset integrals
- delta_ = deltaZero();
- delta_cov_ . setZero();
- delta_integrated_ = deltaZero();
- delta_integrated_cov_ . setZero();
- jacobian_calib_ . setZero();
-
// Reset derived things
resetDerived();
}
@@ -373,9 +441,11 @@ void ProcessorMotion::integrateOneStep()
// integrate Jacobian wrt calib
if ( hasCalibration() )
+ {
jacobian_calib_.noalias()
= jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
+ jacobian_delta_ * jacobian_delta_calib_;
+ }
// Integrate covariance
delta_integrated_cov_.noalias()
@@ -438,7 +508,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
// integrate Jacobian wrt calib
if ( hasCalibration() )
motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * jacobian_calib_
- + motion_it->jacobian_delta_ * jacobian_delta_calib_;
+ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
// Integrate covariance
motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose()
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index af262f1170bdcfffa0c633e858cebc5d9fc41562..e8d572d73a908af0b1df7a6d5fa455e6f3f266c9 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -40,7 +40,7 @@ Motion m4 = newMotion(t4, 4, 10, 1, .1, 1);
TEST(MotionBuffer, QueryTimeStamps)
{
- MotionBuffer MB(1,1,1);
+ MotionBuffer MB;
MB.get().push_back(m0);
MB.get().push_back(m1);
@@ -64,7 +64,7 @@ TEST(MotionBuffer, QueryTimeStamps)
TEST(MotionBuffer, getMotion)
{
- MotionBuffer MB(1,1,1);
+ MotionBuffer MB;
MB.get().push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
@@ -78,7 +78,7 @@ TEST(MotionBuffer, getMotion)
TEST(MotionBuffer, getDelta)
{
- MotionBuffer MB(1,1,1);
+ MotionBuffer MB;
MB.get().push_back(m0);
@@ -92,7 +92,7 @@ TEST(MotionBuffer, getDelta)
TEST(MotionBuffer, Split)
{
- MotionBuffer MB(1,1,1);
+ MotionBuffer MB;
MB.get().push_back(m0);
MB.get().push_back(m1);
@@ -100,7 +100,7 @@ TEST(MotionBuffer, Split)
MB.get().push_back(m3);
MB.get().push_back(m4); // put 5 motions
- MotionBuffer MB_old(1,1,1);
+ MotionBuffer MB_old;
TimeStamp t = 1.5; // between m1 and m2
MB.split(t, MB_old);
@@ -116,7 +116,7 @@ TEST(MotionBuffer, Split)
// TEST(MotionBuffer, integrateCovariance)
// {
-// MotionBuffer MB(1,1,1,0);
+// MotionBuffer MB;
//
// MB.get().push_back(m0);
// MB.get().push_back(m1);
@@ -131,7 +131,7 @@ TEST(MotionBuffer, Split)
//
// TEST(MotionBuffer, integrateCovariance_ts)
// {
-// MotionBuffer MB(1,1,1,0);
+// MotionBuffer MB;
//
// MB.get().push_back(m0);
// MB.get().push_back(m1);
@@ -145,7 +145,7 @@ TEST(MotionBuffer, Split)
//
// TEST(MotionBuffer, integrateCovariance_ti_tf)
// {
-// MotionBuffer MB(1,1,1,0);
+// MotionBuffer MB;
//
// MB.get().push_back(m0);
// MB.get().push_back(m1);
@@ -162,7 +162,7 @@ TEST(MotionBuffer, Split)
TEST(MotionBuffer, print)
{
- MotionBuffer MB(1,1,1);
+ MotionBuffer MB;
MB.get().push_back(m0);
MB.get().push_back(m1);
diff --git a/test/gtest_processor_odom_3D.cpp b/test/gtest_processor_odom_3D.cpp
index 421efc97afa7227e15013bd4a97d8f6c8b409759..f60300e84c6175db1d0567b3371475c1328f8d26 100644
--- a/test/gtest_processor_odom_3D.cpp
+++ b/test/gtest_processor_odom_3D.cpp
@@ -46,10 +46,6 @@ class ProcessorOdom3DTest : public ProcessorOdom3D
ProcessorOdom3DTest();
// getters :-D !!
- VectorXs& delta() {return delta_;}
- VectorXs& deltaInt() {return delta_integrated_;}
- MatrixXs& deltaCov() {return delta_cov_;}
- MatrixXs& deltaIntCov() {return delta_integrated_cov_;}
Scalar& kdd() {return k_disp_to_disp_;}
Scalar& kdr() {return k_disp_to_rot_;}
Scalar& krr() {return k_rot_to_rot_;}