plu_minus test passes

include/IMU/math/IMU_tools_Lie.h

@@ -483,24 +483,24 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
 // }
 
 template<typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& delta1,
-                 const MatrixBase<D2>& delta2,
+inline void plus(const MatrixBase<D1>& delta,
+                 const MatrixBase<D2>& d,
                  MatrixBase<D3>& delta_pert)
 {
-    MatrixSizeCheck<11, 1>::check(delta1);
-    MatrixSizeCheck<10, 1>::check(delta2);
+    MatrixSizeCheck<11, 1>::check(delta);
+    MatrixSizeCheck<10, 1>::check(d);
     MatrixSizeCheck<11, 1>::check(delta_pert);
 
-    compose(delta1, exp_IMU(delta2), delta_pert);
+    compose(delta, exp_IMU(d), delta_pert);
 }
 
 template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& delta1,
-                                               const MatrixBase<D2>& delta2)
+inline Matrix<typename D1::Scalar, 11, 1> plus(const MatrixBase<D1>& delta,
+                                               const MatrixBase<D2>& d)
 {
-    Matrix<typename D1::Scalar, 10, 1> ret;
-    plus(delta1, delta2, ret);
-    return ret;
+    Matrix<typename D1::Scalar, 11, 1> delta_ret;
+    plus(delta, d, delta_ret);
+    return delta_ret;
 }
 
 // template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
@@ -530,6 +530,7 @@ inline void diff(const MatrixBase<D1>& delta1,
                  const MatrixBase<D2>& delta2,
                  MatrixBase<D3>& err)
 {
+    // diff(A, B) = B - A
     MatrixSizeCheck<11, 1>::check(delta1);
     MatrixSizeCheck<11, 1>::check(delta2);
     MatrixSizeCheck<10, 1>::check(err);

test/gtest_IMU_tools_Lie.cpp

@@ -153,24 +153,40 @@ TEST(IMU_tools, lift_retract)
     ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
 }
 
-TEST(IMU_tools, plus)
+TEST(IMU_tools, plus_minus)
 {
-    VectorXs delta1(11), delta2(11), delta3(11);
-    VectorXs err(10);
-    Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
-    delta1 << 0, 1, 2, qv, 7, 8, 9, 0.1;
+    VectorXs delta1(11), delta2(11), delta2_eq(11);
+    VectorXs err(10), err_0(10);
+    Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+    Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+    delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
+    delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3;
     err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09, 0.1;
 
-    delta3.head(3) = delta1.head(3) + err.head(3);
-    delta3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
-    delta3.tail(3) = delta1.tail(3) + err.tail(3);
-
-    plus(delta1, err, delta2);
-    ASSERT_MATRIX_APPROX(diff(delta3, delta2), VectorXs::Zero(10), 1e-10);
+    delta2_eq = plus(delta1, diff(delta1, delta2));
+    ASSERT_MATRIX_APPROX(delta2_eq, delta2, 1e-10);
 }
 
+// TEST(IMU_tools, plus)
+// {
+    // FORSTER's formulas -> TODO remove?
+//     VectorXs delta1(11), delta2(11), delta3(11);
+//     VectorXs err(10);
+//     Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+//     delta1 << 0, 1, 2, qv, 7, 8, 9, 0.1;
+//     err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09, 0.1;
+
+//     delta3.head(3) = delta1.head(3) + err.head(3);
+//     delta3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
+//     delta3.tail(3) = delta1.tail(3) + err.tail(3);
+
+//     plus(delta1, err, delta2);
+//     ASSERT_MATRIX_APPROX(diff(delta3, delta2), VectorXs::Zero(10), 1e-10);
+// }
+
 // TEST(IMU_tools, diff)
 // {
+    // FORSTER's formulas -> TODO remove?
 //     VectorXs delta1(10), delta2(10);
 //     Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
 //     delta1 << 0, 1, 2, qv, 7, 8, 9;
@@ -188,9 +204,11 @@ TEST(IMU_tools, plus)
 //     err.tail(3) = delta3.tail(3) - delta1.tail(3);
 
 //     ASSERT_MATRIX_APPROX(err, diff(delta1, delta3), 1e-10);
-
 // }
 
+
+
+
 // TEST(IMU_tools, compose_jacobians)
 // {
 //     VectorXs delta1(10), delta2(10), delta3(10), delta1_pert(10), delta2_pert(10), delta3_pert(10); // deltas