Add rot() test but commented - Stride in Quaternion not possible

test/gtest_IMU_tools.cpp

@@ -623,8 +623,8 @@ TEST(IMU_tools, full_delta_residual)
 
 template <typename D1, typename D2, typename D3>
 void sum (const MatrixBase<D1>& m,
-        const MatrixBase<D2>& n,
-        MatrixBase<D3>& sum)
+          const MatrixBase<D2>& n,
+          MatrixBase<D3>& sum)
 {
     WOLF_TRACE("Original inputs ---------------");
     WOLF_TRACE("m\n", m);
@@ -652,7 +652,7 @@ void sum (const MatrixBase<D1>& m,
     WOLF_TRACE("sum\n", sum);
 }
 
-TEST(IMU_tools, Stride)
+TEST(IMU_tools, Stride_sum)
 {
     Matrix<Scalar, Dynamic, Dynamic, RowMajor> L(4,5), M(4,5); // RowMajor
     Matrix<Scalar, Dynamic, Dynamic> N(4,5); // ColMajor
@@ -671,10 +671,59 @@ TEST(IMU_tools, Stride)
 
 }
 
+//template <typename D1, typename D2, typename D3>
+//void rot (const MatrixBase<D1>& q,
+//          const MatrixBase<D2>& v,
+//          MatrixBase<D3>& vr)
+//{
+//    WOLF_TRACE("Original inputs ---------------");
+//    WOLF_TRACE("q\n", q);
+//    WOLF_TRACE("v\n", v);
+//
+//    WOLF_TRACE("Subjacent matrix strides ------");
+//    WOLF_TRACE("M Stride: col: ", q.colStride(), " row: ",q.rowStride());
+//    WOLF_TRACE("N Stride: col: ", v.colStride(), " row: ",v.rowStride());
+//
+//    WOLF_TRACE("Map without Stride ------------");
+//    Map<const Quaternions>  q1 (&q(0) + 0);
+//    Map<const Vector3s>     v1 (&v(0) + 0);
+//    WOLF_TRACE("q1\n", q1.coeffs());
+//    WOLF_TRACE("v1\n", v1);
+//
+//    WOLF_TRACE("Map with Stride ---------------");
+////    Map<const Quaternions, 0, Stride<Dynamic, Dynamic> > q2(&q(0) + 0, Stride<Dynamic, Dynamic>(q.colStride(),q.rowStride()));
+////    Map<const Quaternions, 0, OuterStride<Dynamic> >    q2(&q(0) + 0, OuterStride<Dynamic>(q.rowStride()));
+//    Map<const VectorXs, 0, Stride<Dynamic, Dynamic> >   v2(&v(0) + 0, 3, 1, Stride<Dynamic, Dynamic>(v.colStride(),v.rowStride()));
+////    WOLF_TRACE("m2\n", q2);
+//    WOLF_TRACE("n2\n", v2);
+//
+////    vr = q2 * v2;
+//
+//    WOLF_TRACE("Result ------------------------");
+//    WOLF_TRACE("sum\n", vr);
+//}
+//
+//TEST(IMU_tools, Stride_quat)
+//{
+//    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);
+//
+//    rot(M.col(1), N.col(1).head(3), 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.Stride";
+//  ::testing::GTEST_FLAG(filter) = "IMU_tools.Stride*";
   return RUN_ALL_TESTS();
 }