Remove _current_frame from constructor: get it from _feature

src/constraint_AHP.h

@@ -28,11 +28,19 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
 
     public:
 
-        ConstraintAHP(FeatureBasePtr _ftr_ptr, FrameBasePtr _current_frame_ptr, LandmarkAHP::Ptr _landmark_ptr,
-                        bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-                ConstraintSparse<2, 3, 4, 3, 4, 4>(CTR_AHP, _landmark_ptr, _apply_loss_function, _status,
-                                             _current_frame_ptr->getPPtr(), _current_frame_ptr->getOPtr(), _landmark_ptr->getAnchorFrame()->getPPtr()
-                                                   ,_landmark_ptr->getAnchorFrame()->getOPtr(),_landmark_ptr->getPPtr()),
+        ConstraintAHP(FeatureBasePtr    _ftr_ptr,
+                      LandmarkAHP::Ptr  _landmark_ptr,
+                      bool              _apply_loss_function    = false,
+                      ConstraintStatus  _status                 = CTR_ACTIVE) :
+                ConstraintSparse<2, 3, 4, 3, 4, 4>(CTR_AHP,
+                                                   _landmark_ptr,
+                                                   _apply_loss_function,
+                                                   _status,
+                                                   _ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr(),
+                                                   _ftr_ptr->getCapturePtr()->getFramePtr()->getOPtr(),
+                                                   _landmark_ptr->getAnchorFrame()->getPPtr(),
+                                                   _landmark_ptr->getAnchorFrame()->getOPtr(),
+                                                   _landmark_ptr->getPPtr()),
                 anchor_sensor_extrinsics_p_(_ftr_ptr->getCapturePtr()->getSensorPPtr()->getVector()),
                 anchor_sensor_extrinsics_o_(_ftr_ptr->getCapturePtr()->getSensorOPtr()->getVector()),
                 feature_image_(*std::static_pointer_cast<FeaturePointImage>(_ftr_ptr))
@@ -165,10 +173,13 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
             return JAC_AUTO;
         }
 
-        static ConstraintAHP::Ptr create(FeatureBasePtr _ftr_ptr, FrameBasePtr _frm_current_ptr, LandmarkAHP::Ptr _lmk_ahp_ptr)
+        static ConstraintAHP::Ptr create(FeatureBasePtr     _ftr_ptr,
+                                         LandmarkAHP::Ptr   _lmk_ahp_ptr,
+                                         bool               _apply_loss_function    = false,
+                                         ConstraintStatus   _status                 = CTR_ACTIVE)
         {
             // construct constraint
-            Ptr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _frm_current_ptr, _lmk_ahp_ptr);
+            Ptr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _lmk_ahp_ptr, _apply_loss_function, _status);
 
             // link it to wolf tree
 //            _ftr_ptr->addConstraint(ctr_ahp);

src/examples/test_constraint_AHP.cpp

@@ -118,7 +118,7 @@ int main()
 
 
     // Create the constraint
-    ConstraintAHP::Ptr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, last_frame,std::static_pointer_cast<LandmarkAHP>(landmark));
+    ConstraintAHP::Ptr constraint_ptr = std::make_shared<ConstraintAHP>(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------------------
-    ConstraintAHP::Ptr ctr_0 = ConstraintAHP::create(feat_0, kf_2, lmk_1 );
+    ConstraintAHP::Ptr ctr_0 = ConstraintAHP::create(feat_0, lmk_1 );
     feat_0->addConstraint(ctr_0);
-    ConstraintAHP::Ptr ctr_1 = ConstraintAHP::create(feat_1, kf_3, lmk_1 );
+    ConstraintAHP::Ptr ctr_1 = ConstraintAHP::create(feat_1, lmk_1 );
     feat_1->addConstraint(ctr_1);
-    ConstraintAHP::Ptr ctr_2 = ConstraintAHP::create(feat_2, kf_4, lmk_1 );
+    ConstraintAHP::Ptr ctr_2 = ConstraintAHP::create(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
-    ConstraintAHP::Ptr ctr_3 = ConstraintAHP::create(feat_3, kf_3, lmk_2 );
+    ConstraintAHP::Ptr ctr_3 = ConstraintAHP::create(feat_3, lmk_2 );
     feat_3->addConstraint(ctr_3);
-    ConstraintAHP::Ptr ctr_4 = ConstraintAHP::create(feat_4, kf_4, lmk_2 );
+    ConstraintAHP::Ptr ctr_4 = ConstraintAHP::create(feat_4, lmk_2 );
     feat_4->addConstraint(ctr_4);
 
     Eigen::Vector2s pix_3 = ctr_3->expectation();

src/processor_image_landmark.cpp

@@ -302,19 +302,18 @@ ConstraintBasePtr ProcessorImageLandmark::createConstraint(FeatureBasePtr _featu
         assert (last_ptr_ && "bad last ptr");
         assert (_landmark_ptr && "bad lmk ptr");
         auto current_frame = last_ptr_->getFramePtr();
-        auto landmark = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
-//        return std::make_shared<ConstraintAHP>(_feature_ptr, current_frame, landmark );
+        auto landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
 
-        ConstraintAHP::Ptr constraint_ptr = std::make_shared<ConstraintAHP>(_feature_ptr, current_frame, landmark, true);
+        ConstraintAHP::Ptr constraint_ptr = std::make_shared<ConstraintAHP>(_feature_ptr, landmark_ahp, true);
 
 
 
         Eigen::Vector2s expectation_;
         Eigen::Vector3s current_frame_p = last_ptr_->getFramePtr()->getPPtr()->getVector();
         Eigen::Vector4s current_frame_o  = last_ptr_->getFramePtr()->getOPtr()->getVector();
-        Eigen::Vector3s anchor_frame_p = landmark->getAnchorFrame()->getPPtr()->getVector();
-        Eigen::Vector4s anchor_frame_o = landmark->getAnchorFrame()->getOPtr()->getVector();
-        Eigen::Vector4s landmark_ = landmark->getPPtr()->getVector();
+        Eigen::Vector3s anchor_frame_p = landmark_ahp->getAnchorFrame()->getPPtr()->getVector();
+        Eigen::Vector4s anchor_frame_o = landmark_ahp->getAnchorFrame()->getOPtr()->getVector();
+        Eigen::Vector4s landmark_ = landmark_ahp->getPPtr()->getVector();
 
         (*constraint_ptr).expectation(current_frame_p.data(), current_frame_o.data(),
                 anchor_frame_p.data(), anchor_frame_o.data(),