Feature: add NaN set and URT of matrix

.travis.yml

@@ -17,7 +17,7 @@ matrix:
       compiler: clang
       env: CMAKE_BUILD_TYPE=Release CC=clang CXX=clang++ FORMAT=OFF
     - os: osx
-      osx_image: xcode8
+      osx_image: xcode10.1
       env: CMAKE_BUILD_TYPE=Release FORMAT=OFF
 
 addons:
@@ -39,4 +39,3 @@ script:
 after_success:
   - if [ "${CMAKE_BUILD_TYPE}" = "Coverage" ]; then cpp-coveralls -i matrix; fi
   - if [ "${CMAKE_BUILD_TYPE}" = "Coverage" ]; then bash <(curl -s https://codecov.io/bash) -F matrix_tests; fi
-

matrix/Matrix.hpp

@@ -412,6 +412,11 @@ class Matrix
         setAll(1);
     }
 
+    inline void setNaN()
+    {
+        setAll(NAN);
+    }
+
     void setIdentity()
     {
         setZero();
@@ -512,6 +517,13 @@ Matrix<Type, M, N> ones() {
     return m;
 }
 
+template<size_t M, size_t N>
+Matrix<float, M, N> nans() {
+    Matrix<float, M, N> m;
+    m.setNaN();
+    return m;
+}
+
 template<typename Type, size_t  M, size_t N>
 Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
 {

matrix/SquareMatrix.hpp

@@ -48,7 +48,6 @@ class SquareMatrix : public Matrix<Type, M, M>
         }
     }
 
-
     // inverse alias
     inline bool I(SquareMatrix<Type, M> &i) const
     {
@@ -67,6 +66,23 @@ class SquareMatrix : public Matrix<Type, M, M>
         return res;
     }
 
+    // get matrix upper right triangle in a row-major vector format
+    Vector<Type, M * (M + 1) / 2> upper_right_triangle() const
+    {
+        Vector<Type, M * (M + 1) / 2> res;
+        const SquareMatrix<Type, M> &self = *this;
+
+        unsigned idx = 0;
+        for (size_t x = 0; x < M; x++) {
+            for (size_t y = x; y < M; y++) {
+                res(idx) = self(x, y);
+                ++idx;
+            }
+        }
+
+        return res;
+    }
+
     Type trace() const
     {
         Type res = 0;
@@ -133,7 +149,7 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
     for (size_t n = 0; n < M; n++) {
 
         // if diagonal is zero, swap with row below
-        if (fabs(static_cast<float>(U(n, n))) < 1e-8f) {
+        if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
             //printf("trying pivot for row %d\n",n);
             for (size_t i = n + 1; i < M; i++) {
 
@@ -158,7 +174,7 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
 #endif
 
         // failsafe, return zero matrix
-        if (fabs(static_cast<float>(U(n, n))) < 1e-8f) {
+        if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
             return false;
         }
 

matrix/stdlib_imports.hpp

@@ -15,6 +15,10 @@
 
 namespace matrix {
 
+#if !defined(FLT_EPSILON)
+#define FLT_EPSILON     __FLT_EPSILON__
+#endif
+
 #if defined(__PX4_NUTTX)
 /*
  * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually.

test/CMakeLists.txt

@@ -18,6 +18,7 @@ set(tests
     hatvee
     copyto
     least_squares
+    upperRightTriangle
     )
 
 add_custom_target(test_build)

test/helper.cpp

@@ -5,15 +5,15 @@ using namespace matrix;
 
 int main()
 {
-    TEST(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < 1e-5);
-    TEST(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < 1e-5);
-    TEST(fabs(wrap_pi(3.0) - (3.0)) < 1e-3);
+    TEST(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(3.0) - (3.0)) < FLT_EPSILON);
     TEST(!is_finite(wrap_pi(1000.0f)));
     TEST(!is_finite(wrap_pi(-1000.0f)));
     wrap_pi(NAN);
 
-    TEST(fabs(wrap_2pi(-4.0) - (-4.0 + 2*M_PI)) < 1e-5);
-    TEST(fabs(wrap_2pi(3.0) - (3.0)) < 1e-3);
+    TEST(fabs(wrap_2pi(-4.0) - (-4.0 + 2*M_PI)) < FLT_EPSILON);
+    TEST(fabs(wrap_2pi(3.0) - (3.0)) < FLT_EPSILON);
     TEST(!is_finite(wrap_2pi(1000.0f)));
     TEST(!is_finite(wrap_2pi(-1000.0f)));
     wrap_2pi(NAN);

test/inverse.cpp

@@ -20,7 +20,7 @@ int main()
     SquareMatrix<float, 3> A(data);
     SquareMatrix<float, 3> A_I = inv(A);
     SquareMatrix<float, 3> A_I_check(data_check);
-    TEST((A_I - A_I_check).abs().max() < 1e-5);
+    TEST((A_I - A_I_check).abs().max() < 1e-6f);
 
     // stess test
     SquareMatrix<float, n_large> A_large;
@@ -63,7 +63,7 @@ int main()
     SquareMatrix<float, 9> A2(data2);
     SquareMatrix<float, 9> A2_I = inv(A2);
     SquareMatrix<float, 9> A2_I_check(data2_check);
-    TEST((A2_I - A2_I_check).abs().max() < 1e-3);
+    TEST((A2_I - A2_I_check).abs().max() < 1e-3f);
     float data3[9] = {
         0, 1, 2,
         3, 4, 5,

test/matrixAssignment.cpp

@@ -22,7 +22,13 @@ int main()
     Matrix3f m2(data);
 
     for(int i=0; i<9; i++) {
-        TEST(fabs(data[i] - m2.data()[i]) < 1e-6f);
+        TEST(fabs(data[i] - m2.data()[i]) < FLT_EPSILON);
+    }
+
+    Matrix3f m_nan;
+    m_nan.setNaN();
+    for(int i=0; i<9; i++) {
+        TEST(isnan(m_nan.data()[i]));
     }
 
     float data2d[3][3] = {
@@ -32,7 +38,7 @@ int main()
     };
     m2 = Matrix3f(data2d);
     for(int i=0; i<9; i++) {
-        TEST(fabs(data[i] - m2.data()[i]) < 1e-6f);
+        TEST(fabs(data[i] - m2.data()[i]) < FLT_EPSILON);
     }
 
     float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
@@ -94,16 +100,16 @@ int main()
     m4.swapCols(2, 2);
     TEST(isEqual(m4, Matrix3f(data)));
 
-    TEST(fabs(m4.min() - 1) < 1e-5);
-    TEST(fabs((-m4).min() + 9) < 1e-5);
+    TEST(fabs(m4.min() - 1) < FLT_EPSILON);
+    TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
 
     Scalar<float> s = 1;
     const Vector<float, 1> & s_vect = s;
-    TEST(fabs(s - 1) < 1e-5);
-    TEST(fabs(s_vect(0) - 1.0f) < 1e-5);
+    TEST(fabs(s - 1) < FLT_EPSILON);
+    TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
 
     Matrix<float, 1, 1> m5 = s;
-    TEST(fabs(m5(0,0) - s) < 1e-5);
+    TEST(fabs(m5(0,0) - s) < FLT_EPSILON);
 
     Matrix<float, 2, 2> m6;
     m6.setRow(0, Vector2f(1, 2));

test/setIdentity.cpp

@@ -11,10 +11,10 @@ int main()
     for (size_t i = 0; i < 3; i++) {
         for (size_t j = 0; j < 3; j++) {
             if (i == j) {
-                TEST(fabs(A(i, j) -  1) < 1e-7);
+                TEST(fabs(A(i, j) -  1) < FLT_EPSILON);
 
             } else {
-                TEST(fabs(A(i, j) -  0) < 1e-7);
+                TEST(fabs(A(i, j) -  0) < FLT_EPSILON);
             }
         }
     }
@@ -25,10 +25,10 @@ int main()
     for (size_t i = 0; i < 3; i++) {
         for (size_t j = 0; j < 3; j++) {
             if (i == j) {
-                TEST(fabs(B(i, j) -  1) < 1e-7);
+                TEST(fabs(B(i, j) -  1) < FLT_EPSILON);
 
             } else {
-                TEST(fabs(B(i, j) -  0) < 1e-7);
+                TEST(fabs(B(i, j) -  0) < FLT_EPSILON);
             }
         }
     }

test/squareMatrix.cpp

@@ -14,7 +14,7 @@ int main()
     Vector3<float> diag_check(1, 5, 10);
 
     TEST(isEqual(A.diag(), diag_check));
-    TEST(A.trace() - 16 < 1e-3);
+    TEST(A.trace() - 16 < FLT_EPSILON);
 
     float data_check[9] = {
         1.01158503f,  0.02190432f,  0.03238144f,
@@ -25,7 +25,7 @@ int main()
     float dt = 0.01f;
     SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A*dt), 5);
     SquareMatrix<float, 3> eA_check(data_check);
-    TEST((eA - eA_check).abs().max() < 1e-3);
+    TEST((eA - eA_check).abs().max() < 1e-3f);
     return 0;
 }
 

test/test_data.py

@@ -75,18 +75,18 @@ def quat_to_dcm(q):
 print('euler', phi, theta, psi)
 
 q = euler_to_quat(phi, theta, psi)
-assert(abs(norm(q) - 1) < 1e-10)
-assert(abs(norm(q) - 1) < 1e-10)
-assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < 1e-10)
+assert(abs(norm(q) - 1) < FLT_EPSILON)
+assert(abs(norm(q) - 1) < FLT_EPSILON)
+assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON)
 print('\nq:')
 pprint(q)
 
 dcm = euler_to_dcm(phi, theta, psi)
 assert(norm(dcm[:,0]) == 1)
 assert(norm(dcm[:,1]) == 1)
 assert(norm(dcm[:,2]) == 1)
-assert(abs(dcm[:,0].dot(dcm[:,1])) < 1e-10)
-assert(abs(dcm[:,0].dot(dcm[:,2])) < 1e-10)
+assert(abs(dcm[:,0].dot(dcm[:,1])) < FLT_EPSILON)
+assert(abs(dcm[:,0].dot(dcm[:,2])) < FLT_EPSILON)
 print('\ndcm:')
 pprint(dcm)
 

test/upperRightTriangle.cpp

@@ -0,0 +1,23 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    float data[9] = {1, 2, 3,
+                     4, 5, 6,
+                     7, 8, 10
+                    };
+    float urt[6] = {1, 2, 3, 5, 6, 10};
+
+    SquareMatrix<float, 3> A(data);
+
+    for(int i=0; i<6; i++) {
+        TEST(fabs(urt[i] - A.upper_right_triangle().data()[i]) < FLT_EPSILON);
+    }
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/vector2.cpp

@@ -10,29 +10,29 @@ int main()
 {
     Vector2f a(1, 0);
     Vector2f b(0, 1);
-    TEST(fabs(a % b - 1.0f) < 1e-5);
+    TEST(fabs(a % b - 1.0f) < FLT_EPSILON);
 
     Vector2f c;
-    TEST(fabs(c(0) - 0) < 1e-5);
-    TEST(fabs(c(1) - 0) < 1e-5);
+    TEST(fabs(c(0) - 0) < FLT_EPSILON);
+    TEST(fabs(c(1) - 0) < FLT_EPSILON);
 
     Matrix<float, 2, 1> d(a);
-    TEST(fabs(d(0,0) - 1) < 1e-5);
-    TEST(fabs(d(1,0) - 0) < 1e-5);
+    TEST(fabs(d(0,0) - 1) < FLT_EPSILON);
+    TEST(fabs(d(1,0) - 0) < FLT_EPSILON);
 
     Vector2f e(d);
-    TEST(fabs(e(0) - 1) < 1e-5);
-    TEST(fabs(e(1) - 0) < 1e-5);
+    TEST(fabs(e(0) - 1) < FLT_EPSILON);
+    TEST(fabs(e(1) - 0) < FLT_EPSILON);
 
     float data[] = {4,5};
     Vector2f f(data);
-    TEST(fabs(f(0) - 4) < 1e-5);
-    TEST(fabs(f(1) - 5) < 1e-5);
+    TEST(fabs(f(0) - 4) < FLT_EPSILON);
+    TEST(fabs(f(1) - 5) < FLT_EPSILON);
 
     Vector3f g(1.23f, 423.4f, 3221.f);
     Vector2f h(g);
-    TEST(fabs(h(0) - 1.23f) < 1e-5);
-    TEST(fabs(h(1) - 423.4f) < 1e-5);
+    TEST(fabs(h(0) - 1.23f) < FLT_EPSILON);
+    TEST(fabs(h(1) - 423.4f) < FLT_EPSILON);
 
     return 0;
 }