matrix: apply PX4 astyle

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