diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 5a8d56edefd9efb511d5e5a8ca575ef0ff11bc6b..7d7ae85accebb0b627db06b3d133bcb7713d2b00 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -118,28 +118,28 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// 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_),
- getSensor()->getNoiseCov(),
- 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
@@ -202,15 +202,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// 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,
@@ -222,7 +226,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// 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);
@@ -279,40 +283,40 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// 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_),
- getSensor()->getNoiseCov(),
- 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()) ) ;
+ 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();
@@ -389,9 +393,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- 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(),
@@ -437,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()
@@ -502,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()