diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 542540f3d10dc2e76c906081d524473ca3989630..da52631ce822a3ff0bd4e86dd3b2fc1fb602acbd 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -42,13 +42,13 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
/*
* ts_split
* origin target source
- * * ----------------------* <-- old buffer
+ * * ----------------------* <-- source buffer
*
* origin target source
- * * ----------* ----------* <-- old buffer, shorter
+ * * ----------* ----------* <-- source buffer, shorter
* ^
* |
- * new buffer
+ * target buffer
*/
// some fancy variables
@@ -58,16 +58,18 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
// and give the part of the buffer before the target timestamp to the target capture
_capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
+ // push a zero-motion at the front of the source's buffer
+ _capture_source->getBuffer().push_front(motionZero(_capture_target->getTimeStamp()));
+
// Update the target and source capture's origin
_capture_target -> setOriginCapture(capture_origin);
_capture_source -> setOriginCapture(_capture_target);
- // Get optimized calibration params from origin capture
+ // Get optimized calibration params from origin capture (maybe, they have been optimized by a solve()! )
const auto& calib_preint = capture_origin->getCalibrationComposite();
- // Write the calib params into the capture before re-integration
- _capture_source->setCalibrationPreint(calib_preint);
-
+ // Write the calib params into the captures before re-integration
+ _capture_target->setCalibrationPreint(calib_preint);
// re-integrate target capture's buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer(_capture_target);
@@ -291,11 +293,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// integrate data
integrateOneStep();
+
// Update state and time stamps
last_ptr_->setTimeStamp(this->getTimeStamp());
last_ptr_->getFrame()->setTimeStamp (this->getTimeStamp());
last_ptr_->getFrame()->setVectorComposite(this->getStateComposite());
+
if (permittedKeyFrame() && voteForKeyFrame())
{
/*
@@ -347,12 +351,15 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
- key_frame->getTimeStamp(),
+ last_ptr_->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
last_ptr_->getCalibrationComposite(),
last_ptr_);
+ // first buffer entry is zero
+ capture_new->getBuffer().push_back(motionZero(last_ptr_->getTimeStamp()));
+
// reset derived things
resetDerived();
@@ -372,9 +379,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
void ProcessorMotion::getTimeStamp (TimeStamp& _ts) const
{
- if (getBuffer().empty())
- _ts = getLast()->getTimeStamp();
- else
_ts = getBuffer().back().ts_;
}
@@ -383,13 +387,6 @@ bool ProcessorMotion::getStateComposite(VectorComposite& _state) const
if (not last_ptr_ or not last_ptr_->getFrame())
return false;
- // if buffer is empty --> we did not advance from origin!
- if (last_ptr_->getBuffer().empty())
- {
- _state = origin_ptr_->getFrame()->getVectorComposite();
- return true;
- }
-
/* Doing this:
*
* x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
@@ -446,12 +443,13 @@ bool ProcessorMotion::getStateComposite(const TimeStamp& _ts, VectorComposite& _
// We need to search for the capture containing a motion buffer with the queried time stamp
CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
+
if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
{
// if buffer is empty --> we did not advance from origin!
if (capture_motion->getBuffer().empty())
{
- _state = origin_ptr_->getFrame()->getVectorComposite();
+ _state = capture_motion->getFrame()->getVectorComposite();
return true;
}
@@ -496,11 +494,11 @@ bool ProcessorMotion::getStateComposite(const TimeStamp& _ts, VectorComposite& _
const auto& delta_corrected = correctDeltaComposite(delta_preint, delta_step);
// compute current state // this is [+]
- statePlusDelta(x_origin, delta_corrected, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), _state);
+ statePlusDelta(x_origin, delta_corrected, _ts - cap_orig->getTimeStamp(), _state);
}
else
{
- statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), _state);
+ statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), _state);
}
// return success
@@ -554,6 +552,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
getSensor()->getCalibrationComposite(),
origin_ptr_);
+ last_ptr_->getBuffer().push_back(motionZero(last_ptr_->getTimeStamp()));
+
// Reset derived things
resetDerived();
}
@@ -561,20 +561,12 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
void ProcessorMotion::integrateOneStep()
{
-
calib_preint_ = getLast()->getCalibrationPreint();
TimeStamp prev_ts;
- if (getBuffer().empty())
- {
- prev_ts = origin_ptr_->getTimeStamp();
- delta_integrated_ = deltaZero();
- }
- else
- {
- prev_ts = getBuffer().back().ts_;
- }
+ prev_ts = getBuffer().back().ts_;
+ delta_integrated_ = getBuffer().back().delta_integr_;
// Set dt
double dt = incoming_ptr_->getTimeStamp() - prev_ts;
@@ -598,21 +590,17 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_preint_,
jacobian_delta_);
- if (getBuffer().empty()) // first time
- {
- // init integrals of jacobian and covariance
- jacobian_calib_ = jacobian_delta_ * jacobian_delta_calib_;
- delta_integrated_cov_ = jacobian_delta_.propagate(delta_cov_);
- }
- else // other times
- {
- // integrate jacobian and covariance
- jacobian_calib_ = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
- + jacobian_delta_ * jacobian_delta_calib_;
+ // integrate jacobian and covariance
+ jacobian_calib_ = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
+ + jacobian_delta_ * jacobian_delta_calib_;
- delta_integrated_cov_ = jacobian_delta_preint_.propagate(getBuffer().back().delta_integr_cov_)
- + jacobian_delta_.propagate(delta_cov_);
- }
+ jacobian_delta_preint_.check();
+ getBuffer().back().delta_integr_cov_.check();
+ jacobian_delta_.check();
+ delta_cov_.check();
+
+ delta_integrated_cov_ = jacobian_delta_preint_.propagate(getBuffer().back().delta_integr_cov_)
+ + jacobian_delta_.propagate(delta_cov_);
// push all into buffer
getBuffer().emplace_back(incoming_ptr_->getTimeStamp(),
@@ -625,6 +613,7 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_,
jacobian_delta_preint_,
jacobian_calib_);
+
}
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
@@ -634,18 +623,20 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
calib_preint_ = _capture_ptr->getCalibrationPreint();
- TimeStamp prev_ts = _capture_ptr->getOriginCapture()->getTimeStamp();
-
- delta_integrated_ = deltaZero();
-
- bool first_time = true;
-
// Iterate all the buffer
auto motion_it = _capture_ptr->getBuffer().begin();
+ auto motion_prev_it = motion_it;
+ motion_it ++; // start on the second motion, since the first is always zero
+
+ // initialize temporaries
+ delta_integrated_ = motion_prev_it->delta_integr_;
+ delta_integrated_cov_ = motion_prev_it->delta_integr_cov_;
+ jacobian_calib_ = motion_prev_it->jacobian_calib_;
+
while (motion_it != _capture_ptr->getBuffer().end())
{
// get dt
- const double dt = motion_it->ts_ - prev_ts;
+ const double dt = motion_it->ts_ - motion_prev_it->ts_;
// re-convert data to delta with the new calibration parameters
computeCurrentDelta(motion_it->data_,
@@ -663,26 +654,13 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
jacobian_delta_preint_,
jacobian_delta_);
- if (first_time == true)
- {
- first_time = false;
+ // integrate Jacobian wrt calib
+ jacobian_calib_ = jacobian_delta_preint_ * jacobian_calib_
+ + jacobian_delta_ * jacobian_delta_calib_;
- // initialize Jacobian wrt calib
- jacobian_calib_ = jacobian_delta_ * jacobian_delta_calib_;
-
- // initialize covariance
- delta_integrated_cov_ = jacobian_delta_.propagate(delta_cov_);
- }
- else
- {
- // integrate Jacobian wrt calib
- jacobian_calib_ = jacobian_delta_preint_ * jacobian_calib_
- + jacobian_delta_ * jacobian_delta_calib_;
-
- // Integrate covariance
- delta_integrated_cov_ = jacobian_delta_preint_.propagate(delta_integrated_cov_)
- + jacobian_delta_.propagate(delta_cov_);
- }
+ // Integrate covariance
+ delta_integrated_cov_ = jacobian_delta_preint_.propagate(delta_integrated_cov_)
+ + jacobian_delta_ .propagate(delta_cov_);
// store in buffer
motion_it->delta_ = delta_;
@@ -693,11 +671,9 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
motion_it->jacobian_delta_integr_ = jacobian_delta_preint_;
motion_it->jacobian_calib_ = jacobian_calib_;
- // store carry variables
- prev_ts = motion_it->ts_;
-
// advance buffer iterator
motion_it ++;
+ motion_prev_it ++;
}
}
@@ -783,14 +759,11 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
if (pack)
{
if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
- {
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
+
processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
- }
+
else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+
processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
else
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index a308417a982658c0f161f8497a3c472b814077bc..e036682552281db4de689c0283b1c9e9e35f2baf 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -52,7 +52,6 @@ void ProcessorOdom2d::computeCurrentDelta(const Eigen::VectorXd& _data,
_delta_cov = J_delta_data.propagate(data_cov);
-
// data is [dr, dtheta]
// delta is [dx, dy, dtheta]
// motion model is 1/2 turn + straight + 1/2 turn
@@ -87,6 +86,8 @@ void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
bool ProcessorOdom2d::voteForKeyFrame() const
{
+ if (getBuffer().empty()) return false;
+
// last motion
const auto motion = getBuffer().back();