zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Math: fixup matrix algorithms to be in cpp file 

this fixes an issue where optimize O2 was forced on any file that
included AP_Math.h. It also fixes the test suite for matrix_alg, and
fixes the type handling to be consistent
c415-sdk
Andrew Tridgell 4 years ago committed by Peter Barker
parent
3b3e2c01f8
commit
999268cbba
4 changed files with 104 additions and 86 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      libraries/AP_Math/AP_Math.h
  2. 65
      libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp
  3. 7
      libraries/AP_Math/examples/matrix_alg/wscript
  4. 101
      libraries/AP_Math/matrix_alg.cpp

17
libraries/AP_Math/AP_Math.h
Unescape View File

@ -82,23 +82,18 @@ float safe_asin(const T v); @@ -82,23 +82,18 @@ float safe_asin(const T v);
template <typename T>
float safe_sqrt(const T v);
// invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular
template<typename T>
bool inverse3x3(const T m[], T invOut[]) WARN_IF_UNUSED;
// invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular
// matrix multiplication of two NxN matrices
template <typename T>
bool inverse4x4(const T m[],T invOut[]) WARN_IF_UNUSED;
void mat_mul(const T *A, const T *B, T *C, uint16_t n);
// matrix multiplication of two NxN matrices
// matrix inverse
template <typename T>
float *mat_mul(const T *A, const T *B, uint8_t n);
bool mat_inverse(const T *x, T *y, uint16_t dim) WARN_IF_UNUSED;
// matrix algebra
// matrix identity
template <typename T>
bool inverse(const T x[], T y[], uint16_t dim) WARN_IF_UNUSED;
void mat_identity(T *x, uint16_t dim);
#include "matrix_alg.h"
/*
* Constrain an angle to be within the range: -180 to 180 degrees. The second
* parameter changes the units. Default: 1 == degrees, 10 == dezi,

65
libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp
Unescape View File

@ -5,10 +5,16 @@ @@ -5,10 +5,16 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <stdio.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define MAT_ALG_ACCURACY 1e-4f
typedef float ftype;
static uint16_t get_random(void)
{
static uint32_t m_z = 1234;
@ -19,7 +25,7 @@ static uint16_t get_random(void) @@ -19,7 +25,7 @@ static uint16_t get_random(void)
}
static void show_matrix(float *A, int n) {
static void show_matrix(ftype *A, int n) {
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++)
printf("%.10f ", A[i * n + j]);
@ -27,7 +33,7 @@ static void show_matrix(float *A, int n) { @@ -27,7 +33,7 @@ static void show_matrix(float *A, int n) {
}
}
static bool compare_mat(const float *A, const float *B, const uint8_t n)
static bool compare_mat(const ftype *A, const ftype *B, const uint8_t n)
{
for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) {
@ -42,22 +48,17 @@ static bool compare_mat(const float *A, const float *B, const uint8_t n) @@ -42,22 +48,17 @@ static bool compare_mat(const float *A, const float *B, const uint8_t n)
static void test_matrix_inverse(void)
{
//fast inverses
float test_mat[25],ident_mat[25];
float *out_mat;
ftype test_mat[25],ident_mat[25];
ftype out_mat[25], out_mat2[25], mat[25];
for(uint8_t i = 0;i<25;i++) {
test_mat[i] = pow(-1,i)*get_random()/0.7f;
test_mat[i] = powf(-1,i)*get_random()/0.7f;
}
float mat[25];
//Test for 3x3 matrix
memset(ident_mat,0,sizeof(ident_mat));
for(uint8_t i=0; i<3; i++) {
ident_mat[i*3+i] = 1.0f;
}
if(inverse(test_mat,mat,3)){
out_mat = mat_mul(test_mat,mat,3);
inverse(mat,mat,3);
mat_identity(ident_mat, 3);
if (mat_inverse(test_mat,mat,3) && mat_inverse(mat, out_mat2, 3)) {
mat_mul(test_mat, mat, out_mat, 3);
} else {
hal.console->printf("3x3 Matrix is Singular!\n");
return;
@ -70,25 +71,22 @@ static void test_matrix_inverse(void) @@ -70,25 +71,22 @@ static void test_matrix_inverse(void)
printf("\nInv(A) * A\n");
show_matrix(out_mat,3);
printf("\n");
//compare matrix
if(!compare_mat(test_mat,mat,3)) {
// compare matrix
if (!compare_mat(test_mat, out_mat2, 3)) {
printf("Test Failed!!\n");
return;
}
if(!compare_mat(ident_mat,out_mat,3)) {
if (!compare_mat(ident_mat, out_mat, 3)) {
printf("Identity output Test Failed!!\n");
return;
}
//Test for 4x4 matrix
memset(ident_mat,0,sizeof(ident_mat));
for(uint8_t i=0; i<4; i++) {
ident_mat[i*4+i] = 1.0f;
}
if(inverse(test_mat,mat,4)){
out_mat = mat_mul(test_mat,mat,4);
inverse(mat,mat,4);
mat_identity(ident_mat, 4);
if (mat_inverse(test_mat, mat, 4) && mat_inverse(mat, out_mat2, 4)){
mat_mul(test_mat, mat, out_mat, 4);
} else {
hal.console->printf("4x4 Matrix is Singular!\n");
return;
@ -100,29 +98,22 @@ static void test_matrix_inverse(void) @@ -100,29 +98,22 @@ static void test_matrix_inverse(void)
printf("\nInv(A) * A\n");
show_matrix(out_mat,4);
printf("\n");
if(!compare_mat(test_mat,mat,4)) {
if (!compare_mat(test_mat, out_mat2, 4)) {
printf("Test Failed!!\n");
return;
}
if(!compare_mat(ident_mat,out_mat,4)) {
if (!compare_mat(ident_mat,out_mat,4)) {
printf("Identity output Test Failed!!\n");
return;
}
//Test for 5x5 matrix
memset(ident_mat,0,sizeof(ident_mat));
for(uint8_t i=0; i<5; i++) {
ident_mat[i*5+i] = 1.0f;
}
if(inverse(test_mat,mat,5)) {
out_mat = mat_mul(test_mat,mat,5);
inverse(mat,mat,5);
mat_identity(ident_mat, 5);
if (mat_inverse(test_mat,mat,5) && mat_inverse(mat, out_mat2, 5)) {
mat_mul(test_mat, mat, out_mat, 5);
} else {
hal.console->printf("5x5 Matrix is Singular!\n");
return;
}
printf("\n\n5x5 Test Matrix:\n");
@ -132,11 +123,11 @@ static void test_matrix_inverse(void) @@ -132,11 +123,11 @@ static void test_matrix_inverse(void)
printf("\nInv(A) * A\n");
show_matrix(out_mat,5);
printf("\n");
if(!compare_mat(test_mat,mat,5)) {
if (!compare_mat(test_mat, out_mat2, 5)) {
printf("Test Failed!!\n");
return;
}
if(!compare_mat(ident_mat,out_mat,5)) {
if (!compare_mat(ident_mat, out_mat, 5)) {
printf("Identity output Test Failed!!\n");
return;
}

7
libraries/AP_Math/examples/matrix_alg/wscript
Unescape View File

@ -0,0 +1,7 @@ @@ -0,0 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
def build(bld):
bld.ap_example(
use='ap',
)

101
libraries/AP_Math/matrix_alg.h → libraries/AP_Math/matrix_alg.cpp
Unescape View File

@ -15,20 +15,15 @@ @@ -15,20 +15,15 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma GCC optimize("O2")
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_Math.h"
#include <stdio.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <fenv.h>
#endif
// extern const AP_HAL::HAL& hal;
//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations
/*
* Does matrix multiplication of two regular/square matrices
*
@ -38,14 +33,14 @@ @@ -38,14 +33,14 @@
* @returns multiplied matrix i.e. A*B
*/
template<typename T>
T* matrix_multiply(const T *A, const T *B, uint8_t n)
static T* matrix_multiply(const T *A, const T *B, uint16_t n)
{
T* ret = new T[n*n];
memset(ret,0.0f,n*n*sizeof(T));
for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) {
for(uint8_t k = 0;k < n; k++) {
for(uint16_t i = 0; i < n; i++) {
for(uint16_t j = 0; j < n; j++) {
for(uint16_t k = 0;k < n; k++) {
ret[i*n + j] += A[i*n + k] * B[k*n + j];
}
}
@ -71,19 +66,19 @@ static inline void swap(T &a, T &b) @@ -71,19 +66,19 @@ static inline void swap(T &a, T &b)
* @returns false = matrix is Singular or non positive definite, true = matrix inversion successful
*/
template<typename T>
static void mat_pivot(const T* A, T* pivot, uint8_t n)
static void mat_pivot(const T* A, T* pivot, uint16_t n)
{
for(uint8_t i = 0;i<n;i++){
for(uint8_t j=0;j<n;j++) {
for(uint16_t i = 0;i<n;i++){
for(uint16_t j=0;j<n;j++) {
pivot[i*n+j] = static_cast<T>(i==j);
}
}
for(uint8_t i = 0;i < n; i++) {
uint8_t max_j = i;
for(uint8_t j=i;j<n;j++){
for(uint16_t i = 0;i < n; i++) {
uint16_t max_j = i;
for(uint16_t j=i;j<n;j++){
if (std::is_same<T, double>::value) {
if(fabs(A[j*n + i]) > fabs(A[max_j*n + i])) {
if(fabsf(A[j*n + i]) > fabsf(A[max_j*n + i])) {
max_j = j;
}
} else {
@ -94,7 +89,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n) @@ -94,7 +89,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n)
}
if(max_j != i) {
for(uint8_t k = 0; k < n; k++) {
for(uint16_t k = 0; k < n; k++) {
swap(pivot[i*n + k], pivot[max_j*n + k]);
}
}
@ -109,7 +104,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n) @@ -109,7 +104,7 @@ static void mat_pivot(const T* A, T* pivot, uint8_t n)
* @param n, dimension of matrix
*/
template<typename T>
static void mat_forward_sub(const T *L, T *out, uint8_t n)
static void mat_forward_sub(const T *L, T *out, uint16_t n)
{
// Forward substitution solve LY = I
for(int i = 0; i < n; i++) {
@ -131,7 +126,7 @@ static void mat_forward_sub(const T *L, T *out, uint8_t n) @@ -131,7 +126,7 @@ static void mat_forward_sub(const T *L, T *out, uint8_t n)
* @param n, dimension of matrix
*/
template<typename T>
static void mat_back_sub(const T *U, T *out, uint8_t n)
static void mat_back_sub(const T *U, T *out, uint16_t n)
{
// Backward Substitution solve UY = I
for(int i = n-1; i >= 0; i--) {
@ -154,7 +149,7 @@ static void mat_back_sub(const T *U, T *out, uint8_t n) @@ -154,7 +149,7 @@ static void mat_back_sub(const T *U, T *out, uint8_t n)
* @param n, dimension of matrix
*/
template<typename T>
static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n)
static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint16_t n)
{
memset(L,0,n*n*sizeof(T));
memset(U,0,n*n*sizeof(T));
@ -162,20 +157,20 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n) @@ -162,20 +157,20 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n)
mat_pivot(A,P,n);
T *APrime = matrix_multiply(P,A,n);
for(uint8_t i = 0; i < n; i++) {
for(uint16_t i = 0; i < n; i++) {
L[i*n + i] = 1;
}
for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) {
for(uint16_t i = 0; i < n; i++) {
for(uint16_t j = 0; j < n; j++) {
if(j <= i) {
U[j*n + i] = APrime[j*n + i];
for(uint8_t k = 0; k < j; k++) {
for(uint16_t k = 0; k < j; k++) {
U[j*n + i] -= L[j*n + k] * U[k*n + i];
}
}
if(j >= i) {
L[j*n + i] = APrime[j*n + i];
for(uint8_t k = 0; k < i; k++) {
for(uint16_t k = 0; k < i; k++) {
L[j*n + i] -= L[j*n + k] * U[k*n + i];
}
L[j*n + i] /= U[i*n + i];
@ -195,7 +190,7 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n) @@ -195,7 +190,7 @@ static void mat_LU_decompose(const T* A, T* L, T* U, T *P, uint8_t n)
* @returns false = matrix is Singular, true = matrix inversion successful
*/
template<typename T>
static bool mat_inverse(const T* A, T* inv, uint8_t n)
static bool mat_inverseN(const T* A, T* inv, uint16_t n)
{
T *L, *U, *P;
bool ret = true;
@ -221,8 +216,8 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n) @@ -221,8 +216,8 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n)
T *inv_pivoted = matrix_multiply(inv_unpivoted, P, n);
//check sanity of results
for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) {
for(uint16_t i = 0; i < n; i++) {
for(uint16_t j = 0; j < n; j++) {
if(isnan(inv_pivoted[i*n+j]) || isinf(inv_pivoted[i*n+j])){
ret = false;
}
@ -247,7 +242,7 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n) @@ -247,7 +242,7 @@ static bool mat_inverse(const T* A, T* inv, uint8_t n)
* @returns false = matrix is Singular, true = matrix inversion successful
*/
template<typename T>
bool inverse3x3(const T m[], T invOut[])
static bool inverse3x3(const T m[], T invOut[])
{
T inv[9];
// computes the inverse of a matrix m
@ -270,7 +265,7 @@ bool inverse3x3(const T m[], T invOut[]) @@ -270,7 +265,7 @@ bool inverse3x3(const T m[], T invOut[])
inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet;
inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet;
for(uint8_t i = 0; i < 9; i++){
for(uint16_t i = 0; i < 9; i++){
invOut[i] = inv[i];
}
@ -286,10 +281,10 @@ bool inverse3x3(const T m[], T invOut[]) @@ -286,10 +281,10 @@ bool inverse3x3(const T m[], T invOut[])
* @returns false = matrix is Singular, true = matrix inversion successful
*/
template<typename T>
bool inverse4x4(const T m[],T invOut[])
static bool inverse4x4(const T m[],T invOut[])
{
T inv[16], det;
uint8_t i;
uint16_t i;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
//disable FE_INEXACT detection as it fails on mac os runs
@ -440,11 +435,41 @@ bool inverse4x4(const T m[],T invOut[]) @@ -440,11 +435,41 @@ bool inverse4x4(const T m[],T invOut[])
* @returns false = matrix is Singular, true = matrix inversion successful
*/
template<typename T>
bool inverse(const T x[], T y[], uint16_t dim)
bool mat_inverse(const T x[], T y[], uint16_t dim)
{
switch(dim){
case 3: return inverse3x3(x,y);
case 4: return inverse4x4(x,y);
default: return mat_inverse(x,y,(uint8_t)dim);
case 3: return inverse3x3(x,y);
case 4: return inverse4x4(x,y);
default: return mat_inverseN(x,y,dim);
}
}
template <typename T>
void mat_mul(const T *A, const T *B, T *C, uint16_t n)
{
memset(C, 0, sizeof(T)*n*n);
for(uint16_t i = 0; i < n; i++) {
for(uint16_t j = 0; j < n; j++) {
for(uint16_t k = 0;k < n; k++) {
C[i*n + j] += A[i*n + k] * B[k*n + j];
}
}
}
}
template <typename T>
void mat_identity(T *A, uint16_t n)
{
memset(A, 0, sizeof(T)*n*n);
for (uint16_t i=0; i<n; i++) {
A[i*n+i] = 1;
}
}
template bool mat_inverse<float>(const float x[], float y[], uint16_t dim);
template void mat_mul<float>(const float *A, const float *B, float *C, uint16_t n);
template void mat_identity<float>(float x[], uint16_t dim);
template bool mat_inverse<double>(const double x[], double y[], uint16_t dim);
template void mat_mul<double>(const double *A, const double *B, double *C, uint16_t n);
template void mat_identity<double>(double x[], uint16_t dim);
Write Preview
Loading…
Cancel
Save