Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

matrix: apply PX4 astyle

Browse Source
This commit is contained in:
Daniel Agar
2021-11-16 12:30:51 -05:00
parent ab07f5300b
commit 96bf3aa5d0
44 changed files with 5137 additions and 4876 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/astyle/files_to_check_code_style.sh
-1
View File
@@ -17,7 +17,6 @@ exec find boards msg src platforms test \
-path src/drivers/uavcannode_gps_demo/libcanard -prune -o \
-path src/lib/crypto/monocypher -prune -o \
-path src/lib/events/libevents -prune -o \
-path src/lib/matrix -prune -o \
-path src/lib/parameters/uthash -prune -o \
-path src/modules/ekf2/EKF -prune -o \
-path src/modules/gyro_fft/CMSIS_5 -prune -o \
src/lib/matrix/matrix/AxisAngle.hpp
+7 -4
View File
@@ -70,8 +70,10 @@ public:
{
AxisAngle &v = *this;
Type mag = q.imag().norm();
if (fabs(mag) >= Type(1e-10)) {
v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
} else {
v = q.imag() * Type(Type(2) * Type(sign(q(0))));
}
@@ -139,15 +141,18 @@ public:
}
Vector<Type, 3> axis() {
Vector<Type, 3> axis()
{
if (Vector<Type, 3>::norm() > 0) {
return Vector<Type, 3>::unit();
} else {
return Vector3<Type>(1, 0, 0);
}
}
Type angle() {
Type angle()
{
return Vector<Type, 3>::norm();
}
};
@@ -156,5 +161,3 @@ using AxisAnglef = AxisAngle<float>;
using AxisAngled = AxisAngle<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Dcm.hpp
-2
View File
@@ -183,5 +183,3 @@ using Dcmf = Dcm<float>;
using Dcmd = Dcm<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Dual.hpp
+8 -2
View File
@@ -22,8 +22,7 @@ template <typename Type, size_t M>
class Vector;
template <typename Scalar, size_t N>
struct Dual
{
struct Dual {
static constexpr size_t WIDTH = N;
Scalar value {};
@@ -34,6 +33,7 @@ struct Dual
explicit Dual(Scalar v, size_t inputDimension = 65535)
{
value = v;
if (inputDimension < N) {
derivative(inputDimension) = Scalar(1);
}
@@ -334,9 +334,11 @@ template <typename Scalar, size_t M, size_t N>
Matrix<Scalar, M, N> collectDerivatives(const Matrix<Dual<Scalar, N>, M, 1> &input)
{
Matrix<Scalar, M, N> jac;
for (size_t i = 0; i < M; i++) {
jac.row(i) = input(i, 0).derivative;
}
return jac;
}
@@ -345,11 +347,13 @@ template <typename Scalar, size_t M, size_t N, size_t D>
Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N> &input)
{
Matrix<Scalar, M, N> r;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
r(i, j) = input(i, j).value;
}
}
return r;
}
@@ -360,10 +364,12 @@ std::ostream& operator<<(std::ostream& os,
{
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 << "]";
return os;
}
src/lib/matrix/matrix/Euler.hpp
-2
View File
@@ -153,5 +153,3 @@ using Eulerf = Euler<float>;
using Eulerd = Euler<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/LeastSquaresSolver.hpp
+23 -5
View File
@@ -16,7 +16,8 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<typename Type, size_t M, size_t N>
class LeastSquaresSolver
@@ -41,31 +42,39 @@ public:
for (size_t j = 0; j < N; j++) {
Type normx = Type(0);
for (size_t i = j; i < M; i++) {
normx += _A(i, j) * _A(i, j);
}
normx = sqrt(normx);
Type s = _A(j, j) > 0 ? Type(-1) : Type(1);
Type u1 = _A(j, j) - s * normx;
// prevent divide by zero
// also covers u1. normx is never negative
if (normx < Type(1e-8)) {
break;
}
Type w[M] = {};
w[0] = Type(1);
for (size_t i = j + 1; i < M; i++) {
w[i - j] = _A(i, j) / u1;
_A(i, j) = w[i - j];
}
_A(j, j) = s * normx;
_tau(j) = -s * u1 / normx;
for (size_t k = j + 1; k < N; k++) {
Type tmp = Type(0);
for (size_t i = j; i < M; i++) {
tmp += w[i - j] * _A(i, k);
}
for (size_t i = j; i < M; i++) {
_A(i, k) -= _tau(j) * w[i - j] * tmp;
}
@@ -82,17 +91,21 @@ public:
* This function calculates Q^T * b. This is useful for the solver
* because R*x = Q^T*b.
*/
Vector<Type, M> qtb(const Vector<Type, M> &b) {
Vector<Type, M> qtb(const Vector<Type, M> &b)
{
Vector<Type, M> qtbv = b;
for (size_t j = 0; j < N; j++) {
Type w[M];
w[0] = Type(1);
// fill vector w
for (size_t i = j + 1; i < M; i++) {
w[i - j] = _A(i, j);
}
Type tmp = Type(0);
for (size_t i = j; i < M; i++) {
tmp += w[i - j] * qtbv(i);
}
@@ -101,6 +114,7 @@ public:
qtbv(i) -= _tau(j) * w[i - j] * tmp;
}
}
return qtbv;
}
@@ -112,7 +126,8 @@ public:
* Find x in the equation Ax = b.
* A is provided in the initializer of the class.
*/
Vector<Type, N> solve(const Vector<Type, M> &b) {
Vector<Type, N> solve(const Vector<Type, M> &b)
{
Vector<Type, M> qtbv = qtb(b);
Vector<Type, N> x;
@@ -120,18 +135,23 @@ public:
for (size_t i = N - 1; i < N; i--) {
printf("i %d\n", static_cast<int>(i));
x(i) = qtbv(i);
for (size_t r = i + 1; r < N; r++) {
x(i) -= _A(i, r) * x(r);
}
// divide by zero, return vector of zeros
if (isEqualF(_A(i, i), Type(0), Type(1e-8))) {
for (size_t z = 0; z < N; z++) {
x(z) = Type(0);
}
break;
}
x(i) /= _A(i, i);
}
return x;
}
@@ -142,5 +162,3 @@ private:
};
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Matrix.hpp
+77 -18
View File
@@ -71,6 +71,7 @@ public:
Matrix(const Slice<Type, M, N, P, Q> &in_slice)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = in_slice(i, j);
@@ -103,18 +104,21 @@ public:
{
if (this != &other) {
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = other(i, j);
}
}
}
return (*this);
}
void copyTo(Type dst[M * N]) const
{
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
dst[N * i + j] = self(i, j);
@@ -232,6 +236,7 @@ public:
void operator+=(const Matrix<Type, M, N> &other)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) += other(i, j);
@@ -242,6 +247,7 @@ public:
void operator-=(const Matrix<Type, M, N> &other)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) -= other(i, j);
@@ -318,6 +324,7 @@ public:
inline void operator+=(Type scalar)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) += scalar;
@@ -350,10 +357,12 @@ public:
{
buf[0] = '\0'; // make an empty string to begin with (we need the '\0' for strlen to work)
const Matrix<Type, M, N> &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%8.8g", double(self(i, j))); // directly append to the string buffer
}
snprintf(buf + strlen(buf), n - strlen(buf), "\n");
}
}
@@ -477,6 +486,7 @@ public:
Matrix<Type, M, N> &self = *this;
const size_t min_i = M > N ? N : M;
for (size_t i = 0; i < min_i; i++) {
self(i, i) = 1;
}
@@ -526,70 +536,84 @@ public:
Matrix<Type, M, N> abs() const
{
Matrix<Type, M, N> r;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
r(i, j) = Type(fabs((*this)(i, j)));
}
}
return r;
}
Type max() const
{
Type max_val = (*this)(0, 0);
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
Type val = (*this)(i, j);
if (val > max_val) {
max_val = val;
}
}
}
return max_val;
}
Type min() const
{
Type min_val = (*this)(0, 0);
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
Type val = (*this)(i, j);
if (val < min_val) {
min_val = val;
}
}
}
return min_val;
}
bool isAllNan() const {
bool isAllNan() const
{
const Matrix<float, M, N> &self = *this;
bool result = true;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
result = result && isnan(self(i, j));
}
}
return result;
}
};
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> zeros() {
Matrix<Type, M, N> zeros()
{
Matrix<Type, M, N> m;
m.setZero();
return m;
}
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> ones() {
Matrix<Type, M, N> ones()
{
Matrix<Type, M, N> m;
m.setOne();
return m;
}
template<size_t M, size_t N>
Matrix<float, M, N> nans() {
Matrix<float, M, N> nans()
{
Matrix<float, M, N> m;
m.setNaN();
return m;
@@ -603,7 +627,8 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
template<typename Type, size_t M, size_t N>
bool isEqual(const Matrix<Type, M, N> &x,
const Matrix<Type, M, N> &y, const Type eps=Type(1e-4f)) {
const Matrix<Type, M, N> &y, const Type eps = Type(1e-4f))
{
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (!isEqualF(x(i, j), y(i, j), eps)) {
@@ -611,47 +636,59 @@ bool isEqual(const Matrix<Type, M, N> &x,
}
}
}
return true;
}
namespace typeFunction
{
template<typename Type>
Type min(const Type x, const Type y) {
Type min(const Type x, const Type y)
{
bool x_is_nan = isnan(x);
bool y_is_nan = isnan(y);
// take the non-nan value if there is one
if (x_is_nan || y_is_nan) {
if (x_is_nan && !y_is_nan) {
return y;
}
// either !x_is_nan && y_is_nan or both are NAN anyways
return x;
}
return (x < y) ? x : y;
}
template<typename Type>
Type max(const Type x, const Type y) {
Type max(const Type x, const Type y)
{
bool x_is_nan = isnan(x);
bool y_is_nan = isnan(y);
// take the non-nan value if there is one
if (x_is_nan || y_is_nan) {
if (x_is_nan && !y_is_nan) {
return y;
}
// either !x_is_nan && y_is_nan or both are NAN anyways
return x;
}
return (x > y) ? x : y;
}
template<typename Type>
Type constrain(const Type x, const Type lower_bound, const Type upper_bound) {
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));
}
@@ -659,66 +696,83 @@ Type constrain(const Type x, const Type lower_bound, const Type upper_bound) {
}
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> 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) {
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> 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> 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) {
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> 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) {
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++) {
@@ -726,19 +780,23 @@ Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
}
}
}
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) {
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;
}
@@ -749,16 +807,17 @@ std::ostream& operator<<(std::ostream& os,
{
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;
}
return os;
}
#endif // defined(SUPPORT_STDIOSTREAM)
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/PseudoInverse.hpp
+10 -2
View File
@@ -24,16 +24,19 @@ template<typename Type, size_t M, size_t N>
bool geninv(const Matrix<Type, M, N> &G, Matrix<Type, N, M> &res)
{
size_t rank;
if (M <= N) {
SquareMatrix<Type, M> A = G * G.transpose();
SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
A = L.transpose() * L;
SquareMatrix<Type, M> X;
if (!inv(A, X, rank)) {
res = Matrix<Type, N, M>();
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
}
// doing an intermediate assignment reduces stack usage
A = X * X * L.transpose();
res = G.transpose() * (L * A);
@@ -44,14 +47,17 @@ bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
A = L.transpose() * L;
SquareMatrix<Type, N> X;
if (!inv(A, X, rank)) {
res = Matrix<Type, N, M>();
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
}
// doing an intermediate assignment reduces stack usage
A = X * X * L.transpose();
res = (L * A) * G.transpose();
}
return true;
}
@@ -78,6 +84,7 @@ SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
Matrix<Type, N, N> L;
size_t r = 0;
for (size_t k = 0; k < N; k++) {
if (r == 0) {
@@ -89,12 +96,15 @@ SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
for (size_t i = k; i < N; i++) {
// Compute LL = L[k:n, :r] * L[k, :r].T
Type LL = Type();
for (size_t j = 0; j < r; j++) {
LL += L(i, j) * L(k, j);
}
L(i, r) = A(i, k) - LL;
}
}
if (L(k, r) > tol) {
L(k, r) = sqrt(L(k, r));
@@ -115,5 +125,3 @@ SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
}
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Quaternion.hpp
+22 -4
View File
@@ -100,6 +100,7 @@ public:
{
Quaternion &q = *this;
Type t = R.trace();
if (t > Type(0)) {
t = sqrt(Type(1) + t);
q(0) = Type(0.5) * t;
@@ -107,6 +108,7 @@ public:
q(1) = (R(2, 1) - R(1, 2)) * t;
q(2) = (R(0, 2) - R(2, 0)) * t;
q(3) = (R(1, 0) - R(0, 1)) * t;
} else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
t = sqrt(Type(1) + R(0, 0) - R(1, 1) - R(2, 2));
q(1) = Type(0.5) * t;
@@ -114,6 +116,7 @@ public:
q(0) = (R(2, 1) - R(1, 2)) * t;
q(2) = (R(1, 0) + R(0, 1)) * t;
q(3) = (R(0, 2) + R(2, 0)) * t;
} else if (R(1, 1) > R(2, 2)) {
t = sqrt(Type(1) - R(0, 0) + R(1, 1) - R(2, 2));
q(2) = Type(0.5) * t;
@@ -121,6 +124,7 @@ public:
q(0) = (R(0, 2) - R(2, 0)) * t;
q(1) = (R(1, 0) + R(0, 1)) * t;
q(3) = (R(2, 1) + R(1, 2)) * t;
} else {
t = sqrt(Type(1) - R(0, 0) - R(1, 1) + R(2, 2));
q(3) = Type(0.5) * t;
@@ -169,9 +173,11 @@ public:
Quaternion &q = *this;
Type angle = aa.norm();
Vector<Type, 3> axis = aa.unit();
if (angle < Type(1e-10)) {
q(0) = Type(1);
q(1) = q(2) = q(3) = 0;
} else {
Type magnitude = sin(angle / Type(2));
q(0) = cos(angle / Type(2));
@@ -194,30 +200,38 @@ public:
Quaternion &q = *this;
Vector3<Type> cr = src.cross(dst);
const float dt = src.dot(dst);
if (cr.norm() < eps && dt < 0) {
// handle corner cases with 180 degree rotations
// if the two vectors are parallel, cross product is zero
// if they point opposite, the dot product is negative
cr = src.abs();
if (cr(0) < cr(1)) {
if (cr(0) < cr(2)) {
cr = Vector3<Type>(1, 0, 0);
} else {
cr = Vector3<Type>(0, 0, 1);
}
} else {
if (cr(1) < cr(2)) {
cr = Vector3<Type>(0, 1, 0);
} else {
cr = Vector3<Type>(0, 0, 1);
}
}
q(0) = Type(0);
cr = src.cross(cr);
} else {
// normal case, do half-way quaternion solution
q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared());
}
q(1) = cr(0);
q(2) = cr(1);
q(3) = cr(2);
@@ -366,10 +380,12 @@ public:
// compute the first 4 terms of the Taylor serie
sinc_u = Type(1.0) - u2 * c3 + u4 * c5 - u6 * c7;
cos_u = Type(1.0) - u2 * c2 + u4 * c4 - u6 * c6;
} else {
sinc_u = Type(sin(u_norm) / u_norm);
cos_u = Type(cos(u_norm));
}
Vector<Type, 3> v = sinc_u * u;
return Quaternion<Type> (cos_u, v(0), v(1), v(2));
}
@@ -392,8 +408,10 @@ public:
if (u_norm < tol) { // result smaller than O(||.||^3)
return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat);
} else {
return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) / (u_norm * u_norm) * u_hat * u_hat);
return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) /
(u_norm * u_norm) * u_hat * u_hat);
}
}
@@ -443,6 +461,7 @@ public:
return q * Type(matrix::sign(q(i)));
}
}
return q;
}
@@ -466,7 +485,8 @@ public:
* @param vec vector to rotate in frame 1 (typically body frame)
* @return rotated vector in frame 2 (typically reference frame)
*/
Vector3<Type> conjugate(const Vector3<Type> &vec) const {
Vector3<Type> conjugate(const Vector3<Type> &vec) const
{
const Quaternion &q = *this;
Quaternion v(Type(0), vec(0), vec(1), vec(2));
Quaternion res = q * v * q.inversed();
@@ -528,5 +548,3 @@ using Quatd = Quaternion<double>;
using Quaterniond = Quaternion<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Scalar.hpp
+4 -4
View File
@@ -33,13 +33,15 @@ public:
return _value;
}
operator Matrix<Type, 1, 1>() const {
operator Matrix<Type, 1, 1>() const
{
Matrix<Type, 1, 1> m;
m(0, 0) = _value;
return m;
}
operator Vector<Type, 1>() const {
operator Vector<Type, 1>() const
{
Vector<Type, 1> m;
m(0) = _value;
return m;
@@ -54,5 +56,3 @@ using Scalarf = Scalar<float>;
using Scalard = Scalar<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Slice.hpp
+34 -3
View File
@@ -11,7 +11,8 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<typename Type, size_t M, size_t N>
class Matrix;
@@ -20,12 +21,14 @@ template<typename Type, size_t M>
class Vector;
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
class Slice {
class Slice
{
public:
Slice(size_t x0, size_t y0, const Matrix<Type, M, N> *data) :
_x0(x0),
_y0(y0),
_data(const_cast<Matrix<Type, M, N>*>(data)) {
_data(const_cast<Matrix<Type, M, N>*>(data))
{
static_assert(P <= M, "Slice rows bigger than backing matrix");
static_assert(Q <= N, "Slice cols bigger than backing matrix");
assert(x0 + P <= M);
@@ -53,33 +56,39 @@ public:
Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) = other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N> &operator=(const Matrix<Type, P, Q> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) = other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N> &operator=(const Type &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) = other;
}
}
return self;
}
@@ -88,9 +97,11 @@ public:
Slice<Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
{
Slice<Type, 1, Q, M, N> &self = *this;
for (size_t j = 0; j < Q; j++) {
self(0, j) = other(j);
}
return self;
}
@@ -98,33 +109,39 @@ public:
Slice<Type, P, Q, M, N> &operator+=(const Slice<Type, P, Q, MM, NN> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) += other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N> &operator+=(const Matrix<Type, P, Q> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) += other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N> &operator+=(const Type &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) += other;
}
}
return self;
}
@@ -132,44 +149,52 @@ public:
Slice<Type, P, Q, M, N> &operator-=(const Slice<Type, P, Q, MM, NN> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) -= other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N> &operator-=(const Matrix<Type, P, Q> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) -= other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N> &operator-=(const Type &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) -= other;
}
}
return self;
}
Slice<Type, P, Q, M, N> &operator*=(const Type &other)
{
Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) *= other;
}
}
return self;
}
@@ -182,11 +207,13 @@ public:
{
const Slice<Type, P, Q, M, N> &self = *this;
Matrix<Type, P, Q> res;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
res(i, j) = self(i, j) * other;
}
}
return res;
}
@@ -234,9 +261,11 @@ public:
{
const Slice<Type, P, Q, M, N> &self = *this;
Vector < Type, P < Q ? P : Q > res;
for (size_t j = 0; j < (P < Q ? P : Q); j++) {
res(j) = self(j, j);
}
return res;
}
@@ -244,11 +273,13 @@ public:
{
const Slice<Type, P, Q, M, N> &self = *this;
Type accum(0);
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
accum += self(i, j) * self(i, j);
}
}
return accum;
}
src/lib/matrix/matrix/SparseVector.hpp
+57 -19
View File
@@ -12,7 +12,8 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<int N> struct force_constexpr_eval {
static const int value = N;
};
@@ -20,12 +21,14 @@ template<int N> struct force_constexpr_eval {
// Vector that only store nonzero elements,
// which indices are specified as parameter pack
template<typename Type, size_t M, size_t... Idxs>
class SparseVector {
class SparseVector
{
private:
static constexpr size_t N = sizeof...(Idxs);
static constexpr size_t _indices[N] {Idxs...};
static constexpr bool duplicateIndices() {
static constexpr bool duplicateIndices()
{
for (size_t i = 0; i < N; i++) {
for (size_t j = 0; j < i; j++) {
if (_indices[i] == _indices[j]) {
@@ -33,15 +36,19 @@ private:
}
}
}
return false;
}
static constexpr size_t findMaxIndex() {
static constexpr size_t findMaxIndex()
{
size_t maxIndex = 0;
for (size_t i = 0; i < N; i++) {
if (maxIndex < _indices[i]) {
maxIndex = _indices[i];
}
}
return maxIndex;
}
@@ -52,96 +59,118 @@ private:
Type _data[N] {};
static constexpr int findCompressedIndex(size_t index) {
static constexpr int findCompressedIndex(size_t index)
{
int compressedIndex = -1;
for (size_t i = 0; i < N; i++) {
if (index == _indices[i]) {
compressedIndex = static_cast<int>(i);
}
}
return compressedIndex;
}
public:
constexpr size_t non_zeros() const {
constexpr size_t non_zeros() const
{
return N;
}
constexpr size_t index(size_t i) const {
constexpr size_t index(size_t i) const
{
return SparseVector::_indices[i];
}
SparseVector() = default;
SparseVector(const matrix::Vector<Type, M>& data) {
SparseVector(const matrix::Vector<Type, M> &data)
{
for (size_t i = 0; i < N; i++) {
_data[i] = data(_indices[i]);
}
}
explicit SparseVector(const Type data[N]) {
explicit SparseVector(const Type data[N])
{
memcpy(_data, data, sizeof(_data));
}
template <size_t i>
inline Type at() const {
inline Type at() const
{
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
return _data[compressed_index];
}
template <size_t i>
inline Type& at() {
inline Type &at()
{
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
return _data[compressed_index];
}
inline Type atCompressedIndex(size_t i) const {
inline Type atCompressedIndex(size_t i) const
{
assert(i < N);
return _data[i];
}
inline Type& atCompressedIndex(size_t i) {
inline Type &atCompressedIndex(size_t i)
{
assert(i < N);
return _data[i];
}
void setZero() {
void setZero()
{
for (size_t i = 0; i < N; i++) {
_data[i] = Type(0);
}
}
Type dot(const matrix::Vector<Type, M>& other) const {
Type dot(const matrix::Vector<Type, M> &other) const
{
Type accum(0);
for (size_t i = 0; i < N; i++) {
accum += _data[i] * other(_indices[i]);
}
return accum;
}
matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M>& other) const {
matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M> &other) const
{
matrix::Vector<Type, M> vec = other;
for (size_t i = 0; i < N; i++) {
vec(_indices[i]) += _data[i];
}
return vec;
}
SparseVector& operator+=(Type t) {
SparseVector &operator+=(Type t)
{
for (size_t i = 0; i < N; i++) {
_data[i] += t;
}
return *this;
}
Type norm_squared() const
{
Type accum(0);
for (size_t i = 0; i < N; i++) {
accum += _data[i] * _data[i];
}
return accum;
}
@@ -157,26 +186,35 @@ public:
};
template<typename Type, size_t Q, size_t M, size_t ... Idxs>
matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M>& mat, const matrix::SparseVector<Type, M, Idxs...>& vec) {
matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M> &mat,
const matrix::SparseVector<Type, M, Idxs...> &vec)
{
matrix::Vector<Type, Q> res;
for (size_t i = 0; i < Q; i++) {
const Vector<Type, M> row = mat.row(i);
res(i) = vec.dot(row);
}
return res;
}
// returns x.T * A * x
template<typename Type, size_t M, size_t ... Idxs>
Type quadraticForm(const matrix::SquareMatrix<Type, M>& A, const matrix::SparseVector<Type, M, Idxs...>& x) {
Type quadraticForm(const matrix::SquareMatrix<Type, M> &A, const matrix::SparseVector<Type, M, Idxs...> &x)
{
Type res = Type(0);
for (size_t i = 0; i < x.non_zeros(); i++) {
Type tmp = Type(0);
for (size_t j = 0; j < x.non_zeros(); j++) {
tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
}
res += x.atCompressedIndex(i) * tmp;
}
return res;
}
src/lib/matrix/matrix/SquareMatrix.hpp
+34 -4
View File
@@ -77,8 +77,10 @@ public:
inline SquareMatrix<Type, M> I() const
{
SquareMatrix<Type, M> i;
if (inv(*this, i)) {
return i;
} else {
i.setZero();
return i;
@@ -100,6 +102,7 @@ public:
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
@@ -110,6 +113,7 @@ public:
const SquareMatrix<Type, M> &self = *this;
unsigned idx = 0;
for (size_t x = 0; x < M; x++) {
for (size_t y = x; y < M; y++) {
res(idx) = self(x, y);
@@ -128,6 +132,7 @@ public:
for (size_t i = 0; i < M; i++) {
res += self(i, i);
}
return res;
}
@@ -156,6 +161,7 @@ public:
// set diagonals
unsigned vec_idx = 0;
for (size_t idx = first; idx < first + Width; idx++) {
self(idx, idx) = vec(vec_idx);
vec_idx ++;
@@ -187,6 +193,7 @@ public:
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
if (Width > 1) {
for (size_t row_idx = first + 1; row_idx < first + Width; row_idx++) {
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
@@ -207,12 +214,14 @@ public:
SquareMatrix<Type, M> &self = *this;
self.makeBlockSymmetric<Width>(first);
for (size_t row_idx = first; row_idx < first + Width; row_idx++) {
for (size_t col_idx = 0; col_idx < first; col_idx++) {
Type tmp = (self(row_idx, col_idx) + self(col_idx, row_idx)) / Type(2);
self(row_idx, col_idx) = tmp;
self(col_idx, row_idx) = tmp;
}
for (size_t col_idx = first + Width; col_idx < M; col_idx++) {
Type tmp = (self(row_idx, col_idx) + self(col_idx, row_idx)) / Type(2);
self(row_idx, col_idx) = tmp;
@@ -229,6 +238,7 @@ public:
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
if (Width > 1) {
for (size_t row_idx = first + 1; row_idx < first + Width; row_idx++) {
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
@@ -238,6 +248,7 @@ public:
}
}
}
return true;
}
@@ -249,18 +260,21 @@ public:
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
for (size_t row_idx = first; row_idx < first + Width; row_idx++) {
for (size_t col_idx = 0; col_idx < first; col_idx++) {
if (!isEqualF(self(row_idx, col_idx), self(col_idx, row_idx), eps)) {
return false;
}
}
for (size_t col_idx = first + Width; col_idx < M; col_idx++) {
if (!isEqualF(self(row_idx, col_idx), self(col_idx, row_idx), eps)) {
return false;
}
}
}
return self.isBlockSymmetric<Width>(first, eps);
}
@@ -270,18 +284,22 @@ using SquareMatrix3f = SquareMatrix<float, 3>;
using SquareMatrix3d = SquareMatrix<double, 3>;
template<typename Type, size_t M>
SquareMatrix<Type, M> eye() {
SquareMatrix<Type, M> eye()
{
SquareMatrix<Type, M> m;
m.setIdentity();
return m;
}
template<typename Type, size_t M>
SquareMatrix<Type, M> diag(Vector<Type, M> d) {
SquareMatrix<Type, M> diag(Vector<Type, M> d)
{
SquareMatrix<Type, M> m;
for (size_t i = 0; i < M; i++) {
m(i, i) = d(i);
}
return m;
}
@@ -292,6 +310,7 @@ SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5)
SquareMatrix<Type, M> A_pow = A;
res.setIdentity();
size_t i_factorial = 1;
for (size_t i = 1; i <= order; i++) {
i_factorial *= i;
res += A_pow / Type(i_factorial);
@@ -424,6 +443,7 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv, size_t ra
}
}
}
//printf("X:\n"); X.print();
inv = P;
return true;
@@ -477,8 +497,10 @@ template<typename Type, size_t M>
SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> &A)
{
SquareMatrix<Type, M> i;
if (inv(A, i)) {
return i;
} else {
i.setZero();
return i;
@@ -494,32 +516,42 @@ template<typename Type, size_t M>
SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> &A)
{
SquareMatrix<Type, M> 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) = sqrt(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;
}
@@ -540,5 +572,3 @@ using Matrix3f = SquareMatrix<float, 3>;
using Matrix3d = SquareMatrix<double, 3>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Vector.hpp
+31 -14
View File
@@ -44,6 +44,7 @@ public:
Vector(const Slice<Type, 1, M, P, Q> &slice_in)
{
Vector &self(*this);
for (size_t i = 0; i < M; i++) {
self(i) = slice_in(0, i);
}
@@ -65,72 +66,88 @@ public:
return v(i, 0);
}
Type dot(const MatrixM1 & b) const {
Type dot(const MatrixM1 &b) const
{
const Vector &a(*this);
Type r(0);
for (size_t i = 0; i < M; i++) {
r += a(i) * b(i, 0);
}
return r;
}
inline Type operator*(const MatrixM1 & b) const {
inline Type operator*(const MatrixM1 &b) const
{
const Vector &a(*this);
return a.dot(b);
}
inline Vector operator*(Type b) const {
inline Vector operator*(Type b) const
{
return Vector(MatrixM1::operator*(b));
}
Type norm() const {
Type norm() const
{
const Vector &a(*this);
return Type(matrix::sqrt(a.dot(a)));
}
Type norm_squared() const {
Type norm_squared() const
{
const Vector &a(*this);
return a.dot(a);
}
inline Type length() const {
inline Type length() const
{
return norm();
}
inline void normalize() {
inline void normalize()
{
(*this) /= norm();
}
Vector unit() const {
Vector unit() const
{
return (*this) / norm();
}
Vector unit_or_zero(const Type eps = Type(1e-5)) const {
Vector unit_or_zero(const Type eps = Type(1e-5)) const
{
const Type n = norm();
if (n > eps) {
return (*this) / n;
}
return Vector();
}
inline Vector normalized() const {
inline Vector normalized() const
{
return unit();
}
bool longerThan(Type testVal) const {
bool longerThan(Type testVal) const
{
return norm_squared() > testVal * testVal;
}
Vector sqrt() const {
Vector sqrt() const
{
const Vector &a(*this);
Vector r;
for (size_t i = 0; i < M; i++) {
r(i) = Type(matrix::sqrt(a(i)));
}
return r;
}
};
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Vector2.hpp
+4 -4
View File
@@ -60,12 +60,14 @@ public:
v(1) = other(1);
}
Type cross(const Matrix21 & b) const {
Type cross(const Matrix21 &b) const
{
const Vector2 &a(*this);
return a(0) * b(1, 0) - a(1) * b(0, 0);
}
Type operator%(const Matrix21 & b) const {
Type operator%(const Matrix21 &b) const
{
return (*this).cross(b);
}
@@ -76,5 +78,3 @@ using Vector2f = Vector2<float>;
using Vector2d = Vector2<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/Vector3.hpp
+12 -8
View File
@@ -44,7 +44,8 @@ public:
{
}
Vector3(Type x, Type y, Type z) {
Vector3(Type x, Type y, Type z)
{
Vector3 &v(*this);
v(0) = x;
v(1) = y;
@@ -61,7 +62,8 @@ public:
{
}
Vector3 cross(const Matrix31 & b) const {
Vector3 cross(const Matrix31 &b) const
{
const Vector3 &a(*this);
return {a(1) *b(2, 0) - a(2) *b(1, 0), -a(0) *b(2, 0) + a(2) *b(0, 0), a(0) *b(1, 0) - a(1) *b(0, 0)};
}
@@ -105,18 +107,21 @@ public:
return Vector<Type, 3>::operator*(b);
}
inline Vector3 operator%(const Matrix31 & b) const {
inline Vector3 operator%(const Matrix31 &b) const
{
return (*this).cross(b);
}
/**
* Override vector ops so Vector3 type is returned
*/
inline Vector3 unit() const {
inline Vector3 unit() const
{
return Vector3(Vector<Type, 3>::unit());
}
inline Vector3 normalized() const {
inline Vector3 normalized() const
{
return unit();
}
@@ -131,7 +136,8 @@ public:
}
Dcm<Type> hat() const { // inverse to Dcm.vee() operation
Dcm<Type> hat() const // inverse to Dcm.vee() operation
{
const Vector3 &v(*this);
Dcm<Type> A;
A(0, 0) = 0;
@@ -152,5 +158,3 @@ using Vector3f = Vector3<float>;
using Vector3d = Vector3<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/matrix/filter.hpp
+2 -1
View File
@@ -2,7 +2,8 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<typename Type, size_t M, size_t N>
int kalman_correct(
src/lib/matrix/matrix/helper_functions.hpp
+10 -5
View File
@@ -10,7 +10,8 @@ namespace matrix
{
template<typename Type>
bool is_finite(Type x) {
bool is_finite(Type x)
{
#if defined (__PX4_NUTTX)
return PX4_ISFINITE(x);
#elif defined (__PX4_QURT)
@@ -43,7 +44,8 @@ namespace detail
{
template<typename Floating>
Floating wrap_floating(Floating x, Floating low, Floating high) {
Floating wrap_floating(Floating x, Floating low, Floating high)
{
// already in range
if (low <= x && x < high) {
return x;
@@ -65,7 +67,8 @@ Floating wrap_floating(Floating x, Floating low, Floating high) {
* @param high upper limit of the allowed range
* @return wrapped value inside the range
*/
inline float wrap(float x, float low, float high) {
inline float wrap(float x, float low, float high)
{
return matrix::detail::wrap_floating(x, low, high);
}
@@ -77,7 +80,8 @@ inline float wrap(float x, float low, float high) {
* @param high upper limit of the allowed range
* @return wrapped value inside the range
*/
inline double wrap(double x, double low, double high) {
inline double wrap(double x, double low, double high)
{
return matrix::detail::wrap_floating(x, low, high);
}
@@ -90,7 +94,8 @@ inline double wrap(double x, double low, double high) {
* @return wrapped value inside the range
*/
template<typename Integer>
Integer wrap(Integer x, Integer low, Integer high) {
Integer wrap(Integer x, Integer low, Integer high)
{
const auto range = high - low;
if (x < low) {
src/lib/matrix/matrix/integration.hpp
+8 -4
View File
@@ -2,7 +2,8 @@
#include "math.hpp"
namespace matrix {
namespace matrix
{
template<typename Type, size_t M, size_t N>
int integrate_rk4(
@@ -20,13 +21,17 @@ int integrate_rk4(
y1 = y0;
Type h = h0;
Vector<Type, M> k1, k2, k3, k4;
if (tf < t0) return -1; // make sure t1 > t0
if (tf < t0) { return -1; } // make sure t1 > t0
while (t1 < tf) {
if (t1 + h0 < tf) {
h = h0;
} else {
h = tf - t1;
}
k1 = f(t1, y1, u);
k2 = f(t1 + h / 2, y1 + k1 * h / 2, u);
k3 = f(t1 + h / 2, y1 + k2 * h / 2, u);
@@ -34,9 +39,8 @@ int integrate_rk4(
y1 += (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
t1 += h;
}
return 0;
}
} // namespace matrix
// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :
src/lib/matrix/matrix/stdlib_imports.hpp
+2 -1
View File
@@ -20,7 +20,8 @@
#define M_TWOPI (M_PI * 2.0)
#endif
namespace matrix {
namespace matrix
{
#if !defined(FLT_EPSILON)
#define FLT_EPSILON __FLT_EPSILON__
src/lib/matrix/test/attitude.cpp
+4 -3
View File
@@ -7,7 +7,8 @@ using namespace matrix;
// manually instantiated all files we intend to test
// so that coverage works correctly
// doesn't matter what test this is in
namespace matrix {
namespace matrix
{
template class Matrix<float, 3, 3>;
template class Vector3<float>;
template class Vector2<float>;
@@ -171,6 +172,7 @@ int main()
// dcm renormalize
Dcmf A = eye<float, 3>();
Dcmf R(euler_check);
for (size_t i = 0; i < 1000; i++) {
A = R * A;
}
@@ -182,6 +184,7 @@ int main()
Vector3f rvec(matrix::Matrix<float, 1, 3>(A.row(r)).transpose());
err += fabs(1.0f - rvec.length());
}
TEST(err < eps);
// constants
@@ -486,5 +489,3 @@ int main()
#endif
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/copyto.cpp
+8 -2
View File
@@ -3,7 +3,8 @@
using namespace matrix;
namespace {
namespace
{
void doTheCopy(const Matrix<float, 2, 3> &A, float array_A[6])
{
A.copyTo(array_A);
@@ -18,6 +19,7 @@ int main()
const Vector3f v(1, 2, 3);
float dst3[3] = {};
v.copyTo(dst3);
for (size_t i = 0; i < 3; i++) {
TEST(fabs(v(i) - dst3[i]) < eps);
}
@@ -26,6 +28,7 @@ int main()
Quatf q(1, 2, 3, 4);
float dst4[4] = {};
q.copyTo(dst4);
for (size_t i = 0; i < 4; i++) {
TEST(fabs(q(i) - dst4[i]) < eps);
}
@@ -41,6 +44,7 @@ int main()
float array_A[6] = {};
doTheCopy(A, array_A);
float array_row[6] = {1, 2, 3, 4, 5, 6};
for (size_t i = 0; i < 6; i++) {
TEST(fabs(array_A[i] - array_row[i]) < eps);
}
@@ -48,6 +52,7 @@ int main()
// Matrix copyToColumnMajor
A.copyToColumnMajor(array_A);
float array_column[6] = {1, 4, 2, 5, 3, 6};
for (size_t i = 0; i < 6; i++) {
TEST(fabs(array_A[i] - array_column[i]) < eps);
}
@@ -55,6 +60,7 @@ int main()
// Slice copyTo
float dst5[2] = {};
v.slice<2, 1>(0, 0).copyTo(dst5);
for (size_t i = 0; i < 2; i++) {
TEST(fabs(v(i) - dst5[i]) < eps);
}
@@ -62,6 +68,7 @@ int main()
float subarray_A[4] = {};
A.slice<2, 2>(0, 0).copyToColumnMajor(subarray_A);
float subarray_column[4] = {1, 4, 2, 5};
for (size_t i = 0; i < 4; i++) {
TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
}
@@ -69,4 +76,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/dual.cpp
+4 -3
View File
@@ -11,7 +11,8 @@ bool isEqualAll(Dual<Scalar, N> a, Dual<Scalar, N> b)
}
template <typename T>
T testFunction(const Vector<T, 3>& point) {
T testFunction(const Vector<T, 3> &point)
{
// function is f(x,y,z) = x^2 + 2xy + 3y^2 + z
return point(0) * point(0)
+ 2.f * point(0) * point(1)
@@ -266,8 +267,8 @@ int main()
positionMeasurement,
dt);
float h = 0.001f;
for (size_t i = 0; i < 4; i++)
{
for (size_t i = 0; i < 4; i++) {
Quaternion<float> h4 = velocityOrientation;
h4(i) += h;
numerical_jacobian.col(i) = (positionError(positionState,
src/lib/matrix/test/filter.cpp
-1
View File
@@ -26,4 +26,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/hatvee.cpp
-1
View File
@@ -16,4 +16,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/helper.cpp
-1
View File
@@ -75,4 +75,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/integration.cpp
+2 -2
View File
@@ -5,7 +5,8 @@ using namespace matrix;
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/);
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/) {
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/)
{
float v = -sin(t);
return v * ones<float, 6, 1>();
}
@@ -23,4 +24,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/inverse.cpp
+1 -1
View File
@@ -118,6 +118,7 @@ int main()
for (size_t i = 0; i < 9; i++) {
A2(0, i) = 0;
}
A2_I = inv(A2);
SquareMatrix<float, 9> Z9 = zeros<float, 9, 9>();
TEST(!A2.I(A2_I));
@@ -161,4 +162,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/least_squares.cpp
+14 -8
View File
@@ -13,21 +13,26 @@ int main()
int ret;
ret = test_4x4<float>();
if (ret != 0) return ret;
if (ret != 0) { return ret; }
ret = test_4x4<double>();
if (ret != 0) return ret;
if (ret != 0) { return ret; }
ret = test_4x3();
if (ret != 0) return ret;
if (ret != 0) { return ret; }
ret = test_div_zero();
if (ret != 0) return ret;
if (ret != 0) { return ret; }
return 0;
}
int test_4x3() {
int test_4x3()
{
// Start with an (m x n) A matrix
float data[12] = {20.f, -10.f, -13.f,
17.f, 16.f, -18.f,
@@ -53,7 +58,8 @@ int test_4x3() {
}
template<typename Type>
int test_4x4() {
int test_4x4()
{
// Start with an (m x n) A matrix
const Type data[16] = { 20.f, -10.f, -13.f, 21.f,
17.f, 16.f, -18.f, -14.f,
@@ -79,7 +85,8 @@ int test_4x4() {
return 0;
}
int test_div_zero() {
int test_div_zero()
{
float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
Matrix<float, 2, 2> A(data);
@@ -97,4 +104,3 @@ int test_div_zero() {
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/matrixAssignment.cpp
+11 -1
View File
@@ -31,11 +31,13 @@ int main()
Matrix3f m_nan;
m_nan.setNaN();
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
TEST(isnan(m_nan(i, j)));
}
}
TEST(m_nan.isAllNan());
float data2d[3][3] = {
@@ -44,11 +46,13 @@ int main()
{7, 8, 9}
};
m2 = Matrix3f(data2d);
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
TEST(fabs(data[i * 3 + j] - m2(i, j)) < FLT_EPSILON);
}
}
TEST(!m2.isAllNan());
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
@@ -224,11 +228,14 @@ int main()
printf("%s\n", buffer); // for debugging in case of failure
char output[] = "\t 1\t12345.123\n\t12345.123\t0.12345679\n\t1.2345679e+10\t1.234568e+12\n";
printf("%s\n", output); // for debugging in case of failure
for (size_t i = 0; i < len; i++) {
if (buffer[i] != output[i]) { // for debugging in case of failure
printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure
}
TEST(buffer[i] == output[i]);
if (buffer[i] == '\0') {
break;
}
@@ -244,17 +251,20 @@ int main()
FILE *fp = fopen("testoutput.txt", "r");
TEST(fp != nullptr);
TEST(!fseek(fp, 0, SEEK_SET));
for (size_t i = 0; i < len; i++) {
char c = static_cast<char>(fgetc(fp));
if (c == '\n') {
break;
}
printf("%d %d %d\n", static_cast<int>(i), output[i], c);
TEST(c == output[i]);
}
TEST(!fclose(fp));
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/matrixMult.cpp
-1
View File
@@ -68,4 +68,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/matrixScalarMult.cpp
-1
View File
@@ -15,4 +15,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/pseudoInverse.cpp
-1
View File
@@ -150,4 +150,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/setIdentity.cpp
-1
View File
@@ -36,4 +36,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/slice.cpp
+4 -3
View File
@@ -80,7 +80,8 @@ int main()
TEST(isEqual(H, F));
float data_4_check[5] = {3, 11, 9, 0, 0};
{ // assigning row slices to each other
{
// assigning row slices to each other
const Matrix<float, 3, 1> J(data_3_check);
Matrix<float, 5, 1> K;
K.row(2) = J.row(0);
@@ -90,7 +91,8 @@ int main()
Matrix<float, 5, 1> K_check(data_4_check);
TEST(isEqual(K, K_check));
}
{ // assigning col slices to each other
{
// assigning col slices to each other
const Matrix<float, 1, 3> J(data_3_check);
Matrix<float, 1, 5> K;
K.col(2) = J.col(0);
@@ -232,4 +234,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/sparseVector.cpp
+26 -12
View File
@@ -3,7 +3,8 @@
using namespace matrix;
TEST(sparseVectorTest, defaultConstruction) {
TEST(sparseVectorTest, defaultConstruction)
{
SparseVectorf<24, 4, 6> a;
EXPECT_EQ(a.non_zeros(), 2);
EXPECT_EQ(a.index(0), 4);
@@ -12,7 +13,8 @@ TEST(sparseVectorTest, defaultConstruction) {
a.at<6>() = 2.f;
}
TEST(sparseVectorTest, initializationWithData) {
TEST(sparseVectorTest, initializationWithData)
{
const float data[3] = {1.f, 2.f, 3.f};
SparseVectorf<24, 4, 6, 22> a(data);
EXPECT_EQ(a.non_zeros(), 3);
@@ -24,24 +26,29 @@ TEST(sparseVectorTest, initializationWithData) {
EXPECT_FLOAT_EQ(a.at<22>(), data[2]);
}
TEST(sparseVectorTest, initialisationFromVector) {
TEST(sparseVectorTest, initialisationFromVector)
{
const Vector3f vec(1.f, 2.f, 3.f);
const SparseVectorf<3, 0, 2> a(vec);
EXPECT_FLOAT_EQ(a.at<0>(), vec(0));
EXPECT_FLOAT_EQ(a.at<2>(), vec(2));
}
TEST(sparseVectorTest, accessDataWithCompressedIndices) {
TEST(sparseVectorTest, accessDataWithCompressedIndices)
{
const Vector3f vec(1.f, 2.f, 3.f);
SparseVectorf<3, 0, 2> a(vec);
for (size_t i = 0; i < a.non_zeros(); i++) {
a.atCompressedIndex(i) = static_cast<float>(i);
}
EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0));
EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1));
}
TEST(sparseVectorTest, setZero) {
TEST(sparseVectorTest, setZero)
{
const float data[3] = {1.f, 2.f, 3.f};
SparseVectorf<24, 4, 6, 22> a(data);
a.setZero();
@@ -50,7 +57,8 @@ TEST(sparseVectorTest, setZero) {
EXPECT_FLOAT_EQ(a.at<22>(), 0.f);
}
TEST(sparseVectorTest, additionWithDenseVector) {
TEST(sparseVectorTest, additionWithDenseVector)
{
Vector<float, 4> dense_vec;
dense_vec.setAll(1.f);
const float data[3] = {1.f, 2.f, 3.f};
@@ -62,7 +70,8 @@ TEST(sparseVectorTest, additionWithDenseVector) {
EXPECT_FLOAT_EQ(res(3), 4.f);
}
TEST(sparseVectorTest, addScalar) {
TEST(sparseVectorTest, addScalar)
{
const float data[3] = {1.f, 2.f, 3.f};
SparseVectorf<4, 1, 2, 3> sparse_vec(data);
sparse_vec += 2.f;
@@ -71,7 +80,8 @@ TEST(sparseVectorTest, addScalar) {
EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f);
}
TEST(sparseVectorTest, dotProductWithDenseVector) {
TEST(sparseVectorTest, dotProductWithDenseVector)
{
Vector<float, 4> dense_vec;
dense_vec.setAll(3.f);
const float data[3] = {1.f, 2.f, 3.f};
@@ -80,7 +90,8 @@ TEST(sparseVectorTest, dotProductWithDenseVector) {
EXPECT_FLOAT_EQ(res, 18.f);
}
TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
TEST(sparseVectorTest, multiplicationWithDenseMatrix)
{
Matrix<float, 2, 3> dense_matrix;
dense_matrix.setAll(2.f);
dense_matrix(1, 1) = 3.f;
@@ -91,7 +102,8 @@ TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
EXPECT_TRUE(isEqual(res_dense, res_sparse));
}
TEST(sparseVectorTest, quadraticForm) {
TEST(sparseVectorTest, quadraticForm)
{
float matrix_data[9] = {1, 2, 3,
2, 4, 5,
3, 5, 6
@@ -102,7 +114,8 @@ TEST(sparseVectorTest, quadraticForm) {
EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
}
TEST(sparseVectorTest, norms) {
TEST(sparseVectorTest, norms)
{
const float data[2] = {3.f, 4.f};
const SparseVectorf<4, 1, 3> sparse_vec(data);
EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f);
@@ -112,7 +125,8 @@ TEST(sparseVectorTest, norms) {
}
int main(int argc, char **argv) {
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
std::cout << "Run SparseVector tests" << std::endl;
return RUN_ALL_TESTS();
src/lib/matrix/test/squareMatrix.cpp
-1
View File
@@ -145,4 +145,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/test_data.py
-2
View File
@@ -145,5 +145,3 @@ print('b:')
pprint(b)
print('x:')
pprint(x)
# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :
src/lib/matrix/test/transpose.cpp
-1
View File
@@ -16,4 +16,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/upperRightTriangle.cpp
-1
View File
@@ -20,4 +20,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vector.cpp
-1
View File
@@ -45,4 +45,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vector2.cpp
-1
View File
@@ -37,4 +37,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vector3.cpp
-1
View File
@@ -58,4 +58,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vectorAssignment.cpp
-1
View File
@@ -31,4 +31,3 @@ int main()
return 0;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */