diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 92cd31c4586f3e549cba372374bef58ff286fe34..f2be4153e8c5c0d722fb2f3e156ad5f971e89944 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -252,14 +252,13 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) return; } - // copy measurement_ and measurement_sqrt_information_upper_ - measurement_ = _ftr_ptr->getMeasurement(); - measurement_sqrt_information_upper_ = _ftr_ptr->getMeasurementSquareRootInformationUpper(); - // link with feature _ftr_ptr->addFactor(shared_from_this()); this->setFeature(_ftr_ptr); + // update measurement and info according to the feature + updateMeasurementAndSquareRootInformationUpper(); + // if frame, should be key if (getCapture() and getFrame()) assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");