Remove shared_from_this in the lmk case

Frame and Feature cases are marked with FIXME for later evaluation

src/constraint_base.cpp

@@ -38,7 +38,7 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, FrameBasePtr _frame_ptr, bool
     // add constraint to frame
     FrameBasePtr frm_o = frame_other_ptr_.lock();
     if (frm_o)
-        frm_o->addConstrainedBy(shared_from_this());
+        frm_o->addConstrainedBy(shared_from_this()); // FIXME: cannot do shared_from_this in constructor!
     std::cout << "constructed      +c" << id() << std::endl;
 }
 
@@ -58,7 +58,7 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, FeatureBasePtr _feature_ptr,
     // add constraint to feature
     FeatureBasePtr ftr_o = feature_other_ptr_.lock();
     if (ftr_o)
-        ftr_o->addConstrainedBy(shared_from_this());
+        ftr_o->addConstrainedBy(shared_from_this()); // FIXME: cannot do shared_from_this in constructor!
     std::cout << "constructed      +c" << id() << std::endl;
 }
 
@@ -76,11 +76,6 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, LandmarkBasePtr _landmark_ptr
     landmark_other_ptr_(_landmark_ptr)
 {
     std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
-    // add constraint to landmark
-    LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
-    if (lmk_o)
-        std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
-        lmk_o->addConstrainedBy(shared_from_this()); // FIXME: cannot do shared_from_this in constructor!
     std::cout << "constructed      +c" << id() << std::endl;
 }
 

src/processor_tracker_landmark.cpp

@@ -130,17 +130,18 @@ void ProcessorTrackerLandmark::establishConstraints()
     //std::cout << "ProcessorTrackerLandmark::establishConstraints" << std::endl;
     //std::cout << "\tfeatures:" << last_ptr_->getFeatureList().size() << std::endl;
     //std::cout << "\tcorrespondences: " << matches_landmark_from_last_.size() << std::endl;
-    std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
     for (auto last_feature : last_ptr_->getFeatureList())
     {
-        std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
+        auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
         ConstraintBasePtr ctr_ptr = createConstraint(last_feature,
-                                                     matches_landmark_from_last_[last_feature]->landmark_ptr_);
-        if (ctr_ptr != nullptr)
+                                                     lmk);
+        if (ctr_ptr != nullptr) // constraint links
         {
             std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
             last_feature->addConstraint(ctr_ptr);
             std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
+            lmk->addConstrainedBy(ctr_ptr);
+            std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
         }
         std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
     }