WIP: Add test for the Strides

The test now works for a dummy example.

Need a test for realistic IMU case.

Then will need to apply the solution to all IMU_tools.h

test/gtest_IMU_tools.cpp

@@ -6,6 +6,7 @@
  */
 
 #include "IMU/math/IMU_tools.h"
+
 #include <core/utils/utils_gtest.h>
 
 using namespace Eigen;
@@ -620,10 +621,57 @@ TEST(IMU_tools, full_delta_residual)
 //    WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
 }
 
+template <typename D1, typename D2, typename D3>
+void sum (const MatrixBase<D1>& m,
+        const MatrixBase<D2>& n,
+        MatrixBase<D3>& sum)
+{
+    WOLF_TRACE("M col: ", m.colStride(), " row: ",m.rowStride());
+    WOLF_TRACE("N col: ", n.colStride(), " row: ",n.rowStride());
+
+    WOLF_TRACE("m\n", m);
+    WOLF_TRACE("n\n", n);
+
+    Map<const Vector3s> m1 (&m(0) + 0);
+    WOLF_TRACE("m1\n", m1);
+
+    Map<const Vector3s> n1 (&n(0) + 0);
+    WOLF_TRACE("n1\n", n1);
+
+    Map<const VectorXs, 0, Stride<Dynamic, Dynamic> > m2(&m(0) + 0, 3, 1, Stride<Dynamic, Dynamic>(m.colStride(),m.rowStride()));
+    WOLF_TRACE("m2\n", m2);
+
+    Map<const VectorXs, 0, Stride<Dynamic, Dynamic> > n2(&n(0) + 0, 3, 1, Stride<Dynamic, Dynamic>(n.colStride(),n.rowStride()));
+    WOLF_TRACE("n2\n", n2);
+
+    sum = m2 + n2;
+
+    WOLF_TRACE("sum\n", sum);
+}
+
+TEST(IMU_tools, Stride)
+{
+    Matrix<Scalar, Dynamic, Dynamic, RowMajor> L(4,5), M(4,5); // RowMajor
+    Matrix<Scalar, Dynamic, Dynamic> N(4,5); // ColMajor
+    for (int i = 0; i < M.size(); ++i) M(i) = i;
+    for (int i = 0; i < N.size(); ++i) N(i) = i;
+    VectorXs d;
+
+    WOLF_TRACE("M\n", M);
+    WOLF_TRACE("N\n", N);
+
+    sum(M.col(1), N.col(1), d);
+
+    WOLF_TRACE("M(:,1) + N(:,1) = \n", d);
+
+    ASSERT_MATRIX_APPROX(d.head(3), M.col(1).head(3)+N.col(1).head(3), 1e-16);
+
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-//  ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
+  ::testing::GTEST_FLAG(filter) = "IMU_tools.Stride";
   return RUN_ALL_TESTS();
 }