fix factor error() method (not in gtest) + gtest_imu use perturb

include/imu/factor/factor_imu.h

@@ -235,9 +235,9 @@ Eigen::Vector9d FactorImu::error()
     Map<const Vector3d > acc_bias(bias.data());
     Map<const Vector3d > gyro_bias(bias.data() + 3);
 
-    Eigen::Vector9d delta_exp = expectation();
+    Eigen::Vector10d delta_exp = expectation();
 
-    Eigen::Vector9d delta_preint = getMeasurement();
+    Eigen::Vector10d delta_preint = getMeasurement();
 
     Eigen::Vector9d delta_step;
 
@@ -247,9 +247,9 @@ Eigen::Vector9d FactorImu::error()
 
     Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
 
-    Eigen::Vector9d res = imu::diff(delta_exp, delta_corr);
+    Eigen::Vector9d err = imu::diff(delta_exp, delta_corr);
 
-    return res;
+    return err;
 }
 
 template<typename D1, typename D2, typename D3>

test/gtest_imu.cpp

@@ -475,28 +475,10 @@ class Process_Factor_Imu : public testing::Test
         {
             // This perturbs states to estimate around the exact value, then assigns to the keyframe
             // Perturbations are applied only if the state block is unfixed
-
-            VectorXd x_pert(10);
-
-            // KF 0
-            x_pert = x0;
-            if (!p0_fixed)
-                x_pert.head(3)      += Vector3d::Random() * 0.01;
-            if (!q0_fixed)
-                x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
-            if (!v0_fixed)
-                x_pert.tail(3)      += Vector3d::Random() * 0.01;
-            KF_0->setState(x_pert);
-
-            // KF 1
-            x_pert = x1_exact;
-            if (!p1_fixed)
-                x_pert.head(3)      += Vector3d::Random() * 0.01;
-            if (!q1_fixed)
-                x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
-            if (!v1_fixed)
-                x_pert.tail(3)      += Vector3d::Random() * 0.01;
-            KF_1->setState(x_pert);
+            KF_0->setState(x0);
+            KF_0->perturb();
+            KF_1->setState(x1_exact);
+            KF_1->perturb();
         }
 
         virtual void buildProblem()