From 55893ac6ef057801e529d605ba1cef56365075f8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 3 Jun 2020 19:15:38 +0200
Subject: [PATCH] gtest passing
---
include/vision/factor/factor_ahp.h | 1 +
include/vision/factor/factor_epipolar.h | 1 +
include/vision/factor/factor_pixel_hp.h | 1 +
include/vision/factor/factor_trifocal.h | 1 +
test/gtest_factor_epipolar.cpp | 72 +++++++++++++------------
5 files changed, 43 insertions(+), 33 deletions(-)
diff --git a/include/vision/factor/factor_ahp.h b/include/vision/factor/factor_ahp.h
index b45ecbc04..b1a588900 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 55fd40059..f6654884d 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 dd52776d3..13f3d0cff 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 4d99ae089..3781ec621 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 ea3d18606..9cda2a79f 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);
--
GitLab