From b24b64a740a934e06a504a19700f5bf081191654 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Mon, 4 May 2020 20:57:25 +0200 Subject: [PATCH] Optimize code for delta preintegration --- src/processor/processor_motion.cpp | 164 +++++++++++++---------------- 1 file changed, 74 insertions(+), 90 deletions(-) diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 4942a72ba..3b11415f7 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -561,47 +561,52 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) void ProcessorMotion::integrateOneStep() { - // Set dt - double dt_ = incoming_ptr_->getTimeStamp() - getBuffer().back().ts_; - - assert(dt_ >= 0 && "Time interval _dt is negative!"); calib_preint_ = getLast()->getCalibrationPreint(); + TimeStamp prev_ts; + + if (getBuffer().empty()) + { + prev_ts = origin_ptr_->getTimeStamp(); + delta_integrated_ = deltaZero(); + } + else + { + prev_ts = getBuffer().back().ts_; + } + + // Set dt + double dt = incoming_ptr_->getTimeStamp() - prev_ts; + + assert(dt >= 0 && "Time interval _dt is negative!"); + // get data and convert it to delta, and obtain also the delta covariance computeCurrentDelta(incoming_ptr_->getData(), incoming_ptr_->getDataCovariance(), calib_preint_, - dt_, + dt, delta_, delta_cov_, jacobian_delta_calib_); // integrate the current delta to pre-integrated measurements, and get Jacobians - if (getBuffer().empty()) - { // first time - - deltaPlusDelta(deltaZero(), - delta_, - dt_, - delta_integrated_, - jacobian_delta_preint_, - jacobian_delta_); - + deltaPlusDelta(delta_integrated_, + delta_, + dt, + delta_integrated_, + 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 - - deltaPlusDelta(getBuffer().back().delta_integr_, - delta_, - dt_, - delta_integrated_, - jacobian_delta_preint_, - jacobian_delta_); - + else // other times + { + // integrate jacobian and covariance jacobian_calib_ = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_; @@ -611,15 +616,15 @@ void ProcessorMotion::integrateOneStep() // push all into buffer getBuffer().emplace_back(incoming_ptr_->getTimeStamp(), - incoming_ptr_->getData(), - incoming_ptr_->getDataCovariance(), - delta_, - delta_cov_, - delta_integrated_, - delta_integrated_cov_, - jacobian_delta_, - jacobian_delta_preint_, - jacobian_calib_); + incoming_ptr_->getData(), + incoming_ptr_->getDataCovariance(), + delta_, + delta_cov_, + delta_integrated_, + delta_integrated_cov_, + jacobian_delta_, + jacobian_delta_preint_, + jacobian_calib_); } void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) @@ -627,82 +632,59 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) if ( _capture_ptr->getBuffer().empty() ) return; - VectorType calib = _capture_ptr->getCalibrationPreint(); - - // Iterate all the buffer - auto motion_it = _capture_ptr->getBuffer().begin(); - - { // first time - - const auto& capture_origin = _capture_ptr->getOriginCapture(); - - TimeStamp time_origin = capture_origin->getTimeStamp(); // TODO use a time_origin stored in the same buffer + calib_preint_ = _capture_ptr->getCalibrationPreint(); - // get dt - const double dt = motion_it->ts_ - time_origin; - - // re-convert data to delta with the new calibration parameters - computeCurrentDelta(motion_it->data_, - motion_it->data_cov_, - calib, - dt, - delta_, - delta_cov_, - jacobian_delta_calib_); + TimeStamp prev_ts = _capture_ptr->getOriginCapture()->getTimeStamp(); - deltaPlusDelta(deltaZero(), - delta_, - dt, - delta_integrated_, - jacobian_delta_preint_, - jacobian_delta_); + delta_integrated_ = deltaZero(); - jacobian_calib_ = jacobian_delta_ * jacobian_delta_calib_; - delta_integrated_cov_ = jacobian_delta_.propagate(delta_cov_); + bool first_time = true; - motion_it->delta_ = delta_; - motion_it->delta_cov_ = delta_cov_; - motion_it->delta_integr_ = delta_integrated_; - motion_it->delta_integr_cov_ = delta_integrated_cov_; - motion_it->jacobian_delta_ = jacobian_delta_; - motion_it->jacobian_delta_integr_ = jacobian_delta_preint_; - motion_it->jacobian_calib_ = jacobian_calib_; - - } - - // other times - auto prev_motion_it = motion_it; - motion_it++; + // Iterate all the buffer + auto motion_it = _capture_ptr->getBuffer().begin(); while (motion_it != _capture_ptr->getBuffer().end()) { // get dt - const double dt = motion_it->ts_ - prev_motion_it->ts_; + const double dt = motion_it->ts_ - prev_ts; // re-convert data to delta with the new calibration parameters computeCurrentDelta(motion_it->data_, motion_it->data_cov_, - calib, + calib_preint_, dt, delta_, delta_cov_, jacobian_delta_calib_); - // integrate delta into delta_integr, and rewrite the buffer - deltaPlusDelta(prev_motion_it->delta_integr_, + deltaPlusDelta(delta_integrated_, delta_, dt, delta_integrated_, jacobian_delta_preint_, jacobian_delta_); - // integrate Jacobian wrt calib - jacobian_calib_ = jacobian_delta_preint_ * jacobian_calib_ - + jacobian_delta_ * jacobian_delta_calib_; + if (first_time == true) + { + first_time = false; + + // 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_; motion_it->delta_cov_ = delta_cov_; motion_it->delta_integr_ = delta_integrated_; @@ -711,9 +693,11 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) motion_it->jacobian_delta_integr_ = jacobian_delta_preint_; motion_it->jacobian_calib_ = jacobian_calib_; - // advance in buffer - motion_it++; - prev_motion_it++; + // store carry variables + prev_ts = motion_it->ts_; + + // advance buffer iterator + motion_it ++; } } -- GitLab