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/problem.cpp b/src/problem.cpp index 3989d2695e61c95b5b3b1068131f40461c9621ab..11852deb2469e4b8e8861d4a90fcd3fe119894d9 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -849,24 +849,21 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) cout << endl; } - if (C->isMotion() && metric) + if (C->isMotion() ) { - try + CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C); + cout << " buffer size : " << CM->getBuffer().get().size() << endl; + if ( metric && ! CM->getBuffer().get().empty()) { - CaptureMotionPtr CM = std::static_pointer_cast<CaptureMotion>(C); - cout << " buffer size : " << CM->getBuffer().get().size() << endl; - if ( CM->getCalibSize() > 0 && ! CM->getBuffer().get().empty()) + cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl; + if (CM->hasCalibration()) { - cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl; cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl; cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl; cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl; cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl; } } - catch (std::runtime_error& e) - { - } } if (depth >= 3) 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();