fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 09:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Got coverage working for templates.

Browse Source
This commit is contained in:
jgoppert
2015-11-04 12:11:32 -05:00
parent bf6de4b710
commit 18f80462b7
13 changed files with 243 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -49,7 +49,7 @@ enable_testing()
include_directories(matrix)
file(GLOB_RECURSE COV_SRCS matrix/*.hpp matrix/*.cpp test/*.hpp test/*.cpp)
file(GLOB_RECURSE COV_SRCS matrix/*.hpp matrix/*.cpp)
# Setup the coveralls target and tell it to gather
# coverage data for all the lib sources.
matrix/Matrix.hpp
+2 -162
View File
@@ -151,7 +151,8 @@ public:
self = self - other;
}
void operator*=(const Matrix<Type, M, N> &other)
template<size_t P>
void operator*=(const Matrix<Type, N, P> &other)
{
Matrix<Type, M, N> &self = *this;
self = self * other;
@@ -247,42 +248,6 @@ public:
return transpose();
}
Matrix<Type, M, M> expm(float dt, size_t n) const
{
Matrix<float, M, M> res;
res.setIdentity();
Matrix<float, M, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) {
break;
}
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
Vector<Type, M> diagonal() const
{
Vector<Type, M> res;
// force square for now
const Matrix<Type, M, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
void setZero()
{
memset(_data, 0, sizeof(_data));
@@ -328,133 +293,8 @@ public:
}
}
/**
* inverse based on LU factorization with partial pivotting
*/
Matrix <Type, M, M> inverse() const
{
Matrix<Type, M, M> L;
L.setIdentity();
const Matrix<Type, M, M> &A = (*this);
Matrix<Type, M, M> U = A;
Matrix<Type, M, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < N; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) {
if (i == n) {
continue;
}
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
Matrix<Type, M, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
Matrix<Type, M, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < N; c++) {
// for all rows of L
for (size_t i = 0; i < N; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
Matrix<Type, M, M> X = Y;
// for all columns of X
for (size_t c = 0; c < N; c++) {
// for all rows of U
for (size_t k = 0; k < N; k++) {
// have to go in reverse order
size_t i = N - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < N; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
// inverse alias
inline Matrix<Type, N, M> I() const
{
return inverse();
}
};
typedef Matrix<float, 3, 3> Matrix3f;
}; // namespace matrix
matrix/SquareMatrix.hpp
+207
View File
@@ -0,0 +1,207 @@
/**
* @file SquareMatrix.hpp
*
* A square matrix
*
* @author James Goppert <james.goppert@gmail.com>
*/
#pragma once
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "Matrix.hpp"
namespace matrix
{
template <typename Type, size_t M, size_t N>
class Matrix;
template<typename Type, size_t M>
class SquareMatrix : public Matrix<Type, M, M>
{
public:
SquareMatrix() :
Matrix<Type, M, M>()
{
}
SquareMatrix(const Type *data) :
Matrix<Type, M, M>(data)
{
}
SquareMatrix(const Matrix<float, M, M> &other) :
Matrix<Type, M, M>(other)
{
}
/**
* inverse based on LU factorization with partial pivotting
*/
SquareMatrix <Type, M> inverse() const
{
SquareMatrix<Type, M> L;
L.setIdentity();
const SquareMatrix<Type, M> &A = (*this);
SquareMatrix<Type, M> U = A;
SquareMatrix<Type, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < M; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < M; i++) {
if (i == n) {
continue;
}
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
SquareMatrix<Type, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < M; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < M; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
SquareMatrix<Type, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < M; c++) {
// for all rows of L
for (size_t i = 0; i < M; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
SquareMatrix<Type, M> X = Y;
// for all columns of X
for (size_t c = 0; c < M; c++) {
// for all rows of U
for (size_t k = 0; k < M; k++) {
// have to go in reverse order
size_t i = M - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < M; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
// inverse alias
inline SquareMatrix<Type, M> I() const
{
return inverse();
}
Vector<Type, M> diagonal() const
{
Vector<Type, M> res;
const SquareMatrix<Type, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
SquareMatrix<Type, M> expm(float dt, size_t n) const
{
SquareMatrix<float, M> res;
res.setIdentity();
SquareMatrix<float, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) {
break;
}
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
};
typedef SquareMatrix<float, 3> SquareMatrix3f;
}; // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
test/CMakeLists.txt
-6
View File
@@ -1,9 +1,3 @@
add_library(instantiation
instantiation.cpp
)
link_libraries(instantiation)
set(tests
setIdentity
inverse
test/instantiation.cpp
-21
View File
@@ -1,21 +0,0 @@
// dummy code to instantiate all templates for coverage
#include <Matrix.hpp>
#include <Vector.hpp>
#include <Vector3.hpp>
#include <Euler.hpp>
#include <Scalar.hpp>
#include <Quaternion.hpp>
namespace matrix {
template class Vector<float, 2>;
template class Vector<float, 3>;
template class Vector<float, 4>;
template class Euler<float>;
template class Scalar<float>;
template class Matrix<float, 3, 3>;
template class Matrix<float, 1, 1>;
template class Quaternion<float>;
};
test/inverse.cpp
+12 -8
View File
@@ -1,27 +1,31 @@
#include "Matrix.hpp"
#include "SquareMatrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
static const size_t n_large = 50;
template class SquareMatrix<float, 3>;
template class SquareMatrix<float, n_large>;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
Matrix3f A_I = A.inverse();
SquareMatrix<float, 3> A(data);
SquareMatrix<float, 3> A_I = A.inverse();
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I_check(data_check);
SquareMatrix<float, 3> A_I_check(data_check);
(void)A_I;
assert(A_I == A_I_check);
// stess test
static const size_t n = 50;
Matrix<float, n, n> A_large;
SquareMatrix<float, n_large> A_large;
A_large.setIdentity();
Matrix<float, n, n> A_large_I;
SquareMatrix<float, n_large> A_large_I;
A_large_I.setZero();
for (size_t i = 0; i < 50; i++) {
for (size_t i = 0; i < n_large; i++) {
A_large_I = A_large.inverse();
assert(A_large == A_large_I);
}
test/matrixAssignment.cpp
+2
View File
@@ -3,6 +3,8 @@
using namespace matrix;
template class Matrix<float, 3, 3>;
int main()
{
Matrix3f m;
test/quaternion.cpp
+6 -1
View File
@@ -4,6 +4,11 @@
using namespace matrix;
// instantiate so coverage works
template class Quaternion<float>;
template class Euler<float>;
template class Dcm<float>;
int main()
{
// test default ctor
@@ -12,7 +17,7 @@ int main()
assert(q(1) == 0);
assert(q(2) == 0);
assert(q(3) == 0);
q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
assert(q(0) == 0.1825742f);
assert(q(1) == 0.3651484f);
test/setIdentity.cpp
+2
View File
@@ -3,6 +3,8 @@
using namespace matrix;
template class Matrix<float, 3, 3>;
int main()
{
Matrix3f A;
test/transpose.cpp
+5 -2
View File
@@ -4,12 +4,15 @@
using namespace matrix;
template class Matrix<float, 2, 3>;
template class Matrix<float, 3, 2>;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6};
float data[6] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[9] = {1, 4, 2, 5, 3, 6};
float data_check[6] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
A_T.print();
A_T_check.print();
test/vector.cpp
+2
View File
@@ -4,6 +4,8 @@
using namespace matrix;
template class Vector<float, 5>;
int main()
{
Vector<float, 5> v;
test/vector3.cpp
+2
View File
@@ -4,6 +4,8 @@
using namespace matrix;
template class Vector<float, 3>;
int main()
{
Vector3f a(1, 0, 0);
test/vectorAssignment.cpp
+2
View File
@@ -3,6 +3,8 @@
using namespace matrix;
template class Vector<float, 3>;
int main()
{
Vector3f v;