diff --git a/src/capture_base.h b/src/capture_base.h index 02d4c5352d725597bf186962a1211969104df81f..e7626e1d5b0f7fbb12a45912afcb25e696e5f464 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 90ebde7321583660c47b0b86ec28239e902d9072..a8af1d1a594bd710f4f94c6b4e581bb7947a5057 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 3ca6aba0c0a022a6173c9f2a5d4c25ba02593e16..ea1421aa9e0ad7b6be65fb8d6df5f2b6663dc9a9 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();