diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index c5e830084b3790b1501acfe81fa8dc2086dc645a..7119d09bc82890e823d8a0575b612603aeabdb79 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -278,78 +278,95 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
{
assert(_new_keyframe->getTrajectoryPtr() != nullptr
- && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
+ && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
// get keyframe's time stamp
TimeStamp new_ts = _new_keyframe->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
auto existing_capture = findCaptureContainingTimeStamp(new_ts);
- assert(existing_capture != nullptr
- && "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found");
-
- // Find the frame acting as the capture's origin
- auto keyframe_origin = existing_capture->getOriginFramePtr();
-
- // emplace a new motion capture to the new keyframe
- auto new_capture = emplaceCapture(_new_keyframe,
- getSensorPtr(),
- new_ts,
- Eigen::VectorXs::Zero(data_size_),
- existing_capture->getDataCovariance(),
- existing_capture->getCalibration(),
- existing_capture->getCalibration(),
- keyframe_origin);
-
- // split the buffer
- // and give the part of the buffer before the new keyframe to the key_capture
- existing_capture->getBuffer().split(new_ts, new_capture->getBuffer());
-
- // interpolate individual delta
- if (!existing_capture->getBuffer().get().empty() && new_capture->getBuffer().get().back().ts_ != new_ts)
+
+
+ if(existing_capture == nullptr) // Keyframe without Capture --> first time
{
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(new_capture->getBuffer().get().back(), // last Motion of old buffer
- existing_capture->getBuffer().get().front(), // first motion of new buffer
- new_ts);
- // add to old buffer
- new_capture->getBuffer().get().push_back(motion_interpolated);
+ CaptureMotionPtr new_capture = createCapture(new_ts,
+ getSensorPtr(),
+ Eigen::VectorXs::Zero(data_size_),
+ getSensorPtr()->getNoiseCov(),
+ _new_keyframe);
+
+ new_capture->setCalibration(getSensorPtr()->getCalibration());
+ new_capture->setCalibrationPreint(getSensorPtr()->getCalibration());
+
+ emplaceFrame(NON_KEY_FRAME, new_capture);
}
+ else // Normal operation
+ {
+
+ // Find the frame acting as the capture's origin
+ auto keyframe_origin = existing_capture->getOriginFramePtr();
+
+ // emplace a new motion capture to the new keyframe
+ auto new_capture = emplaceCapture(_new_keyframe,
+ getSensorPtr(),
+ new_ts,
+ Eigen::VectorXs::Zero(data_size_),
+ existing_capture->getDataCovariance(),
+ existing_capture->getCalibration(),
+ existing_capture->getCalibration(),
+ keyframe_origin);
+
+ // split the buffer
+ // and give the part of the buffer before the new keyframe to the key_capture
+ existing_capture->getBuffer().split(new_ts, new_capture->getBuffer());
+
+ // interpolate individual delta
+ if (!existing_capture->getBuffer().get().empty() && new_capture->getBuffer().get().back().ts_ != new_ts)
+ {
+ // interpolate Motion at the new time stamp
+ Motion motion_interpolated = interpolate(new_capture->getBuffer().get().back(), // last Motion of old buffer
+ existing_capture->getBuffer().get().front(), // first motion of new buffer
+ new_ts);
+ // add to old buffer
+ new_capture->getBuffer().get().push_back(motion_interpolated);
+ }
- // create motion feature and add it to the capture
- auto new_feature = emplaceFeature(new_capture);
-// reintegrateBuffer(new_capture);
+ // create motion feature and add it to the capture
+ auto new_feature = emplaceFeature(new_capture);
+ // reintegrateBuffer(new_capture);
- // create motion constraint and add it to the feature, and constrain to the other capture (origin)
- emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
+ // create motion constraint and add it to the feature, and constrain to the other capture (origin)
+ emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
- /////////////////////////////////////////////////////////
- // Update the existing capture
- if (existing_capture == last_ptr_)
- // reset processor origin
- origin_ptr_ = new_capture;
+ /////////////////////////////////////////////////////////
+ // Update the existing capture
+ if (existing_capture == last_ptr_)
+ // reset processor origin
+ origin_ptr_ = new_capture;
- existing_capture->setOriginFramePtr(_new_keyframe);
+ existing_capture->setOriginFramePtr(_new_keyframe);
- // reintegrate existing buffer -- note: the result of re-integration is stored in the same buffer!
- reintegrateBuffer(existing_capture);
+ // reintegrate existing buffer -- note: the result of re-integration is stored in the same buffer!
+ reintegrateBuffer(existing_capture);
- // modify existing feature and constraint (if they exist in the existing capture)
- if (!existing_capture->getFeatureList().empty())
- {
- auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
+ // modify existing feature and constraint (if they exist in the existing capture)
+ if (!existing_capture->getFeatureList().empty())
+ {
+ auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
+
+ // Modify existing feature --------
+ existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_);
+ existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
- // Modify existing feature --------
- existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_);
- existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
+ // Modify existing constraint --------
+ // Instead of modifying, we remove one ctr, and create a new one.
+ auto ctr_to_remove = existing_feature->getConstraintList().back(); // there is only one constraint!
+ auto new_ctr = emplaceConstraint(existing_feature, new_capture);
+ ctr_to_remove ->remove(); // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ }
- // Modify existing constraint --------
- // Instead of modifying, we remove one ctr, and create a new one.
- auto ctr_to_remove = existing_feature->getConstraintList().back(); // there is only one constraint!
- auto new_ctr = emplaceConstraint(existing_feature, new_capture);
- ctr_to_remove ->remove(); // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
return true;