From 34ad55849254d7c51afda1a0c554acd68ac1a9df Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Wed, 21 Mar 2018 14:33:58 +0100 Subject: [PATCH] Add method hasCalibration() --- src/capture_base.h | 1 + src/processor_motion.cpp | 7 ++----- src/processor_motion.h | 2 ++ 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/capture_base.h b/src/capture_base.h index 02d4c5352..e7626e1d5 100644 --- a/src/capture_base.h +++ b/src/capture_base.h @@ -92,6 +92,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture void fixIntrinsics(); void unfixIntrinsics(); + bool hasCalibration() {return calib_size_ > 0;} Size getCalibSize() const; virtual Eigen::VectorXs getCalibration() const; void setCalibration(const Eigen::VectorXs& _calib); diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp index 90ebde732..a8af1d1a5 100644 --- a/src/processor_motion.cpp +++ b/src/processor_motion.cpp @@ -404,11 +404,8 @@ void ProcessorMotion::integrateOneStep() jacobian_delta_); // integrate Jacobian wrt calib - if (calib_size_ > 0) - { + if ( hasCalibration() ) jacobian_calib_ = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_; - // WOLF_TRACE("jac calib: ", jacobian_calib_.row(0)); - } // Integrate covariance delta_integrated_cov_ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose() @@ -461,7 +458,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) motion_it->jacobian_delta_); // integrate Jacobian wrt calib - if (calib_size_ > 0) + if ( hasCalibration() ) motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * prev_motion_it->jacobian_calib_ + motion_it->jacobian_delta_ * jacobian_delta_calib_; // Integrate covariance diff --git a/src/processor_motion.h b/src/processor_motion.h index 3ca6aba0c..ea1421aa9 100644 --- a/src/processor_motion.h +++ b/src/processor_motion.h @@ -397,6 +397,8 @@ class ProcessorMotion : public ProcessorBase Motion motionZero(const TimeStamp& _ts); + bool hasCalibration() {return calib_size_ > 0;} + public: CaptureMotionPtr getOriginPtr(); CaptureMotionPtr getLastPtr(); -- GitLab