From 91e5905677052ecb02525dca337ea2712878c6df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Mon, 21 Dec 2020 00:00:36 +0100 Subject: [PATCH] Use perturb() --- test/gtest_factor_trifocal.cpp | 26 +++----------------------- 1 file changed, 3 insertions(+), 23 deletions(-) diff --git a/test/gtest_factor_trifocal.cpp b/test/gtest_factor_trifocal.cpp index 2dd5fd72c..07266a710 100644 --- a/test/gtest_factor_trifocal.cpp +++ b/test/gtest_factor_trifocal.cpp @@ -384,12 +384,7 @@ TEST_F(FactorTrifocalTest, solve_F1) ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8); // Residual with perturbated state - -// Vector7d pose_perturbated = F1->getState() + 0.1 * Vector7d::Random(); -// pose_perturbated.segment(3,4).normalize(); -// F1->setState(pose_perturbated); F1->perturb(0.1); -// VectorXd pose_perturbated = F1->getState().vector("PO"); F1_p = F1->getP()->getState(); F1_o = F1->getO()->getState(); @@ -769,12 +764,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point) F3->getO()->unfix(); // Estimate Cam 3 ori // Perturbate states, keep scale - F1->getP()->setState( pos1 ); - F1->getO()->setState( vquat1 ); - F2->getP()->setState( pos2 ); // this fixes the scale - F2->getO()->setState((vquat2 + 0.2*Vector4d::Random()).normalized()); - F3->getP()->setState( pos3 + 0.2*Vector3d::Random()); - F3->getO()->setState((vquat3 + 0.2*Vector4d::Random()).normalized()); + problem->perturb(0.1); std::string report = solver->solve(SolverManager::ReportVerbosity::FULL); @@ -834,12 +824,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale) F3->getO()->unfix(); // Estimate Cam 3 ori // Perturbate states, change scale - F1->getP()->setState( 2 * pos1 ); - F1->getO()->setState( vquat1 ); - F2->getP()->setState( 2 * pos2 ); - F2->getO()->setState(( vquat2 + 0.2*Vector4d::Random()).normalized()); - F3->getP()->setState( 2 * pos3 + 0.2*Vector3d::Random()); - F3->getO()->setState(( vquat3 + 0.2*Vector4d::Random()).normalized()); + problem->perturb(0.1); std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); @@ -900,12 +885,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance) F3->getO()->unfix(); // Estimate Cam 3 ori // Perturbate states, change scale - F1->getP()->setState( pos1 ); - F1->getO()->setState( vquat1 ); - F2->getP()->setState( pos2 + 0.2*Vector3d::Random() ); - F2->getO()->setState((vquat2 + 0.2*Vector4d::Random()).normalized()); - F3->getP()->setState( pos3 + 0.2*Vector3d::Random()); - F3->getO()->setState((vquat3 + 0.2*Vector4d::Random()).normalized()); + problem->perturb(0.1); // Add a distance factor to fix the scale double distance = sqrt(2.0); -- GitLab