dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

matrix: use output stream function template 

to enable proper automatic output in gtest unit tests
that compare two `Matrix`, `Vector` or two `Dual` objects.

Credits to @jwidauer for showing me this trick.
This commit is contained in:
Matthias Grob 2023-02-21 16:18:43 +01:00
parent b0189d95af
commit b1435c6e34
4 changed files with 59 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/lib/matrix/matrix/Dual.hpp
View File

@ -92,6 +92,16 @@ struct Dual {
return (*this = *this / a);
}
bool operator==(const Dual<Scalar, N> &other) const
{
return isEqualF(value, other.value) && (derivative == other.derivative);
}
bool operator!=(const Dual<Scalar, N> &other) const
{
const Dual<Scalar, N> &self = *this;
return !(self == other);
}
};
// operators
@ -359,23 +369,12 @@ Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N> &input)
return r;
}
#if defined(SUPPORT_STDIOSTREAM)
template<typename Type, size_t N>
std::ostream &operator<<(std::ostream &os,
const matrix::Dual<Type, N> &dual)
template<typename OStream, typename Type, size_t N>
OStream &operator<<(OStream &os, const matrix::Dual<Type, N> &dual)
{
os << "[";
os << std::setw(10) << dual.value << ";";
for (size_t j = 0; j < N; ++j) {
os << "\t";
os << std::setw(10) << static_cast<double>(dual.derivative(j));
}
os << "]";
os << "\nValue: " << dual.value << "\nDerivative:" << dual.derivative;
return os;
}
#endif // defined(SUPPORT_STDIOSTREAM)
}

29
src/lib/matrix/matrix/Matrix.hpp
View File

@ -12,11 +12,6 @@
#include <cstdio>
#include <cstring>
#if defined(SUPPORT_STDIOSTREAM)
#include <iostream>
#include <iomanip>
#endif // defined(SUPPORT_STDIOSTREAM)
#include "math.hpp"
namespace matrix
@ -816,24 +811,18 @@ Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
return m;
}
#if defined(SUPPORT_STDIOSTREAM)
template<typename Type, size_t M, size_t N>
std::ostream &operator<<(std::ostream &os,
const matrix::Matrix<Type, M, N> &matrix)
template<typename OStream, typename Type, size_t M, size_t N>
OStream &operator<<(OStream &os, const matrix::Matrix<Type, M, N> &matrix)
{
for (size_t i = 0; i < M; ++i) {
os << "[";
for (size_t j = 0; j < N; ++j) {
os << std::setw(10) << matrix(i, j);
os << "\t";
}
os << "]" << std::endl;
}
os << "\n";
// element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
static const size_t n = 15 * N * M + M + 1;
char *buf = new char[n];
matrix.write_string(buf, n);
os << buf;
delete[] buf;
return os;
}
#endif // defined(SUPPORT_STDIOSTREAM)
} // namespace matrix

19
src/lib/matrix/matrix/Vector.hpp
View File

@ -148,6 +148,25 @@ public:
return r;
}
void print() const
{
(*this).transpose().print();
}
};
template<typename OStream, typename Type, size_t M>
OStream &operator<<(OStream &os, const matrix::Vector<Type, M> &vector)
{
os << "\n";
// element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
static const size_t n = 15 * M * 1 + 1 + 1;
char *buf = new char[n];
vector.transpose().write_string(buf, n);
os << buf;
delete[] buf;
return os;
}
} // namespace matrix

42
src/lib/matrix/test/MatrixDualTest.cpp
View File

@ -39,12 +39,6 @@
using namespace matrix;
template <typename Scalar, size_t N>
bool isEqualAll(Dual<Scalar, N> a, Dual<Scalar, N> b)
{
return isEqualF(a.value, b.value) && a.derivative == b.derivative;
}
template <typename T>
T testFunction(const Vector<T, 3> &point)
{
@ -83,9 +77,9 @@ TEST(MatrixDualTest, Dual)
EXPECT_FLOAT_EQ(c.derivative(0), 2.f);
Dual<float, 1> d = +a;
EXPECT_TRUE(isEqualAll(d, a));
EXPECT_EQ(d, a);
d += b;
EXPECT_TRUE(isEqualAll(d, c));
EXPECT_EQ(d, c);
Dual<float, 1> e = a;
e += b.value;
@ -93,7 +87,7 @@ TEST(MatrixDualTest, Dual)
EXPECT_EQ(e.derivative, a.derivative);
Dual<float, 1> f = b.value + a;
EXPECT_TRUE(isEqualAll(f, e));
EXPECT_EQ(f, e);
}
{
@ -103,9 +97,9 @@ TEST(MatrixDualTest, Dual)
EXPECT_FLOAT_EQ(c.derivative(0), 0.f);
Dual<float, 1> d = b;
EXPECT_TRUE(isEqualAll(d, b));
EXPECT_EQ(d, b);
d -= a;
EXPECT_TRUE(isEqualAll(d, c));
EXPECT_EQ(d, c);
Dual<float, 1> e = b;
e -= a.value;
@ -113,7 +107,7 @@ TEST(MatrixDualTest, Dual)
EXPECT_EQ(e.derivative, b.derivative);
Dual<float, 1> f = a.value - b;
EXPECT_TRUE(isEqualAll(f, -e));
EXPECT_EQ(f, -e);
}
{
@ -123,9 +117,9 @@ TEST(MatrixDualTest, Dual)
EXPECT_FLOAT_EQ(c.derivative(0), 9.f);
Dual<float, 1> d = a;
EXPECT_TRUE(isEqualAll(d, a));
EXPECT_EQ(d, a);
d *= b;
EXPECT_TRUE(isEqualAll(d, c));
EXPECT_EQ(d, c);
Dual<float, 1> e = a;
e *= b.value;
@ -133,7 +127,7 @@ TEST(MatrixDualTest, Dual)
EXPECT_EQ(e.derivative, a.derivative * b.value);
Dual<float, 1> f = b.value * a;
EXPECT_TRUE(isEqualAll(f, e));
EXPECT_EQ(f, e);
}
{
@ -143,9 +137,9 @@ TEST(MatrixDualTest, Dual)
EXPECT_FLOAT_EQ(c.derivative(0), -1.f / 3.f);
Dual<float, 1> d = b;
EXPECT_TRUE(isEqualAll(d, b));
EXPECT_EQ(d, b);
d /= a;
EXPECT_TRUE(isEqualAll(d, c));
EXPECT_EQ(d, c);
Dual<float, 1> e = b;
e /= a.value;
@ -153,7 +147,7 @@ TEST(MatrixDualTest, Dual)
EXPECT_EQ(e.derivative, b.derivative / a.value);
Dual<float, 1> f = a.value / b;
EXPECT_TRUE(isEqualAll(f, 1.f / e));
EXPECT_EQ(f, 1.f / e);
}
{
@ -170,9 +164,9 @@ TEST(MatrixDualTest, Dual)
{
// abs
EXPECT_TRUE(isEqualAll(a, abs(-a)));
EXPECT_FALSE(isEqualAll(-a, abs(a)));
EXPECT_TRUE(isEqualAll(-a, -abs(a)));
EXPECT_EQ(a, abs(-a));
EXPECT_NE(-a, abs(a));
EXPECT_EQ(-a, -abs(a));
}
{
@ -197,8 +191,8 @@ TEST(MatrixDualTest, Dual)
{
// max/min
EXPECT_TRUE(isEqualAll(b, max(a, b)));
EXPECT_TRUE(isEqualAll(a, min(a, b)));
EXPECT_EQ(b, max(a, b));
EXPECT_EQ(a, min(a, b));
}
{
@ -248,7 +242,7 @@ TEST(MatrixDualTest, Dual)
{
// atan2
EXPECT_FLOAT_EQ(atan2(a, b).value, atan2(a.value, b.value));
EXPECT_TRUE(isEqualAll(atan2(a, Dual<float, 1>(b.value)), atan(a / b.value))); // atan2'(y, x) = atan'(y/x)
EXPECT_EQ(atan2(a, Dual<float, 1>(b.value)), atan(a / b.value)); // atan2'(y, x) = atan'(y/x)
}
{