Fix some processor pointers in Constraint constructors

src/examples/test_constraint_AHP.cpp

@@ -43,7 +43,7 @@ int main()
 
     // PROCESSOR
     // one-liner API
-    wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_ORB.yaml");
+    ProcessorBasePtr processor_ptr = wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_ORB.yaml");
 
 
     // create the current frame
@@ -118,7 +118,7 @@ int main()
 
 
     // Create the constraint
-    ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark));
+    ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(processor_ptr, feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark));
 
     feat_point_image_ptr->addConstraint(constraint_ptr);
     std::cout << "Constraint AHP created" << std::endl;

src/examples/test_simple_AHP.cpp

@@ -159,11 +159,11 @@ int main(int argc, char** argv)
     std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
 
     // Constraints------------------
-    ConstraintAHPPtr ctr_0 = ConstraintAHP::create(feat_0, lmk_1 );
+    ConstraintAHPPtr ctr_0 = ConstraintAHP::create(nullptr, feat_0, lmk_1 );
     feat_0->addConstraint(ctr_0);
-    ConstraintAHPPtr ctr_1 = ConstraintAHP::create(feat_1, lmk_1 );
+    ConstraintAHPPtr ctr_1 = ConstraintAHP::create(nullptr, feat_1, lmk_1 );
     feat_1->addConstraint(ctr_1);
-    ConstraintAHPPtr ctr_2 = ConstraintAHP::create(feat_2, lmk_1 );
+    ConstraintAHPPtr ctr_2 = ConstraintAHP::create(nullptr, feat_2, lmk_1 );
     feat_2->addConstraint(ctr_2);
 
     // Projections----------------------------
@@ -210,9 +210,9 @@ int main(int argc, char** argv)
     std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
 
     // New constraints from kf3 and kf4
-    ConstraintAHPPtr ctr_3 = ConstraintAHP::create(feat_3, lmk_2 );
+    ConstraintAHPPtr ctr_3 = ConstraintAHP::create(nullptr, feat_3, lmk_2 );
     feat_3->addConstraint(ctr_3);
-    ConstraintAHPPtr ctr_4 = ConstraintAHP::create(feat_4, lmk_2 );
+    ConstraintAHPPtr ctr_4 = ConstraintAHP::create(nullptr, feat_4, lmk_2 );
     feat_4->addConstraint(ctr_4);
 
     Eigen::Vector2s pix_3 = ctr_3->expectation();

src/processor_image_feature.h

@@ -185,7 +185,7 @@ inline bool ProcessorImageFeature::voteForKeyFrame()
 
 inline ConstraintBasePtr ProcessorImageFeature::createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
 {
-    ConstraintEpipolarPtr const_epipolar_ptr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr);
+    ConstraintEpipolarPtr const_epipolar_ptr = std::make_shared<ConstraintEpipolar>(shared_from_this(), _feature_ptr, _feature_other_ptr);
 //    _feature_ptr->addConstraint(const_epipolar_ptr);
 //    _feature_other_ptr->addConstrainedBy(const_epipolar_ptr);
     return const_epipolar_ptr;

src/processor_image_landmark.cpp

@@ -292,7 +292,7 @@ ConstraintBasePtr ProcessorImageLandmark::createConstraint(FeatureBasePtr _featu
 
         LandmarkAHPPtr landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
 
-        ConstraintAHPPtr constraint_ptr = ConstraintAHP::create(_feature_ptr, landmark_ahp, true);
+        ConstraintAHPPtr constraint_ptr = ConstraintAHP::create(shared_from_this(), _feature_ptr, landmark_ahp, true);
 
         return constraint_ptr;
     }