diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index adec4caf9e8554a4b95baa11dd3dcf5846c06eae..4e29f0bf7f10105fa98d9446b2dd1370e0535275 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -145,20 +145,22 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) { assert(this->getFeature() == nullptr && "linking an already linked factor"); - if(_ftr_ptr) + // not link if nullptr + if(_ftr_ptr == nullptr) { - _ftr_ptr->addFactor(shared_from_this()); - this->setFeature(_ftr_ptr); - this->setProblem(_ftr_ptr->getProblem()); - // add factor to be added in solver - if (this->getProblem() == nullptr) - { - WOLF_WARN("ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM."); - } - } - else WOLF_WARN("Linking with nullptr"); + return; + } + + // link with feature + _ftr_ptr->addFactor(shared_from_this()); + this->setFeature(_ftr_ptr); + + // set problem ( and register factor ) + WOLF_WARN_COND(this->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM."); + this->setProblem(_ftr_ptr->getProblem()); + // constrained by auto frame_other = this->frame_other_ptr_.lock(); if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this()); auto capture_other = this->capture_other_ptr_.lock();