diff --git a/include/vision/factor/factor_ahp.h b/include/vision/factor/factor_ahp.h
index b45ecbc0498da40eff71a91e885530c12c508e94..b1a58890015ec8f7688307d5eb64359743d9b67c 100644
--- a/include/vision/factor/factor_ahp.h
+++ b/include/vision/factor/factor_ahp.h
@@ -63,6 +63,7 @@ inline FactorAhp::FactorAhp(const FeatureBasePtr& _ftr_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff<FactorAhp, 2, 3, 4, 3, 4, 4>("AHP",
+ _ftr_ptr,
_landmark_ptr->getAnchorFrame(),
nullptr,
nullptr,
diff --git a/include/vision/factor/factor_epipolar.h b/include/vision/factor/factor_epipolar.h
index 55fd40059e47b7fb9e325d7e15dcc9593e99be66..f6654884dca7183e1264934ee7d95ca90aaea83a 100644
--- a/include/vision/factor/factor_epipolar.h
+++ b/include/vision/factor/factor_epipolar.h
@@ -47,6 +47,7 @@ inline FactorEpipolar::FactorEpipolar(const FeatureBasePtr& _feature_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff<FactorEpipolar, 1, 3, 4, 3, 4, 3, 4>("FEATURE EPIPOLAR",
+ _feature_ptr,
nullptr,
nullptr,
_feature_other_ptr,
diff --git a/include/vision/factor/factor_pixel_hp.h b/include/vision/factor/factor_pixel_hp.h
index dd52776d393edb285d58ce364d39dd951a903a39..13f3d0cff4241218dd3f4d0e814cda409b741bfc 100644
--- a/include/vision/factor/factor_pixel_hp.h
+++ b/include/vision/factor/factor_pixel_hp.h
@@ -62,6 +62,7 @@ inline FactorPixelHp::FactorPixelHp(const FeatureBasePtr& _ftr_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff<FactorPixelHp, 2, 3, 4, 3, 4, 4>("PIXELHP",
+ _ftr_ptr,
nullptr,
nullptr,
nullptr,
diff --git a/include/vision/factor/factor_trifocal.h b/include/vision/factor/factor_trifocal.h
index 4d99ae0892b7fe90d671a0f5232c615bca7bc239..3781ec621ed91a5efb1f1d3594b60e56d590c8f9 100644
--- a/include/vision/factor/factor_trifocal.h
+++ b/include/vision/factor/factor_trifocal.h
@@ -151,6 +151,7 @@ FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff( "TRIFOCAL PLP",
+ _feature_own_ptr,
nullptr,
nullptr,
_feature_2_ptr, //< this sets feature 2 (the one between the oldest and the newest)
diff --git a/test/gtest_factor_epipolar.cpp b/test/gtest_factor_epipolar.cpp
index ea3d18606081d30668fcec87de8295faa6432d7e..9cda2a79fa4e8cbc0de72ebd40975a4a35ab976e 100644
--- a/test/gtest_factor_epipolar.cpp
+++ b/test/gtest_factor_epipolar.cpp
@@ -46,59 +46,65 @@ TEST(FactorEpipolar, exemple)
auto C0 = CaptureBase ::emplace<CaptureImage>(F0, F0->getTimeStamp(), camera, cv::Mat());
auto C1 = CaptureBase ::emplace<CaptureImage>(F1, F1->getTimeStamp(), camera, cv::Mat());
auto f0 = FeatureBase ::emplace<FeaturePointImage>(C0, pix0, 0, cv::Mat(), Matrix2d::Identity());
- auto f1 = FeatureBase ::emplace<FeaturePointImage>(C1, pix1, 0, cv::Mat(), Matrix2d::Identity());
- auto c = FactorBase ::emplace<FactorEpipolar>(f0, f0, f1, nullptr, false);
double residual_0, residual_1, residual_2, residual_n1;
// same line
- c->operator()(F0->getP()->getState().data(),
- F0->getO()->getState().data(),
- F1->getP()->getState().data(),
- F1->getO()->getState().data(),
- camera->getP()->getState().data(),
- camera->getO()->getState().data(),
- &residual_0);
+ auto f1 = FeatureBase ::emplace<FeaturePointImage>(C1, pix1, 0, cv::Mat(), Matrix2d::Identity());
+ auto c0 = FactorBase ::emplace<FactorEpipolar>(f0, f0, f1, nullptr, false);
+ c0->operator()(F0->getP()->getState().data(),
+ F0->getO()->getState().data(),
+ F1->getP()->getState().data(),
+ F1->getO()->getState().data(),
+ camera->getP()->getState().data(),
+ camera->getO()->getState().data(),
+ &residual_0);
WOLF_TRACE("residual @ 0 pix: ", residual_0);
ASSERT_NEAR(residual_0, 0.0, 1e-6);
// lines 1 pix difference
- f1->setMeasurement(Vector2d(300, 241));
- c->operator()(F0->getP()->getState().data(),
- F0->getO()->getState().data(),
- F1->getP()->getState().data(),
- F1->getO()->getState().data(),
- camera->getP()->getState().data(),
- camera->getO()->getState().data(),
- &residual_1);
+ auto f2 = FeatureBase ::emplace<FeaturePointImage>(C1, Vector2d(300, 241), 0, cv::Mat(), Matrix2d::Identity());
+ auto c1 = FactorBase ::emplace<FactorEpipolar>(f0, f0, f2, nullptr, false);
+ //f1->setMeasurement(Vector2d(300, 241));
+ c1->operator()(F0->getP()->getState().data(),
+ F0->getO()->getState().data(),
+ F1->getP()->getState().data(),
+ F1->getO()->getState().data(),
+ camera->getP()->getState().data(),
+ camera->getO()->getState().data(),
+ &residual_1);
WOLF_TRACE("residual @ 1 pix : ", residual_1);
// lines 2 pixels difference
- f1->setMeasurement(Vector2d(300, 242));
- c->operator()(F0->getP()->getState().data(),
- F0->getO()->getState().data(),
- F1->getP()->getState().data(),
- F1->getO()->getState().data(),
- camera->getP()->getState().data(),
- camera->getO()->getState().data(),
- &residual_2);
+ auto f3 = FeatureBase ::emplace<FeaturePointImage>(C1, Vector2d(300, 242), 0, cv::Mat(), Matrix2d::Identity());
+ auto c2 = FactorBase ::emplace<FactorEpipolar>(f0, f0, f3, nullptr, false);
+ //f1->setMeasurement(Vector2d(300, 242));
+ c2->operator()(F0->getP()->getState().data(),
+ F0->getO()->getState().data(),
+ F1->getP()->getState().data(),
+ F1->getO()->getState().data(),
+ camera->getP()->getState().data(),
+ camera->getO()->getState().data(),
+ &residual_2);
WOLF_TRACE("residual @ 2 pix : ", residual_2);
ASSERT_NEAR(residual_2, 2.0 * residual_1, 1e-6);
// lines 1 pix difference in the other direction
- f1->setMeasurement(Vector2d(300, 239));
- c->operator()(F0->getP()->getState().data(),
- F0->getO()->getState().data(),
- F1->getP()->getState().data(),
- F1->getO()->getState().data(),
- camera->getP()->getState().data(),
- camera->getO()->getState().data(),
- &residual_n1);
+ auto f4 = FeatureBase ::emplace<FeaturePointImage>(C1, Vector2d(300, 239), 0, cv::Mat(), Matrix2d::Identity());
+ auto c3 = FactorBase ::emplace<FactorEpipolar>(f0, f0, f4, nullptr, false);
+ //f1->setMeasurement(Vector2d(300, 239));
+ c3->operator()(F0->getP()->getState().data(),
+ F0->getO()->getState().data(),
+ F1->getP()->getState().data(),
+ F1->getO()->getState().data(),
+ camera->getP()->getState().data(),
+ camera->getO()->getState().data(),
+ &residual_n1);
WOLF_TRACE("residual @ -1 pix : ", residual_n1);