From 63aea23f9ed8dc5a160803804a429efda95b83b6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 3 Feb 2017 17:54:16 -0500 Subject: [PATCH] Add cholesky decomp, Closes #30, and dynamic print buf --- CMakeLists.txt | 6 ++++ matrix/AxisAngle.hpp | 2 +- matrix/Dcm.hpp | 2 +- matrix/Matrix.hpp | 20 ++++++----- matrix/Quaternion.hpp | 72 ++++++++++++++++++++++--------------- matrix/Scalar.hpp | 5 --- matrix/SquareMatrix.hpp | 52 +++++++++++++++++++++++++++ matrix/helper_functions.hpp | 10 +++--- test/attitude.cpp | 30 ++++++++++------ test/inverse.cpp | 20 +++++++++++ test/matrixAssignment.cpp | 2 -- test/setIdentity.cpp | 8 ++--- 12 files changed, 165 insertions(+), 64 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c0284b1e7..ab4e2a0a3f 100644 --- a/CMakeLists.txt +++ b/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}") diff --git a/matrix/AxisAngle.hpp b/matrix/AxisAngle.hpp index b3ad7305e3..ab945379d6 100644 --- a/matrix/AxisAngle.hpp +++ b/matrix/AxisAngle.hpp @@ -76,7 +76,7 @@ public: Vector() { 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; diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index 1b4bf03fda..ba61d66a54 100644 --- a/matrix/Dcm.hpp +++ b/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()); } diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index 707c39b9d6..9082d6b6a2 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -282,7 +282,7 @@ public: const Matrix &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 transpose() const @@ -499,11 +501,13 @@ bool isEqual(const Matrix &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; } diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 08d18d55bc..a06165fb72 100644 --- a/matrix/Quaternion.hpp +++ b/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() { 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 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 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 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; diff --git a/matrix/Scalar.hpp b/matrix/Scalar.hpp index df82674f74..86025dfc0f 100644 --- a/matrix/Scalar.hpp +++ b/matrix/Scalar.hpp @@ -44,11 +44,6 @@ public: return _value; } - operator Type const &() const - { - return _value; - } - operator Matrix() const { Matrix m; m(0, 0) = _value; diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp index 912a27a31a..0457a03b1f 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -265,6 +265,58 @@ SquareMatrix inv(const SquareMatrix & A) return i; } } + +/** + * cholesky decomposition + * + * Note: A must be positive definite + */ +template +SquareMatrix cholesky(const SquareMatrix & A) +{ + SquareMatrix 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 +SquareMatrix choleskyInv(const SquareMatrix & A) +{ + SquareMatrix L_inv = inv(cholesky(A)); + return L_inv.T()*L_inv; +} + typedef SquareMatrix Matrix3f; } // namespace matrix diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp index bfd5b35f11..7e0fd4bff4 100644 --- a/matrix/helper_functions.hpp +++ b/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) } -}; +} diff --git a/test/attitude.cpp b/test/attitude.cpp index c2c6089d5c..bb5baa42d8 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -99,12 +99,12 @@ int main() // dcm renormalize Dcmf A = eye(); 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 q1_dot = q1.derivative(Vector3f(1, 2, 3)); - float data_q_dot_check[] = { -0.5f, 0.0f, -1.5f, 1.0f}; - Vector q1_dot_check(data_q_dot_check); - TEST(isEqual(q1_dot, q1_dot_check)); + Vector q1_dot1 = q1.derivative1(Vector3f(1, 2, 3)); + float data_q_dot1_check[] = { -0.5f, 0.0f, 1.5f, -1.0f}; + Vector q1_dot1_check(data_q_dot1_check); + TEST(isEqual(q1_dot1, q1_dot1_check)); + + // quaternion derivative in frame 2 + Vector q1_dot2 = q1.derivative2(Vector3f(1, 2, 3)); + float data_q_dot2_check[] = { -0.5f, 0.0f, -1.5f, 1.0f}; + Vector 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 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}; Quaternionq_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 : */ diff --git a/test/inverse.cpp b/test/inverse.cpp index 3c157388c0..98f0c92d8f 100644 --- a/test/inverse.cpp +++ b/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 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 A4_cholesky_check(data4_cholesky); + SquareMatrix A4_cholesky = cholesky(A4); + TEST(isEqual(A4_cholesky_check, A4_cholesky)); + + SquareMatrix I3; + I3.setIdentity(); + TEST(isEqual(choleskyInv(A4)*A4, I3)); + TEST(isEqual(cholesky(Z3), Z3)); return 0; } diff --git a/test/matrixAssignment.cpp b/test/matrixAssignment.cpp index 69124d9235..1cba50f097 100644 --- a/test/matrixAssignment.cpp +++ b/test/matrixAssignment.cpp @@ -101,10 +101,8 @@ int main() Scalar s; s = 1; - float const & s_float = (const Scalar)s; const Vector & 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 m5 = s; diff --git a/test/setIdentity.cpp b/test/setIdentity.cpp index e17614dafd..f266d62732 100644 --- a/test/setIdentity.cpp +++ b/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);