Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

Add min, max, constrain function for Matrix (#105)

Browse Source 
* Add min, max, constrain function for Matrix

* Set individual elements to nan in constrain

* Deal with NANs
This commit is contained in:
kritz
2019-11-12 18:57:12 +01:00
committed by Julian Kent
parent 93d42947b6
commit 38e966cea1
2 changed files with 176 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Matrix.hpp
+122
View File
@@ -580,6 +580,128 @@ bool isEqual(const Matrix<Type, M, N> &x,
return true;
}
namespace typeFunction
{
template<typename Type>
Type min(const Type x, const Type y) {
bool x_is_nan = isnan(x);
bool y_is_nan = isnan(y);
// z > nan for z != nan is required by C the standard
if(x_is_nan || y_is_nan) {
if(x_is_nan && !y_is_nan) return y;
if(!x_is_nan && y_is_nan) return x;
return x;
}
return (x < y) ? x : y;
}
template<typename Type>
Type max(const Type x, const Type y) {
bool x_is_nan = isnan(x);
bool y_is_nan = isnan(y);
// z > nan for z != nan is required by C the standard
if(x_is_nan || y_is_nan) {
if(x_is_nan && !y_is_nan) return y;
if(!x_is_nan && y_is_nan) return x;
return x;
}
return (x > y) ? x : y;
}
template<typename Type>
Type constrain(const Type x, const Type lower_bound, const Type upper_bound) {
if(lower_bound > upper_bound) {
return NAN;
} else if(isnan(x)) {
return NAN;
} else {
return typeFunction::max(lower_bound, typeFunction::min(upper_bound, x));
}
}
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> min(const Matrix<Type, M, N> &x, const Type scalar_upper_bound) {
Matrix<Type,M,N> m;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
m(i,j) = typeFunction::min(x(i,j),scalar_upper_bound);
}
}
return m;
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> min(const Type scalar_upper_bound, const Matrix<Type, M, N> &x) {
return min(x, scalar_upper_bound);
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> min(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2) {
Matrix<Type,M,N> m;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
m(i,j) = typeFunction::min(x1(i,j),x2(i,j));
}
}
return m;
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> max(const Matrix<Type, M, N> &x, const Type scalar_lower_bound) {
Matrix<Type,M,N> m;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
m(i,j) = typeFunction::max(x(i,j),scalar_lower_bound);
}
}
return m;
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> max(const Type scalar_lower_bound, const Matrix<Type, M, N> &x) {
return max(x, scalar_lower_bound);
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> max(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2) {
Matrix<Type,M,N> m;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
m(i,j) = typeFunction::max(x1(i,j),x2(i,j));
}
}
return m;
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
const Type scalar_lower_bound,
const Type scalar_upper_bound) {
Matrix<Type,M,N> m;
if(scalar_lower_bound > scalar_upper_bound) {
m.setNaN();
} else {
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
m(i,j) = typeFunction::constrain(x(i,j), scalar_lower_bound, scalar_upper_bound);
}
}
}
return m;
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
const Matrix<Type, M, N> &x_lower_bound,
const Matrix<Type, M, N> &x_upper_bound) {
Matrix<Type,M,N> m;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
m(i,j) = typeFunction::constrain(x(i,j), x_lower_bound(i,j), x_upper_bound(i,j));
}
}
return m;
}
#if defined(SUPPORT_STDIOSTREAM)
template<typename Type, size_t M, size_t N>
std::ostream& operator<<(std::ostream& os,
test/matrixAssignment.cpp
+54
View File
@@ -134,6 +134,60 @@ int main()
m7.setNaN();
TEST(m7 != m8);
// min, max, constrain matrix values with scalar
float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
float lower_bound = 7;
float upper_bound = 11;
float data_m9_lower_bounded[9] = {7, 7, 7, 8, 10, 12, 14, 16, 18};
float data_m9_upper_bounded[9] = {2, 4, 6, 8, 10, 11, 11, 11, 11};
float data_m9_lower_constrained[9] = {7, 7, 7, 8, 10, 11, 11, 11, 11};
Matrix3f m9(data_m9);
Matrix3f m9_lower_bounded(data_m9_lower_bounded);
Matrix3f m9_upper_bounded(data_m9_upper_bounded);
Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained);
TEST(isEqual(max(m9, lower_bound), m9_lower_bounded));
TEST(isEqual(max(lower_bound, m9), m9_lower_bounded));
TEST(isEqual(min(m9, upper_bound), m9_upper_bounded));
TEST(isEqual(min(upper_bound, m9), m9_upper_bounded));
TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained));
TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan));
// min, max, constrain matrix values with matrix of same size
float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
float data_m10_lower_bound[9] = {5, 7, 4, 8, 19, 10, 20, 16, 18};
float data_m10_lower_bounded_ref[9] = {5, 7, 6, 8, 19, 12, 20, 16, 18};
float data_m10_upper_bound[9] = {6, 4, 8, 18, 20, 11, 30, 16, 18};
float data_m10_upper_bounded_ref[9] = {2, 4, 6, 8, 10, 11, 14, 16, 18};
float data_m10_constrained_ref[9] = {5, NAN, 6, 8, 19, 11, 20, 16, 18};
Matrix3f m10(data_m10);
Matrix3f m10_lower_bound(data_m10_lower_bound);
Matrix3f m10_lower_bounded_ref(data_m10_lower_bounded_ref);
Matrix3f m10_upper_bound(data_m10_upper_bound);
Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref);
Matrix3f m10_constrained_ref(data_m10_constrained_ref);
TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref));
TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref));
TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref));
TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref));
TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref));
// min, max, constrain with NAN
TEST(isEqualF(matrix::typeFunction::min(5.0f,NAN), 5.0f));
TEST(isEqualF(matrix::typeFunction::min(NAN,5.0f), 5.0f));
TEST(isEqualF(matrix::typeFunction::min(NAN,NAN), NAN));
TEST(isEqualF(matrix::typeFunction::max(5.0f,NAN), 5.0f));
TEST(isEqualF(matrix::typeFunction::max(NAN,5.0f), 5.0f));
TEST(isEqualF(matrix::typeFunction::max(NAN,NAN), NAN));
TEST(isEqualF(matrix::typeFunction::constrain(NAN,5.0f,6.0f), NAN));
TEST(isEqualF(matrix::typeFunction::constrain(1.0f,5.0f,4.0f), NAN));
TEST(isEqualF(matrix::typeFunction::constrain(6.0f,NAN,5.0f), 5.0f));
TEST(isEqualF(matrix::typeFunction::constrain(1.0f,5.0f,NAN), 5.0f));
Vector2f v1{NAN, 5.0f};
Vector2f v1_min = min(v1,1.0f);
Matrix3f m11 = min(m10_constrained_ref,NAN);
TEST(isEqualF(fmin(NAN,1.0f), float(v1_min(0))));
TEST(isEqual(m11, m10_constrained_ref));
// check write_string()
float comma[6] = {
1.f, 12345.678f,