Update gtest_IMU_tools.cpp

test/gtest_IMU_tools.cpp

@@ -671,25 +671,34 @@ TEST(IMU_tools, Stride_sum)
 
 }
 
-//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);
-//
+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("Construct without Stride ------------");
+    Vector4s     qv = q;
+    Quaternions  q0 (&qv(0));
+    Quaternions  q01 (qv);
+    Quaternions  q02; q02.coeffs() = q;
+    WOLF_TRACE("q0\n", q0.coeffs());
+    WOLF_TRACE("q01\n", q01.coeffs());
+    WOLF_TRACE("q02\n", q02.coeffs());
+
+    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()));
@@ -701,24 +710,24 @@ TEST(IMU_tools, Stride_sum)
 //
 //    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);
-//
-//}
+}
+
+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)
 {