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