Add cholesky decomp, Closes #30, and dynamic print buf

CMakeLists.txt

@@ -54,6 +54,12 @@ set(CMAKE_CXX_FLAGS
 	-Wconversion
 	-Wcast-align
 	-Werror
+	-pedantic -Wctor-dtor-privacy -Wdisabled-optimization -Wformat=2
+	-Winit-self -Wlogical-op -Wmissing-declarations
+	-Wmissing-include-dirs -Wnoexcept -Wold-style-cast
+	-Woverloaded-virtual -Wredundant-decls -Wshadow -Wsign-conversion
+	-Wsign-promo -Wstrict-null-sentinel -Wstrict-overflow=5
+	-Wswitch-default -Wundef -Werror -Wno-unused
 	)
 string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
 

matrix/AxisAngle.hpp

@@ -76,7 +76,7 @@ public:
         Vector<Type, 3>()
     {
         AxisAngle &v = *this;
-        Type ang = (Type)2.0f*acosf(q(0));
+        Type ang = Type(2.0f)*acosf(q(0));
         Type mag = sinf(ang/2.0f);
         if (fabsf(mag) > 0) {
             v(0) = ang*q(1)/mag;

matrix/Dcm.hpp

@@ -164,7 +164,7 @@ public:
     void renormalize()
     {
         /* renormalize rows */
-        for (int row = 0; row < 3; row++) {
+        for (size_t row = 0; row < 3; row++) {
             matrix::Vector3f rvec(this->_data[row]);
             this->setRow(row, rvec.normalized());
         }

matrix/Matrix.hpp

@@ -282,7 +282,7 @@ public:
         const Matrix<Type, M, N> &self = *this;
         for (size_t i = 0; i < M; i++) {
             for (size_t j = 0; j < N; j++) {
-                snprintf(buf + strlen(buf), n - strlen(buf), "\t%g", double(self(i, j))); // directly append to the string buffer
+                snprintf(buf + strlen(buf), n - strlen(buf), "\t%8.2g", double(self(i, j))); // directly append to the string buffer
             }
             snprintf(buf + strlen(buf), n - strlen(buf), "\n");
         }
@@ -290,9 +290,11 @@ public:
 
     void print() const
     {
-        char buf[200];
-        write_string(buf, 200);
+        static const size_t n = 10*N*M;
+        char * buf = new char[n];
+        write_string(buf, n);
         printf("%s\n", buf);
+        delete[] buf;
     }
 
     Matrix<Type, N, M> transpose() const
@@ -499,11 +501,13 @@ bool isEqual(const Matrix<Type, M, N> &x,
 
 
     if (!equal) {
-        char buf_x[100];
-        char buf_y[100];
-        x.write_string(buf_x, 100);
-        y.write_string(buf_y, 100);
-        printf("not equal\nx:\n%s\ny:\n%s\n", buf_x, buf_y);
+        static const size_t n = 10*N*M;
+        char * buf = new char[n];
+        x.write_string(buf, n);
+        printf("not equal\nx:\n%s\n", buf);
+        y.write_string(buf, n);
+        printf("y:\n%s\n", buf);
+        delete[] buf;
     }
     return equal;
 }

matrix/Quaternion.hpp

@@ -2,6 +2,7 @@
  * @file Quaternion.hpp
  *
  * All rotations and axis systems follow the right-hand rule.
+ * The Hamilton quaternion product definition is used.
  *
  * In order to rotate a vector v by a righthand rotation defined by the quaternion q
  * one can use the following operation:
@@ -139,12 +140,12 @@ public:
         Vector<Type, 4>()
     {
         Quaternion &q = *this;
-        Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0));
-        Type cosTheta_2 = Type(cos(euler.theta() / (Type)2.0));
-        Type cosPsi_2 = Type(cos(euler.psi() / (Type)2.0));
-        Type sinPhi_2 = Type(sin(euler.phi() / (Type)2.0));
-        Type sinTheta_2 = Type(sin(euler.theta() / (Type)2.0));
-        Type sinPsi_2 = Type(sin(euler.psi() / (Type)2.0));
+        Type cosPhi_2 = Type(cos(euler.phi() / Type(2.0)));
+        Type cosTheta_2 = Type(cos(euler.theta() / Type(2.0)));
+        Type cosPsi_2 = Type(cos(euler.psi() / Type(2.0)));
+        Type sinPhi_2 = Type(sin(euler.phi() / Type(2.0)));
+        Type sinTheta_2 = Type(sin(euler.theta() / Type(2.0)));
+        Type sinPsi_2 = Type(sin(euler.psi() / Type(2.0)));
         q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
                sinPhi_2 * sinTheta_2 * sinPsi_2;
         q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -166,8 +167,8 @@ public:
         Quaternion &q = *this;
         Type angle = aa.norm();
         Vector<Type, 3> axis = aa.unit();
-        if (angle < (Type)1e-10) {
-            q(0) = (Type)1.0;
+        if (angle < Type(1e-10)) {
+            q(0) = Type(1.0);
             q(1) = q(2) = q(3) = 0;
         } else {
             Type magnitude = sinf(angle / 2.0f);
@@ -253,11 +254,29 @@ public:
     }
 
     /**
-     * Computes the derivative
+     * Computes the derivative of q_12 when
+     * rotated with angular velocity expressed in frame 2
+     * v_2 = q_12 * v_1 * q_12^-1
+     * d/dt q_12 = 0.5 * q_12 * omega_12_2
      *
-     * @param w direction
+     * @param w angular rate in frame 2
      */
-    Matrix41 derivative(const Matrix31 &w) const
+    Matrix41 derivative1(const Matrix31 &w) const
+    {
+        const Quaternion &q = *this;
+        Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
+        return q * v  * Type(0.5);
+    }
+
+    /**
+     * Computes the derivative of q_12 when
+     * rotated with angular velocity expressed in frame 2
+     * v_2 = q_12 * v_1 * q_12^-1
+     * d/dt q_12 = 0.5 * omega_12_1 * q_12
+     *
+     * @param w angular rate in frame (typically reference frame)
+     */
+    Matrix41 derivative2(const Matrix31 &w) const
     {
         const Quaternion &q = *this;
         Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
@@ -265,14 +284,11 @@ public:
     }
 
     /**
-     * Invert quaternion
+     * Invert quaternion in place
      */
     void invert()
     {
-        Quaternion &q = *this;
-        q(1) *= -1;
-        q(2) *= -1;
-        q(3) *= -1;
+        *this = this->inversed();
     }
 
     /**
@@ -283,12 +299,12 @@ public:
     Quaternion inversed()
     {
         Quaternion &q = *this;
-        Quaternion ret;
-        ret(0) = q(0);
-        ret(1) = -q(1);
-        ret(2) = -q(2);
-        ret(3) = -q(3);
-        return ret;
+        Type normSq = q.dot(q);
+        return Quaternion(
+                   q(0)/normSq,
+                   -q(1)/normSq,
+                   -q(2)/normSq,
+                   -q(3)/normSq);
     }
 
     /**
@@ -332,8 +348,8 @@ public:
         Quaternion &q = *this;
         Type theta = vec.norm();
 
-        if (theta < (Type)1e-10) {
-            q(0) = (Type)1.0;
+        if (theta < Type(1e-10)) {
+            q(0) = Type(1.0);
             q(1) = q(2) = q(3) = 0;
             return;
         }
@@ -354,8 +370,8 @@ public:
     {
         Quaternion &q = *this;
 
-        if (theta < (Type)1e-10) {
-            q(0) = (Type)1.0;
+        if (theta < Type(1e-10)) {
+            q(0) = Type(1.0);
             q(1) = q(2) = q(3) = 0;
         }
 
@@ -386,9 +402,9 @@ public:
         vec(1) = q(2);
         vec(2) = q(3);
 
-        if (axis_magnitude >= (Type)1e-10) {
+        if (axis_magnitude >= Type(1e-10)) {
             vec = vec / axis_magnitude;
-            vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude, q(0)));
+            vec = vec * wrap_pi(Type(2.0) * atan2f(axis_magnitude, q(0)));
         }
 
         return vec;

matrix/Scalar.hpp

@@ -44,11 +44,6 @@ public:
         return _value;
     }
 
-    operator Type const &() const
-    {
-        return _value;
-    }
-
     operator Matrix<Type, 1, 1>() const {
         Matrix<Type, 1, 1> m;
         m(0, 0) = _value;

matrix/SquareMatrix.hpp

@@ -265,6 +265,58 @@ SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> & A)
         return i;
     }
 }
+
+/**
+ * cholesky decomposition
+ *
+ * Note: A must be positive definite
+ */
+template<typename Type, size_t M>
+SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> & A)
+{
+    SquareMatrix<Type, M> L;
+    for (size_t j = 0; j < M; j++) {
+        for (size_t i = j; i < M; i++) {
+            if (i==j) {
+                float sum = 0;
+                for (size_t k = 0; k < j; k++) {
+                    sum += L(j, k)*L(j, k);
+                }
+                Type res = A(j, j) - sum;
+                if (res <= 0) {
+                    L(j, j) = 0;
+                } else {
+                    L(j, j) = sqrtf(res);
+                }
+            } else {
+                float sum = 0;
+                for (size_t k = 0; k < j; k++) {
+                    sum += L(i, k)*L(j, k);
+                }
+                if (L(j, j) <= 0) {
+                    L(i, j) = 0;
+                } else {
+                    L(i, j) = (A(i, j) - sum)/L(j, j);
+                }
+            }
+        }
+    }
+    return L;
+}
+
+/**
+ * cholesky inverse
+ *
+ * TODO: Check if gaussian elimination jumps straight to back-substitution
+ * for L or we need to do it manually. Will impact speed otherwise.
+ */
+template<typename Type, size_t M>
+SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> & A)
+{
+    SquareMatrix<Type, M> L_inv = inv(cholesky(A));
+    return L_inv.T()*L_inv;
+}
+
 typedef SquareMatrix<float, 3> Matrix3f;
 
 } // namespace matrix

matrix/helper_functions.hpp

@@ -29,13 +29,13 @@ Type wrap_pi(Type x)
         return x;
     }
 
-    while (x >= (Type)M_PI) {
-        x -= (Type)(2.0 * M_PI);
+    while (x >= Type(M_PI)) {
+        x -= Type(2.0 * M_PI);
 
     }
 
-    while (x < (Type)(-M_PI)) {
-        x += (Type)(2.0 * M_PI);
+    while (x < Type(-M_PI)) {
+        x += Type(2.0 * M_PI);
 
     }
 
@@ -43,4 +43,4 @@ Type wrap_pi(Type x)
 }
 
 
-};
+}

test/attitude.cpp

@@ -99,12 +99,12 @@ int main()
     // dcm renormalize
     Dcmf A = eye<float, 3>();
     Dcmf R(euler_check);
-    for (int i = 0; i < 1000; i++) {
+    for (size_t i = 0; i < 1000; i++) {
         A = R * A;
     }
     A.renormalize();
     float err = 0.0f;
-    for (int row = 0; row < 3; row++) {
+    for (size_t row = 0; row < 3; row++) {
         matrix::Vector3f rvec(A._data[row]);
         err += fabsf(1.0f - rvec.length());
     }
@@ -179,12 +179,18 @@ int main()
     Quatf q_from_m(m4);
     TEST(isEqual(q_from_m, m4));
 
-    // quaternion derivate
+    // quaternion derivative in frame 1
     Quatf q1(0, 1, 0, 0);
-    Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
-    float data_q_dot_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
-    Vector<float, 4> q1_dot_check(data_q_dot_check);
-    TEST(isEqual(q1_dot, q1_dot_check));
+    Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
+    float data_q_dot1_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
+    Vector<float, 4> q1_dot1_check(data_q_dot1_check);
+    TEST(isEqual(q1_dot1, q1_dot1_check));
+
+    // quaternion derivative in frame 2
+    Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
+    float data_q_dot2_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
+    Vector<float, 4> q1_dot2_check(data_q_dot2_check);
+    TEST(isEqual(q1_dot2, q1_dot2_check));
 
     // quaternion product
     Quatf q_prod_check(
@@ -220,8 +226,12 @@ int main()
     TEST(fabsf(q_check(2) + q(2)) < eps);
     TEST(fabsf(q_check(3) + q(3)) < eps);
 
-    // rotate quaternion (nonzero rotation)
+    // non-unit quaternion invese
     Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
+    Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
+    TEST(isEqual(qI, q_nonunit*q_nonunit.inversed()));
+
+    // rotate quaternion (nonzero rotation)
     Vector<float, 3> rot;
     rot(0) = 1.0f;
     rot(1) = rot(2) = 0.0f;
@@ -270,7 +280,7 @@ int main()
     float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
     Quaternion<float>q_from_array(q_array);
 
-    for (int i = 0; i < 4; i++) {
+    for (size_t i = 0; i < 4; i++) {
         TEST(fabsf(q_from_array(i) - q_array[i]) < eps);
     }
 
@@ -333,6 +343,6 @@ int main()
     q = Quatf(0,0,0,1); // 180 degree rotation around the z axis
     R = Dcmf(q);
     TEST(isEqual(q, Quatf(R)));
-};
+}
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/inverse.cpp

@@ -106,6 +106,26 @@ int main()
     TEST(isEqual(A3_I, Z3));
     TEST(isEqual(A3.I(), Z3));
 
+    float data4[9] = {
+        1.33471626f,  0.74946721f, -0.0531679f,
+        0.74946721f,  1.07519593f,  0.08036323f,
+        -0.0531679f,  0.08036323f,  1.01618474f
+    };
+    SquareMatrix<float, 3> A4(data4);
+
+    float data4_cholesky[9] = {
+        1.15529921f,  0.f,  0.f,
+        0.6487213f ,  0.80892311f,  0.f,
+        -0.04602089f,  0.13625271f,  0.99774847f
+    };
+    SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
+    SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
+    TEST(isEqual(A4_cholesky_check, A4_cholesky));
+
+    SquareMatrix<float, 3> I3;
+    I3.setIdentity();
+    TEST(isEqual(choleskyInv(A4)*A4, I3));
+    TEST(isEqual(cholesky(Z3), Z3));
     return 0;
 }
 

test/matrixAssignment.cpp

@@ -101,10 +101,8 @@ int main()
 
     Scalar<float> s;
     s = 1;
-    float const & s_float = (const Scalar<float>)s;
     const Vector<float, 1> & s_vect = s;
     TEST(fabs(s - 1) < 1e-5);
-    TEST(fabs(s_float - 1.0f) < 1e-5);
     TEST(fabs(s_vect(0) - 1.0f) < 1e-5);
 
     Matrix<float, 1, 1> m5 = s;

test/setIdentity.cpp

@@ -10,8 +10,8 @@ int main()
     Matrix3f A;
     A.setIdentity();
 
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
+    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);
 
@@ -24,8 +24,8 @@ int main()
     Matrix3f B;
     B.identity();
 
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
+    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);