From 6fd04e758ceb0f8b4bfd5250bb9d62d8b7aa12f6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 1 Nov 2016 23:49:41 +0100
Subject: [PATCH] Do not use makeFrame()
---
src/processor_motion.cpp | 15 +++++++++++----
1 file changed, 11 insertions(+), 4 deletions(-)
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index c6c7efb01..0b9ac37cc 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -60,7 +60,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
Eigen::MatrixXs::Zero(data_size_, data_size_), key_frame_ptr);
// create a new last frame
-// makeFrame(last_ptr_, key_frame_ptr->getState(), NON_KEY_FRAME);
FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME, key_frame_ptr->getState(), last_ptr_->getTimeStamp());
new_frame_ptr->addCapture(last_ptr_); // Add Capture to the new Frame
@@ -129,15 +128,23 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
_origin_frame->addCapture(origin_ptr_);
// make (emtpy) last Capture
- last_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(), getSensorPtr(),
+ last_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(),
+ getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_), _origin_frame);
+ Eigen::MatrixXs::Zero(data_size_, data_size_),
+ _origin_frame);
// Make frame at last Capture
- makeFrame(last_ptr_, _origin_frame->getState(), NON_KEY_FRAME);
+ // makeFrame(last_ptr_, _origin_frame->getState(), NON_KEY_FRAME);
+ FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME,
+ _origin_frame->getState(),
+ last_ptr_->getTimeStamp());
+ new_frame_ptr->addCapture(last_ptr_); // Add last Capture to the new Frame
// Reset deltas
delta_ = deltaZero();
delta_integrated_ = deltaZero();
+ delta_cov_.setZero();
+ delta_integrated_cov_.setZero();
// clear and reset buffer
getBuffer().get().clear();
--
GitLab