diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp index c1b438d68d3471f5439730ab50c015cd0260f8fa..ea95ccc27b9242c4c198ab592db346a813b45329 100644 --- a/test/gtest_IMU_tools.cpp +++ b/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(); }