From 3b2db45f4cba8cf810e17ed027405c895861d1e8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Sat, 29 Feb 2020 16:23:53 +0100
Subject: [PATCH] New test cases for factor block difference
---
test/gtest_factor_block_difference.cpp | 68 +++++++++++++++++++++++---
1 file changed, 61 insertions(+), 7 deletions(-)
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 806914051..0945b328d 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -15,6 +15,7 @@
#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
#include "core/factor/factor_block_difference.h"
+#include "core/factor/factor_block_absolute.h"
using namespace Eigen;
@@ -47,8 +48,14 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t0(0);
TimeStamp t1(1);
+ Vector10d x_origin = problem_->zeroState();
Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
- KF0_ =problem_->setPrior(problem_->zeroState(), cov_prior, t0, 0.1);
+ KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+
+ CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
+ FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
+ FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+
// KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
@@ -68,21 +75,68 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
Feat_, KF0_->getP(), KF1_->getP()
);
- KF1_->getO()->fix();
- KF1_->getV()->fix();
-
// perturbate
KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
- problem_->print(4,1,1,1);
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
- std::cout << report << std::endl;
- problem_->print(4,1,1,1);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
}
+TEST_F(FixtureFactorBlockDifference, EqualV)
+{
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ Feat_, KF0_->getV(), KF1_->getV()
+ );
+
+ // perturbate
+ KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
+
+ std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8);
+
+}
+
+
+
+TEST_F(FixtureFactorBlockDifference, DiffP)
+{
+ Vector3d diffP = Vector3d::Random();
+ Feat_->setMeasurement(diffP);
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ Feat_, KF0_->getP(), KF1_->getP()
+ );
+
+ // perturbate
+ KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
+
+ std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diffP, 1e-8);
+
+}
+
+
+
+TEST_F(FixtureFactorBlockDifference, DiffV)
+{
+ Vector3d diffV = Vector3d::Random();
+ Feat_->setMeasurement(diffV);
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ Feat_, KF0_->getV(), KF1_->getV()
+ );
+
+ // perturbate
+ KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
+
+ std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diffV, 1e-8);
+
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
--
GitLab