Skip to content
Snippets Groups Projects
Commit 55893ac6 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

gtest passing

parent 765c77e8
No related branches found
No related tags found
2 merge requests!24After 2nd RAL submission,!18Resolve "Copying measurement and sqrtinfo in FactorBase (multi-threading)"
......@@ -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,
......
......@@ -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,
......
......@@ -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,
......
......@@ -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)
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment