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