diff --git a/test/gtest_factor_trifocal.cpp b/test/gtest_factor_trifocal.cpp
index 07266a710b5f89bff1f50ad467e7c2d8f5fdd1b6..0d15c6bb3d4e17d8e79f159c36d6904276caf533 100644
--- a/test/gtest_factor_trifocal.cpp
+++ b/test/gtest_factor_trifocal.cpp
@@ -182,7 +182,7 @@ TEST_F(FactorTrifocalTest, InfoMatrix)
      */
     Matrix3d sqrt_info_gt = Matrix3d::Identity() / pixel_noise_std / sqrt(2.0);
 
-    ASSERT_MATRIX_APPROX(c123->getSqrtInformationUpper(), sqrt_info_gt, 1e-8);
+    ASSERT_MATRIX_APPROX(c123->getSqrtInformationUpper(), sqrt_info_gt, 1e-6);
 
 }
 
@@ -228,10 +228,10 @@ TEST_F(FactorTrifocalTest, expectation)
     l2 = c2Ec1 * _m1;
 
     // check epipolar lines (check only director vectors for equal direction)
-    ASSERT_MATRIX_APPROX(l2/l2(1), _l2/_l2(1), 1e-8);
+    ASSERT_MATRIX_APPROX(l2/l2(1), _l2/_l2(1), 1e-6);
 
     // check perpendicular lines (check only director vectors for orthogonal direction)
-    ASSERT_NEAR(l2(0)*_p2(0) + l2(1)*_p2(1), 0, 1e-8);
+    ASSERT_NEAR(l2(0)*_p2(0) + l2(1)*_p2(1), 0, 1e-6);
 
     // Verify trilinearities
 
@@ -241,18 +241,18 @@ TEST_F(FactorTrifocalTest, expectation)
 
     // Point-line-point
     Vector3d plp = wolf::skew(_m3) * m1Tt * _p2;
-    ASSERT_MATRIX_APPROX(plp, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(plp, Vector3d::Zero(), 1e-6);
 
     // Point-point-line
     Vector3d ppl = _p3.transpose() * m1Tt * wolf::skew(_m2);
-    ASSERT_MATRIX_APPROX(ppl, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(ppl, Vector3d::Zero(), 1e-6);
 
     // Point-point-point
     Matrix3d ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2);
-    ASSERT_MATRIX_APPROX(ppp, Matrix3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(ppp, Matrix3d::Zero(), 1e-6);
 
     // check epipolars
-    ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-8);
+    ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-6);
 }
 
 TEST_F(FactorTrifocalTest, residual)
@@ -265,7 +265,7 @@ TEST_F(FactorTrifocalTest, residual)
     c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1);
     residual = c123->residual(tensor, c2Ec1);
 
-    ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-6);
 }
 
 TEST_F(FactorTrifocalTest, error_jacobians)
@@ -350,7 +350,7 @@ TEST_F(FactorTrifocalTest, operator_parenthesis)
                       pos_cam.data(), vquat_cam.data(),
                       res.data());
 
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 }
 
 TEST_F(FactorTrifocalTest, solve_F1)
@@ -381,7 +381,7 @@ TEST_F(FactorTrifocalTest, solve_F1)
 
     WOLF_DEBUG("Initial state:              ", F1->getState().transpose());
     WOLF_DEBUG("residual before perturbing: ", res.transpose());
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
     // Residual with perturbated state
     F1->perturb(0.1);
@@ -427,7 +427,7 @@ TEST_F(FactorTrifocalTest, solve_F1)
 
     WOLF_DEBUG(report, " AND UNION");
 
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
 }
 
@@ -460,7 +460,7 @@ TEST_F(FactorTrifocalTest, solve_F2)
 
     WOLF_DEBUG("Initial state:              ", F2->getState().vector("PO").transpose());
     WOLF_DEBUG("residual before perturbing: ", res.transpose());
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
     // Residual with perturbated state
 
@@ -507,7 +507,7 @@ TEST_F(FactorTrifocalTest, solve_F2)
 
     WOLF_DEBUG(report, " AND UNION");
 
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
 }
 
@@ -540,7 +540,7 @@ TEST_F(FactorTrifocalTest, solve_F3)
 
     WOLF_DEBUG("Initial state:              ", F3->getState().vector("PO").transpose());
     WOLF_DEBUG("residual before perturbing: ", res.transpose());
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
     // Residual with perturbated state
 
@@ -557,7 +557,7 @@ TEST_F(FactorTrifocalTest, solve_F3)
 
     WOLF_DEBUG("perturbed state:            ", F3->getState().vector("PO").transpose());
     WOLF_DEBUG("residual before solve:      ", res.transpose());
-    ASSERT_NEAR(res(2), 0, 1e-8); // Epipolar c2-c1 should be respected when perturbing F3
+    ASSERT_NEAR(res(2), 0, 1e-6); // Epipolar c2-c1 should be respected when perturbing F3
 
     // Residual with solved state
 
@@ -588,7 +588,7 @@ TEST_F(FactorTrifocalTest, solve_F3)
 
     WOLF_DEBUG(report, " AND UNION");
 
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
 }
 
@@ -621,7 +621,7 @@ TEST_F(FactorTrifocalTest, solve_S)
 
     WOLF_DEBUG("Initial state:              ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
     WOLF_DEBUG("residual before perturbing: ", res.transpose());
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
     // Residual with perturbated state
 
@@ -668,7 +668,7 @@ TEST_F(FactorTrifocalTest, solve_S)
 
     WOLF_DEBUG(report, " AND UNION");
 
-    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
 
 }
 
@@ -797,7 +797,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point)
                                  S_p. data(), S_o. data(),
                                  res.data());
 
-        ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+        ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
     }
 
 }
@@ -824,7 +824,13 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale)
     F3->getO()->unfix(); // Estimate Cam 3 ori
 
     // Perturbate states, change scale
-    problem->perturb(0.1);
+//    problem->perturb(0.1);
+    F1->getP()->setState( 2 * pos1 );
+    F1->getO()->setState(   vquat1 );
+    F2->getP()->setState( 2 * pos2 );
+    F2->getO()->setState((  vquat2 + 0.1*Vector4d::Random()).normalized());
+    F3->getP()->setState( 2 * pos3 + 0.1*Vector3d::Random());
+    F3->getO()->setState((  vquat3 + 0.1*Vector4d::Random()).normalized());
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
@@ -833,10 +839,10 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale)
     problem->print(1,0,1,0);
 
     // Evaluate final states
-    ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-8);
-    ASSERT_MATRIX_APPROX(F2->getO()->getState(),   vquat2, 1e-8);
-    ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-8);
-    ASSERT_MATRIX_APPROX(F3->getO()->getState(),   vquat3, 1e-8);
+    ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-6);
+    ASSERT_MATRIX_APPROX(F2->getO()->getState(),   vquat2, 1e-6);
+    ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-6);
+    ASSERT_MATRIX_APPROX(F3->getO()->getState(),   vquat3, 1e-6);
 
     Eigen::VectorXd F1_p = F1->getP()->getState();
     Eigen::VectorXd F1_o = F1->getO()->getState();
@@ -857,7 +863,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale)
                                  S_p. data(), S_o. data(),
                                  res.data());
 
-        ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+        ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
     }
 }
 
@@ -885,7 +891,13 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance)
     F3->getO()->unfix(); // Estimate Cam 3 ori
 
     // Perturbate states, change scale
-    problem->perturb(0.1);
+//    problem->perturb(0.1);
+    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());
 
     // Add a distance factor to fix the scale
     double distance     = sqrt(2.0);
@@ -916,10 +928,10 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance)
     problem->print(1,0,1,0);
 
     // Evaluate final states
-    ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2  , 1e-8);
-    ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
-    ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3  , 1e-8);
-    ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
+    ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2  , 1e-6);
+    ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-6);
+    ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3  , 1e-6);
+    ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-6);
 
     Eigen::VectorXd F1_p = F1->getP()->getState();
     Eigen::VectorXd F1_o = F1->getO()->getState();
@@ -940,7 +952,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance)
                                  S_p. data(), S_o. data(),
                                  res.data());
 
-        ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+        ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
     }
 }