Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 5 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/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 \
src/lib/matrix/matrix/AxisAngle.hpp
+11 -8
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))));
} }
@@ -127,27 +129,30 @@ public:
* @param axis An axis of rotation, normalized if not unit length * @param axis An axis of rotation, normalized if not unit length
* @param angle The amount to rotate * @param angle The amount to rotate
*/ */
AxisAngle(const Matrix31 & axis_, Type angle_) AxisAngle(const Matrix31 &axis_, Type angle_)
{ {
AxisAngle &v = *this; AxisAngle &v = *this;
// make sure axis is a unit vector // make sure axis is a unit vector
Vector<Type, 3> a = axis_; Vector<Type, 3> a = axis_;
a = a.unit(); a = a.unit();
v(0) = a(0)*angle_; v(0) = a(0) * angle_;
v(1) = a(1)*angle_; v(1) = a(1) * angle_;
v(2) = a(2)*angle_; v(2) = a(2) * angle_;
} }
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 : */
src/lib/matrix/matrix/Dcm.hpp
+2 -4
View File
@@ -173,8 +173,8 @@ public:
{ {
/* renormalize rows */ /* renormalize rows */
for (size_t r = 0; r < 3; r++) { for (size_t r = 0; r < 3; r++) {
matrix::Vector3<Type> rvec(Matrix<Type,1,3>(this->Matrix<Type,3,3>::row(r)).transpose()); matrix::Vector3<Type> rvec(Matrix<Type, 1, 3>(this->Matrix<Type, 3, 3>::row(r)).transpose());
this->Matrix<Type,3,3>::row(r) = rvec.normalized(); this->Matrix<Type, 3, 3>::row(r) = rvec.normalized();
} }
} }
}; };
@@ -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 : */
src/lib/matrix/matrix/Dual.hpp
+54 -48
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,58 +33,59 @@ 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);
} }
} }
explicit Dual(Scalar v, const Vector<Scalar, N>& d) : explicit Dual(Scalar v, const Vector<Scalar, N> &d) :
value(v), derivative(d) value(v), derivative(d)
{} {}
Dual<Scalar, N>& operator=(const Scalar& a) Dual<Scalar, N> &operator=(const Scalar &a)
{ {
derivative.setZero(); derivative.setZero();
value = a; value = a;
return *this; return *this;
} }
Dual<Scalar, N>& operator +=(const Dual<Scalar, N>& a) Dual<Scalar, N> &operator +=(const Dual<Scalar, N> &a)
{ {
return (*this = *this + a); return (*this = *this + a);
} }
Dual<Scalar, N>& operator *=(const Dual<Scalar, N>& a) Dual<Scalar, N> &operator *=(const Dual<Scalar, N> &a)
{ {
return (*this = *this * a); return (*this = *this * a);
} }
Dual<Scalar, N>& operator -=(const Dual<Scalar, N>& a) Dual<Scalar, N> &operator -=(const Dual<Scalar, N> &a)
{ {
return (*this = *this - a); return (*this = *this - a);
} }
Dual<Scalar, N>& operator /=(const Dual<Scalar, N>& a) Dual<Scalar, N> &operator /=(const Dual<Scalar, N> &a)
{ {
return (*this = *this / a); return (*this = *this / a);
} }
Dual<Scalar, N>& operator +=(Scalar a) Dual<Scalar, N> &operator +=(Scalar a)
{ {
return (*this = *this + a); return (*this = *this + a);
} }
Dual<Scalar, N>& operator -=(Scalar a) Dual<Scalar, N> &operator -=(Scalar a)
{ {
return (*this = *this - a); return (*this = *this - a);
} }
Dual<Scalar, N>& operator *=(Scalar a) Dual<Scalar, N> &operator *=(Scalar a)
{ {
return (*this = *this * a); return (*this = *this * a);
} }
Dual<Scalar, N>& operator /=(Scalar a) Dual<Scalar, N> &operator /=(Scalar a)
{ {
return (*this = *this / a); return (*this = *this / a);
} }
@@ -95,73 +95,73 @@ struct Dual
// operators // operators
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a) Dual<Scalar, N> operator+(const Dual<Scalar, N> &a)
{ {
return a; return a;
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a) Dual<Scalar, N> operator-(const Dual<Scalar, N> &a)
{ {
return Dual<Scalar, N>(-a.value, -a.derivative); return Dual<Scalar, N>(-a.value, -a.derivative);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator+(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
{ {
return Dual<Scalar, N>(a.value + b.value, a.derivative + b.derivative); return Dual<Scalar, N>(a.value + b.value, a.derivative + b.derivative);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator-(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
{ {
return a + (-b); return a + (-b);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, Scalar b) Dual<Scalar, N> operator+(const Dual<Scalar, N> &a, Scalar b)
{ {
return Dual<Scalar, N>(a.value + b, a.derivative); return Dual<Scalar, N>(a.value + b, a.derivative);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, Scalar b) Dual<Scalar, N> operator-(const Dual<Scalar, N> &a, Scalar b)
{ {
return a + (-b); return a + (-b);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(Scalar a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator+(Scalar a, const Dual<Scalar, N> &b)
{ {
return Dual<Scalar, N>(a + b.value, b.derivative); return Dual<Scalar, N>(a + b.value, b.derivative);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(Scalar a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator-(Scalar a, const Dual<Scalar, N> &b)
{ {
return a + (-b); return a + (-b);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator*(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
{ {
return Dual<Scalar, N>(a.value * b.value, a.value * b.derivative + b.value * a.derivative); return Dual<Scalar, N>(a.value * b.value, a.value * b.derivative + b.value * a.derivative);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, Scalar b) Dual<Scalar, N> operator*(const Dual<Scalar, N> &a, Scalar b)
{ {
return Dual<Scalar, N>(a.value * b, a.derivative * b); return Dual<Scalar, N>(a.value * b, a.derivative * b);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator*(Scalar a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator*(Scalar a, const Dual<Scalar, N> &b)
{ {
return b * a; return b * a;
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator/(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
{ {
const Scalar inv_b_real = Scalar(1) / b.value; const Scalar inv_b_real = Scalar(1) / b.value;
return Dual<Scalar, N>(a.value * inv_b_real, a.derivative * inv_b_real - return Dual<Scalar, N>(a.value * inv_b_real, a.derivative * inv_b_real -
@@ -169,13 +169,13 @@ Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, Scalar b) Dual<Scalar, N> operator/(const Dual<Scalar, N> &a, Scalar b)
{ {
return a * (Scalar(1) / b); return a * (Scalar(1) / b);
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N>& b) Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N> &b)
{ {
const Scalar inv_b_real = Scalar(1) / b.value; const Scalar inv_b_real = Scalar(1) / b.value;
return Dual<Scalar, N>(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative); return Dual<Scalar, N>(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative);
@@ -185,7 +185,7 @@ Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N>& b)
// sqrt // sqrt
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> sqrt(const Dual<Scalar, N>& a) Dual<Scalar, N> sqrt(const Dual<Scalar, N> &a)
{ {
Scalar real = sqrt(a.value); Scalar real = sqrt(a.value);
return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real))); return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real)));
@@ -193,42 +193,42 @@ Dual<Scalar, N> sqrt(const Dual<Scalar, N>& a)
// abs // abs
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> abs(const Dual<Scalar, N>& a) Dual<Scalar, N> abs(const Dual<Scalar, N> &a)
{ {
return a.value >= Scalar(0) ? a : -a; return a.value >= Scalar(0) ? a : -a;
} }
// ceil // ceil
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> ceil(const Dual<Scalar, N>& a) Dual<Scalar, N> ceil(const Dual<Scalar, N> &a)
{ {
return Dual<Scalar, N>(ceil(a.value)); return Dual<Scalar, N>(ceil(a.value));
} }
// floor // floor
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> floor(const Dual<Scalar, N>& a) Dual<Scalar, N> floor(const Dual<Scalar, N> &a)
{ {
return Dual<Scalar, N>(floor(a.value)); return Dual<Scalar, N>(floor(a.value));
} }
// fmod // fmod
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> fmod(const Dual<Scalar, N>& a, Scalar mod) Dual<Scalar, N> fmod(const Dual<Scalar, N> &a, Scalar mod)
{ {
return Dual<Scalar, N>(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative); return Dual<Scalar, N>(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative);
} }
// max // max
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> max(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b) Dual<Scalar, N> max(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
{ {
return a.value >= b.value ? a : b; return a.value >= b.value ? a : b;
} }
// min // min
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> min(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b) Dual<Scalar, N> min(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
{ {
return a.value < b.value ? a : b; return a.value < b.value ? a : b;
} }
@@ -241,7 +241,7 @@ bool IsNan(Scalar a)
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
bool IsNan(const Dual<Scalar, N>& a) bool IsNan(const Dual<Scalar, N> &a)
{ {
return IsNan(a.value); return IsNan(a.value);
} }
@@ -254,7 +254,7 @@ bool IsFinite(Scalar a)
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
bool IsFinite(const Dual<Scalar, N>& a) bool IsFinite(const Dual<Scalar, N> &a)
{ {
return IsFinite(a.value); return IsFinite(a.value);
} }
@@ -267,7 +267,7 @@ bool IsInf(Scalar a)
} }
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
bool IsInf(const Dual<Scalar, N>& a) bool IsInf(const Dual<Scalar, N> &a)
{ {
return IsInf(a.value); return IsInf(a.value);
} }
@@ -276,21 +276,21 @@ bool IsInf(const Dual<Scalar, N>& a)
// sin // sin
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> sin(const Dual<Scalar, N>& a) Dual<Scalar, N> sin(const Dual<Scalar, N> &a)
{ {
return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative); return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative);
} }
// cos // cos
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> cos(const Dual<Scalar, N>& a) Dual<Scalar, N> cos(const Dual<Scalar, N> &a)
{ {
return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative); return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative);
} }
// tan // tan
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> tan(const Dual<Scalar, N>& a) Dual<Scalar, N> tan(const Dual<Scalar, N> &a)
{ {
Scalar real = tan(a.value); Scalar real = tan(a.value);
return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative); return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative);
@@ -298,7 +298,7 @@ Dual<Scalar, N> tan(const Dual<Scalar, N>& a)
// asin // asin
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> asin(const Dual<Scalar, N>& a) Dual<Scalar, N> asin(const Dual<Scalar, N> &a)
{ {
Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value); Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative); return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
@@ -306,7 +306,7 @@ Dual<Scalar, N> asin(const Dual<Scalar, N>& a)
// acos // acos
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> acos(const Dual<Scalar, N>& a) Dual<Scalar, N> acos(const Dual<Scalar, N> &a)
{ {
Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value); Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative); return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
@@ -314,7 +314,7 @@ Dual<Scalar, N> acos(const Dual<Scalar, N>& a)
// atan // atan
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> atan(const Dual<Scalar, N>& a) Dual<Scalar, N> atan(const Dual<Scalar, N> &a)
{ {
Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value); Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value);
return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative); return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative);
@@ -322,7 +322,7 @@ Dual<Scalar, N> atan(const Dual<Scalar, N>& a)
// atan2 // atan2
template <typename Scalar, size_t N> template <typename Scalar, size_t N>
Dual<Scalar, N> atan2(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b) Dual<Scalar, N> atan2(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
{ {
// derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify // derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify
Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value); Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value);
@@ -331,39 +331,45 @@ Dual<Scalar, N> atan2(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
// retrieve the derivative elements of a vector of Duals into a matrix // retrieve the derivative elements of a vector of Duals into a matrix
template <typename Scalar, size_t M, size_t N> 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;
} }
// retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix // retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix
template <typename Scalar, size_t M, size_t N, size_t D> 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;
} }
#if defined(SUPPORT_STDIOSTREAM) #if defined(SUPPORT_STDIOSTREAM)
template<typename Type, size_t N> template<typename Type, size_t N>
std::ostream& operator<<(std::ostream& os, std::ostream &operator<<(std::ostream &os,
const matrix::Dual<Type, N>& dual) const matrix::Dual<Type, N> &dual)
{ {
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;
} }
src/lib/matrix/matrix/Euler.hpp
-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 : */
src/lib/matrix/matrix/LeastSquaresSolver.hpp
+45 -27
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,33 +42,41 @@ 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++) {
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++) { for (size_t i = j + 1; i < M; i++) {
Type tmp = Type(0); w[i - j] = _A(i, j) / u1;
for (size_t i = j; i < M; i++) { _A(i, j) = w[i - j];
tmp += w[i-j] * _A(i,k);
} }
_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++) { for (size_t i = j; i < M; i++) {
_A(i,k) -= _tau(j) * w[i-j] * tmp; 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,25 +91,30 @@ 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);
} }
for (size_t i = j; i < M; i++) { for (size_t i = j; i < M; i++) {
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++) {
x(i) -= _A(i,r) * x(r); for (size_t r = i + 1; r < N; 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 : */
src/lib/matrix/matrix/Matrix.hpp
+136 -77
View File
File diff suppressed because it is too large Load Diff
src/lib/matrix/matrix/PseudoInverse.hpp
+14 -6
View File
@@ -21,19 +21,22 @@ namespace matrix
* Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809 * Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
*/ */
template<typename Type, size_t M, size_t N> 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;
} }
@@ -69,8 +75,8 @@ float typeEpsilon<float>()
* Full rank Cholesky factorization of A * Full rank Cholesky factorization of A
*/ */
template<typename Type, size_t N> template<typename Type, size_t N>
SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A, SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> &A,
size_t& rank) size_t &rank)
{ {
// Loses one ulp accuracy per row of diag, relative to largest magnitude // Loses one ulp accuracy per row of diag, relative to largest magnitude
const Type tol = N * typeEpsilon<Type>() * A.diag().max(); const Type tol = N * typeEpsilon<Type>() * A.diag().max();
@@ -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 : */
src/lib/matrix/matrix/Quaternion.hpp
+52 -34
View File
@@ -100,34 +100,38 @@ 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;
t = Type(0.5) / t; t = Type(0.5) / t;
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)) {
t = sqrt(Type(1) + R(0,0) - R(1,1) - 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));
q(1) = Type(0.5) * t; q(1) = Type(0.5) * t;
t = Type(0.5) / t; t = Type(0.5) / t;
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)) {
t = sqrt(Type(1) - R(0,0) + 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));
q(2) = Type(0.5) * t; q(2) = Type(0.5) * t;
t = Type(0.5) / t; t = Type(0.5) / t;
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;
t = Type(0.5) / t; t = Type(0.5) / t;
q(0) = (R(1,0) - R(0,1)) * t; q(0) = (R(1, 0) - R(0, 1)) * t;
q(1) = (R(0,2) + R(2,0)) * t; q(1) = (R(0, 2) + R(2, 0)) * t;
q(2) = (R(2,1) + R(1,2)) * t; q(2) = (R(2, 1) + R(1, 2)) * 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);
@@ -255,10 +269,10 @@ public:
{ {
const Quaternion &q = *this; const Quaternion &q = *this;
return { return {
q(0) * p(0) - q(1) * p(1) - q(2) * p(2) - q(3) * p(3), q(0) *p(0) - q(1) *p(1) - q(2) *p(2) - q(3) *p(3),
q(1) * p(0) + q(0) * p(1) - q(3) * p(2) + q(2) * p(3), q(1) *p(0) + q(0) *p(1) - q(3) *p(2) + q(2) *p(3),
q(2) * p(0) + q(3) * p(1) + q(0) * p(2) - q(1) * p(3), q(2) *p(0) + q(3) *p(1) + q(0) *p(2) - q(1) *p(3),
q(3) * p(0) - q(2) * p(1) + q(1) * p(2) + q(0) * p(3) }; q(3) *p(0) - q(2) *p(1) + q(1) *p(2) + q(0) *p(3) };
} }
/** /**
@@ -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));
} }
@@ -384,7 +400,7 @@ public:
* *
* @param u 3D vector u * @param u 3D vector u
*/ */
static Dcm<Type> inv_r_jacobian (const Vector3<Type> &u) static Dcm<Type> inv_r_jacobian(const Vector3<Type> &u)
{ {
const Type tol = Type(1.0e-4); const Type tol = Type(1.0e-4);
Type u_norm = u.norm(); Type u_norm = u.norm();
@@ -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);
} }
} }
@@ -415,10 +433,10 @@ public:
const Quaternion &q = *this; const Quaternion &q = *this;
Type normSq = q.dot(q); Type normSq = q.dot(q);
return Quaternion( return Quaternion(
q(0)/normSq, q(0) / normSq,
-q(1)/normSq, -q(1) / normSq,
-q(2)/normSq, -q(2) / normSq,
-q(3)/normSq); -q(3) / normSq);
} }
/** /**
@@ -443,6 +461,7 @@ public:
return q * Type(matrix::sign(q(i))); return q * Type(matrix::sign(q(i)));
} }
} }
return q; return q;
} }
@@ -466,10 +485,11 @@ 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();
return Vector3<Type>(res(1), res(2), res(3)); return Vector3<Type>(res(1), res(2), res(3));
} }
@@ -484,9 +504,9 @@ public:
*/ */
Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const Vector3<Type> conjugate_inversed(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.inversed()*v*q; Quaternion res = q.inversed() * v * q;
return Vector3<Type>(res(1), res(2), res(3)); return Vector3<Type>(res(1), res(2), res(3));
} }
@@ -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 : */
src/lib/matrix/matrix/Scalar.hpp
+6 -6
View File
@@ -19,8 +19,8 @@ class Scalar
public: public:
Scalar() = delete; Scalar() = delete;
Scalar(const Matrix<Type, 1, 1> & other) : Scalar(const Matrix<Type, 1, 1> &other) :
_value{other(0,0)} _value{other(0, 0)}
{ {
} }
@@ -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 : */
src/lib/matrix/matrix/Slice.hpp
+79 -48
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);
@@ -50,149 +53,173 @@ public:
} }
template<size_t MM, size_t NN> template<size_t MM, size_t NN>
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;
} }
// allow assigning vectors to a slice that are in the axis // allow assigning vectors to a slice that are in the axis
template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
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;
} }
template<size_t MM, size_t NN> template<size_t MM, size_t NN>
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;
} }
template<size_t MM, size_t NN> template<size_t MM, size_t NN>
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;
} }
Slice<Type, P, Q, M, N>& operator/=(const Type& other) Slice<Type, P, Q, M, N> &operator/=(const Type &other)
{ {
return operator*=(Type(1) / other); return operator*=(Type(1) / other);
} }
Matrix<Type, P, Q> operator*(const Type& other) const Matrix<Type, P, Q> operator*(const Type &other) const
{ {
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;
} }
Matrix<Type, P, Q> operator/(const Type& other) const Matrix<Type, P, Q> operator/(const Type &other) const
{ {
const Slice<Type, P, Q, M, N>& self = *this; const Slice<Type, P, Q, M, N> &self = *this;
return self * (Type(1) / other); return self * (Type(1) / other);
} }
@@ -208,47 +235,51 @@ public:
return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data); return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
} }
void copyTo(Type dst[P*Q]) const void copyTo(Type dst[P * Q]) const
{ {
const Slice<Type, P, Q, M, N> &self = *this; const 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++) {
dst[i*N+j] = self(i, j); dst[i * N + j] = self(i, j);
} }
} }
} }
void copyToColumnMajor(Type dst[P*Q]) const void copyToColumnMajor(Type dst[P * Q]) const
{ {
const Slice<Type, P, Q, M, N> &self = *this; const 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++) {
dst[i+(j*M)] = self(i, j); dst[i + (j * M)] = self(i, j);
} }
} }
} }
Vector<Type, P<Q?P:Q> diag() const Vector < Type, P < Q ? P : Q > diag() const
{ {
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++) {
res(j) = self(j,j); for (size_t j = 0; j < (P < Q ? P : Q); j++) {
res(j) = self(j, j);
} }
return res; return res;
} }
Type norm_squared() const Type norm_squared() const
{ {
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;
} }
@@ -259,16 +290,16 @@ public:
bool longerThan(Type testVal) const bool longerThan(Type testVal) const
{ {
return norm_squared() > testVal*testVal; return norm_squared() > testVal * testVal;
} }
Type max() const Type max() const
{ {
Type max_val = (*this)(0,0); Type max_val = (*this)(0, 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++) {
Type val = (*this)(i,j); Type val = (*this)(i, j);
if (val > max_val) { if (val > max_val) {
max_val = val; max_val = val;
@@ -281,11 +312,11 @@ public:
Type min() const Type min() const
{ {
Type min_val = (*this)(0,0); Type min_val = (*this)(0, 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++) {
Type val = (*this)(i,j); Type val = (*this)(i, j);
if (val < min_val) { if (val < min_val) {
min_val = val; min_val = val;
@@ -298,7 +329,7 @@ public:
private: private:
size_t _x0, _y0; size_t _x0, _y0;
Matrix<Type,M,N>* _data; Matrix<Type, M, N> *_data;
}; };
} }
src/lib/matrix/matrix/SparseVector.hpp
+60 -22
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;
} }
@@ -152,35 +181,44 @@ public:
bool longerThan(Type testVal) const bool longerThan(Type testVal) const
{ {
return norm_squared() > testVal*testVal; return norm_squared() > testVal * testVal;
} }
}; };
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;
} }
template<typename Type,size_t M, size_t... Idxs> template<typename Type, size_t M, size_t... Idxs>
constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N]; constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N];
template<size_t M, size_t ... Idxs> template<size_t M, size_t ... Idxs>
src/lib/matrix/matrix/SquareMatrix.hpp
+82 -52
View File
@@ -33,7 +33,7 @@ public:
{ {
} }
explicit SquareMatrix(const Type data_[M*M]) : explicit SquareMatrix(const Type data_[M * M]) :
Matrix<Type, M, M>(data_) Matrix<Type, M, M>(data_)
{ {
} }
@@ -44,18 +44,18 @@ public:
} }
template<size_t P, size_t Q> template<size_t P, size_t Q>
SquareMatrix(const Slice<Type, M, M, P, Q>& in_slice) : Matrix<Type, M, M>(in_slice) SquareMatrix(const Slice<Type, M, M, P, Q> &in_slice) : Matrix<Type, M, M>(in_slice)
{ {
} }
SquareMatrix<Type, M>& operator=(const Matrix<Type, M, M>& other) SquareMatrix<Type, M> &operator=(const Matrix<Type, M, M> &other)
{ {
Matrix<Type, M, M>::operator=(other); Matrix<Type, M, M>::operator=(other);
return *this; return *this;
} }
template <size_t P, size_t Q> template <size_t P, size_t Q>
SquareMatrix<Type, M> & operator=(const Slice<Type, M, M, P, Q>& in_slice) SquareMatrix<Type, M> &operator=(const Slice<Type, M, M, P, Q> &in_slice)
{ {
Matrix<Type, M, M>::operator=(in_slice); Matrix<Type, M, M>::operator=(in_slice);
return *this; return *this;
@@ -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,16 +102,18 @@ 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;
} }
// get matrix upper right triangle in a row-major vector format // get matrix upper right triangle in a row-major vector format
Vector<Type, M * (M + 1) / 2> upper_right_triangle() const Vector < Type, M *(M + 1) / 2 > upper_right_triangle() const
{ {
Vector<Type, M * (M + 1) / 2> res; Vector < Type, M * (M + 1) / 2 > res;
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,8 +161,9 @@ public:
// set diagonals // set diagonals
unsigned vec_idx = 0; unsigned vec_idx = 0;
for (size_t idx = first; idx < first+Width; idx++) {
self(idx,idx) = vec(vec_idx); for (size_t idx = first; idx < first + Width; idx++) {
self(idx, idx) = vec(vec_idx);
vec_idx ++; vec_idx ++;
} }
} }
@@ -174,8 +180,8 @@ public:
self.slice<M, Width>(0, first) = Type(0); self.slice<M, Width>(0, first) = Type(0);
// set diagonals // set diagonals
for (size_t idx = first; idx < first+Width; idx++) { for (size_t idx = first; idx < first + Width; idx++) {
self(idx,idx) = val; self(idx, idx) = val;
} }
} }
@@ -187,12 +193,13 @@ public:
assert(first + Width <= M); assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this; SquareMatrix<Type, M> &self = *this;
if(Width>1) {
for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) { 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++) { for (size_t col_idx = first; col_idx < row_idx; 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;
} }
} }
} }
@@ -207,16 +214,18 @@ 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++) {
Type tmp = (self(row_idx,col_idx) + self(col_idx,row_idx)) / Type(2); for (size_t col_idx = first + Width; col_idx < M; col_idx++) {
self(row_idx,col_idx) = tmp; Type tmp = (self(row_idx, col_idx) + self(col_idx, row_idx)) / Type(2);
self(col_idx,row_idx) = tmp; self(row_idx, col_idx) = tmp;
self(col_idx, row_idx) = tmp;
} }
} }
} }
@@ -229,15 +238,17 @@ public:
assert(first + Width <= M); assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this; SquareMatrix<Type, M> &self = *this;
if(Width>1) {
for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) { 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++) { for (size_t col_idx = first; col_idx < row_idx; 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 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++) {
if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) { 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 false;
} }
} }
} }
return self.isBlockSymmetric<Width>(first, eps); return self.isBlockSymmetric<Width>(first, eps);
} }
@@ -270,29 +284,34 @@ 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++) {
m(i,i) = d(i); for (size_t i = 0; i < M; i++) {
m(i, i) = d(i);
} }
return m; return m;
} }
template<typename Type, size_t M> template<typename Type, size_t M>
SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5) SquareMatrix<Type, M> expm(const Matrix<Type, M, M> &A, size_t order = 5)
{ {
SquareMatrix<Type, M> res; SquareMatrix<Type, M> res;
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);
A_pow *= A_pow; A_pow *= A_pow;
@@ -306,7 +325,7 @@ SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5)
* inverse based on LU factorization with partial pivotting * inverse based on LU factorization with partial pivotting
*/ */
template<typename Type, size_t M> template<typename Type, size_t M>
bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv, size_t rank = M) bool inv(const SquareMatrix<Type, M> &A, SquareMatrix<Type, M> &inv, size_t rank = M)
{ {
SquareMatrix<Type, M> L; SquareMatrix<Type, M> L;
L.setIdentity(); L.setIdentity();
@@ -419,22 +438,23 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv, size_t ra
//check sanity of results //check sanity of results
for (size_t i = 0; i < rank; i++) { for (size_t i = 0; i < rank; i++) {
for (size_t j = 0; j < rank; j++) { for (size_t j = 0; j < rank; j++) {
if (!is_finite(P(i,j))) { if (!is_finite(P(i, j))) {
return false; return false;
} }
} }
} }
//printf("X:\n"); X.print(); //printf("X:\n"); X.print();
inv = P; inv = P;
return true; return true;
} }
template<typename Type> template<typename Type>
bool inv(const SquareMatrix<Type, 2> & A, SquareMatrix<Type, 2> & inv) bool inv(const SquareMatrix<Type, 2> &A, SquareMatrix<Type, 2> &inv)
{ {
Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1); Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1);
if(fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) { if (fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) {
return false; return false;
} }
@@ -447,13 +467,13 @@ bool inv(const SquareMatrix<Type, 2> & A, SquareMatrix<Type, 2> & inv)
} }
template<typename Type> template<typename Type>
bool inv(const SquareMatrix<Type, 3> & A, SquareMatrix<Type, 3> & inv) bool inv(const SquareMatrix<Type, 3> &A, SquareMatrix<Type, 3> &inv)
{ {
Type det = A(0, 0) * (A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2)) - Type det = A(0, 0) * (A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2)) -
A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) + A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) +
A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0)); A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0));
if(fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) { if (fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) {
return false; return false;
} }
@@ -474,11 +494,13 @@ bool inv(const SquareMatrix<Type, 3> & A, SquareMatrix<Type, 3> & inv)
* inverse based on LU factorization with partial pivotting * inverse based on LU factorization with partial pivotting
*/ */
template<typename Type, size_t M> 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;
@@ -491,35 +513,45 @@ SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> & A)
* Note: A must be positive definite * Note: A must be positive definite
*/ */
template<typename Type, size_t M> 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;
} }
@@ -530,15 +562,13 @@ SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> & A)
* for L or we need to do it manually. Will impact speed otherwise. * for L or we need to do it manually. Will impact speed otherwise.
*/ */
template<typename Type, size_t M> template<typename Type, size_t M>
SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> & A) SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
{ {
SquareMatrix<Type, M> L_inv = inv(cholesky(A)); SquareMatrix<Type, M> L_inv = inv(cholesky(A));
return L_inv.T()*L_inv; return L_inv.T() * L_inv;
} }
using Matrix3f = SquareMatrix<float, 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 : */
src/lib/matrix/matrix/Vector.hpp
+39 -22
View File
@@ -24,7 +24,7 @@ public:
Vector() = default; Vector() = default;
Vector(const MatrixM1 & other) : Vector(const MatrixM1 &other) :
MatrixM1(other) MatrixM1(other)
{ {
} }
@@ -35,16 +35,17 @@ public:
} }
template<size_t P, size_t Q> template<size_t P, size_t Q>
Vector(const Slice<Type, M, 1, P, Q>& slice_in) : Vector(const Slice<Type, M, 1, P, Q> &slice_in) :
Matrix<Type, M, 1>(slice_in) Matrix<Type, M, 1>(slice_in)
{ {
} }
template<size_t P, size_t Q, size_t DUMMY = 1> template<size_t P, size_t Q, size_t DUMMY = 1>
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++) {
r += a(i)*b(i,0); for (size_t i = 0; i < M; i++) {
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 : */
src/lib/matrix/matrix/Vector2.hpp
+9 -9
View File
@@ -26,7 +26,7 @@ public:
Vector2() = default; Vector2() = default;
Vector2(const Matrix21 & other) : Vector2(const Matrix21 &other) :
Vector<Type, 2>(other) Vector<Type, 2>(other)
{ {
} }
@@ -44,28 +44,30 @@ public:
} }
template<size_t P, size_t Q> template<size_t P, size_t Q>
Vector2(const Slice<Type, 2, 1, P, Q>& slice_in) : Vector<Type, 2>(slice_in) Vector2(const Slice<Type, 2, 1, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
{ {
} }
template<size_t P, size_t Q> template<size_t P, size_t Q>
Vector2(const Slice<Type, 1, 2, P, Q>& slice_in) : Vector<Type, 2>(slice_in) Vector2(const Slice<Type, 1, 2, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
{ {
} }
explicit Vector2(const Vector3 & other) explicit Vector2(const Vector3 &other)
{ {
Vector2 &v(*this); Vector2 &v(*this);
v(0) = other(0); v(0) = other(0);
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 : */
src/lib/matrix/matrix/Vector3.hpp
+25 -21
View File
@@ -34,7 +34,7 @@ public:
Vector3() = default; Vector3() = default;
Vector3(const Matrix31 & other) : Vector3(const Matrix31 &other) :
Vector<Type, 3>(other) Vector<Type, 3>(other)
{ {
} }
@@ -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;
@@ -52,18 +53,19 @@ public:
} }
template<size_t P, size_t Q> template<size_t P, size_t Q>
Vector3(const Slice<Type, 3, 1, P, Q>& slice_in) : Vector<Type, 3>(slice_in) Vector3(const Slice<Type, 3, 1, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
{ {
} }
template<size_t P, size_t Q> template<size_t P, size_t Q>
Vector3(const Slice<Type, 1, 3, P, Q>& slice_in) : Vector<Type, 3>(slice_in) Vector3(const Slice<Type, 1, 3, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
{ {
} }
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,18 +136,19 @@ 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;
A(0,1) = -v(2); A(0, 1) = -v(2);
A(0,2) = v(1); A(0, 2) = v(1);
A(1,0) = v(2); A(1, 0) = v(2);
A(1,1) = 0; A(1, 1) = 0;
A(1,2) = -v(0); A(1, 2) = -v(0);
A(2,0) = -v(1); A(2, 0) = -v(1);
A(2,1) = v(0); A(2, 1) = v(0);
A(2,2) = 0; A(2, 2) = 0;
return A; return A;
} }
@@ -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 : */
src/lib/matrix/matrix/filter.hpp
+13 -12
View File
@@ -2,24 +2,25 @@
#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(
const Matrix<Type, M, M> & P, const Matrix<Type, M, M> &P,
const Matrix<Type, N, M> & C, const Matrix<Type, N, M> &C,
const Matrix<Type, N, N> & R, const Matrix<Type, N, N> &R,
const Matrix<Type, N, 1> &r, const Matrix<Type, N, 1> &r,
Matrix<Type, M, 1> & dx, Matrix<Type, M, 1> &dx,
Matrix<Type, M, M> & dP, Matrix<Type, M, M> &dP,
Type & beta Type &beta
) )
{ {
SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I(); SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C * P * C.T() + R).I();
Matrix<Type, M, N> K = P*C.T()*S_I; Matrix<Type, M, N> K = P * C.T() * S_I;
dx = K*r; dx = K * r;
beta = Scalar<Type>(r.T()*S_I*r); beta = Scalar<Type>(r.T() * S_I * r);
dP = K*C*P*(-1); dP = K * C * P * (-1);
return 0; return 0;
} }
src/lib/matrix/matrix/helper_functions.hpp
+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) {
src/lib/matrix/matrix/integration.hpp
+16 -12
View File
@@ -2,17 +2,18 @@
#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(
Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> & u), Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> &u),
const Matrix<Type, M, 1> & y0, const Matrix<Type, M, 1> &y0,
const Matrix<Type, N, 1> & u, const Matrix<Type, N, 1> &u,
Type t0, Type t0,
Type tf, Type tf,
Type h0, Type h0,
Matrix<Type, M, 1> & y1 Matrix<Type, M, 1> &y1
) )
{ {
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
@@ -20,23 +21,26 @@ 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);
k4 = f(t1 + h, y1 + k3*h, u); k4 = f(t1 + h, y1 + k3 * h, u);
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 :
src/lib/matrix/matrix/stdlib_imports.hpp
+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__
src/lib/matrix/test/attitude.cpp
+41 -40
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>;
@@ -55,7 +56,7 @@ int main()
// quaternion ctor: vector to vector // quaternion ctor: vector to vector
// identity test // identity test
Quatf quat_v(v,v); Quatf quat_v(v, v);
TEST(isEqual(quat_v.conjugate(v), v)); TEST(isEqual(quat_v.conjugate(v), v));
// random test (vector norm can not be preserved with a pure rotation) // random test (vector norm can not be preserved with a pure rotation)
Vector3f v1(-80.1f, 1.5f, -6.89f); Vector3f v1(-80.1f, 1.5f, -6.89f);
@@ -92,12 +93,12 @@ int main()
// quaternion exponential with v=0 // quaternion exponential with v=0
v = Vector3f(); v = Vector3f();
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
Dcmf M = Dcmf()*0.5f; Dcmf M = Dcmf() * 0.5f;
TEST(isEqual(q, Quatf::expq(v))); TEST(isEqual(q, Quatf::expq(v)));
TEST(isEqual(M, Quatf::inv_r_jacobian(v))); TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
// quaternion exponential with small v // quaternion exponential with small v
v = Vector3f(0.001f,0.002f,-0.003f); v = Vector3f(0.001f, 0.002f, -0.003f);
q = Quatf(0.999993000008167f, 0.000999997666668f, q = Quatf(0.999993000008167f, 0.000999997666668f,
0.001999995333337f, -0.002999993000005f); 0.001999995333337f, -0.002999993000005f);
{ {
@@ -128,11 +129,11 @@ int main()
// quaternion kinematic update // quaternion kinematic update
q = Quatf(); q = Quatf();
float h=0.001f; // sampling time [s] float h = 0.001f; // sampling time [s]
Vector3f w_B=Vector3f(0.1f,0.2f,0.3f); // body rate in body frame Vector3f w_B = Vector3f(0.1f, 0.2f, 0.3f); // body rate in body frame
Quatf qa=q+0.5f*h*q.derivative1(w_B); Quatf qa = q + 0.5f * h * q.derivative1(w_B);
qa.normalize(); qa.normalize();
Quatf qb=q*Quatf::expq(0.5f*h*w_B); Quatf qb = q * Quatf::expq(0.5f * h * w_B);
TEST(isEqual(qa, qb)); TEST(isEqual(qa, qb));
// euler to quaternion // euler to quaternion
@@ -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;
} }
@@ -179,9 +181,10 @@ int main()
float err = 0.0f; float err = 0.0f;
for (size_t r = 0; r < 3; r++) { for (size_t r = 0; r < 3; r++) {
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
@@ -196,7 +199,7 @@ int main()
double roll_expected = roll; double roll_expected = roll;
double yaw_expected = yaw; double yaw_expected = yaw;
if (fabs(pitch -90) < eps) { if (fabs(pitch - 90) < eps) {
roll_expected = 0; roll_expected = 0;
yaw_expected = yaw - roll; yaw_expected = yaw - roll;
@@ -301,55 +304,55 @@ int main()
TEST(fabs(q_check(3) + q(3)) < eps); TEST(fabs(q_check(3) + q(3)) < eps);
// quaternion canonical // quaternion canonical
Quatf q_non_canonical_1(-0.7f,0.4f, 0.3f, -0.3f); Quatf q_non_canonical_1(-0.7f, 0.4f, 0.3f, -0.3f);
Quatf q_canonical_1(0.7f,-0.4f, -0.3f, 0.3f); Quatf q_canonical_1(0.7f, -0.4f, -0.3f, 0.3f);
Quatf q_canonical_ref_1(0.7f,-0.4f, -0.3f, 0.3f); Quatf q_canonical_ref_1(0.7f, -0.4f, -0.3f, 0.3f);
TEST(isEqual(q_non_canonical_1.canonical(),q_canonical_ref_1)); TEST(isEqual(q_non_canonical_1.canonical(), q_canonical_ref_1));
TEST(isEqual(q_canonical_1.canonical(),q_canonical_ref_1)); TEST(isEqual(q_canonical_1.canonical(), q_canonical_ref_1));
q_non_canonical_1.canonicalize(); q_non_canonical_1.canonicalize();
q_canonical_1.canonicalize(); q_canonical_1.canonicalize();
TEST(isEqual(q_non_canonical_1,q_canonical_ref_1)); TEST(isEqual(q_non_canonical_1, q_canonical_ref_1));
TEST(isEqual(q_canonical_1,q_canonical_ref_1)); TEST(isEqual(q_canonical_1, q_canonical_ref_1));
Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f); Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f);
Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f); Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f);
Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f); Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f);
TEST(isEqual(q_non_canonical_2.canonical(),q_canonical_ref_2)); TEST(isEqual(q_non_canonical_2.canonical(), q_canonical_ref_2));
TEST(isEqual(q_canonical_2.canonical(),q_canonical_ref_2)); TEST(isEqual(q_canonical_2.canonical(), q_canonical_ref_2));
q_non_canonical_2.canonicalize(); q_non_canonical_2.canonicalize();
q_canonical_2.canonicalize(); q_canonical_2.canonicalize();
TEST(isEqual(q_non_canonical_2,q_canonical_ref_2)); TEST(isEqual(q_non_canonical_2, q_canonical_ref_2));
TEST(isEqual(q_canonical_2,q_canonical_ref_2)); TEST(isEqual(q_canonical_2, q_canonical_ref_2));
Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f); Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f);
Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f); Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f);
Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f); Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f);
TEST(isEqual(q_non_canonical_3.canonical(),q_canonical_ref_3)); TEST(isEqual(q_non_canonical_3.canonical(), q_canonical_ref_3));
TEST(isEqual(q_canonical_3.canonical(),q_canonical_ref_3)); TEST(isEqual(q_canonical_3.canonical(), q_canonical_ref_3));
q_non_canonical_3.canonicalize(); q_non_canonical_3.canonicalize();
q_canonical_3.canonicalize(); q_canonical_3.canonicalize();
TEST(isEqual(q_non_canonical_3,q_canonical_ref_3)); TEST(isEqual(q_non_canonical_3, q_canonical_ref_3));
TEST(isEqual(q_canonical_3,q_canonical_ref_3)); TEST(isEqual(q_canonical_3, q_canonical_ref_3));
Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f); Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f);
Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f); Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f);
Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f); Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f);
TEST(isEqual(q_non_canonical_4.canonical(),q_canonical_ref_4)); TEST(isEqual(q_non_canonical_4.canonical(), q_canonical_ref_4));
TEST(isEqual(q_canonical_4.canonical(),q_canonical_ref_4)); TEST(isEqual(q_canonical_4.canonical(), q_canonical_ref_4));
q_non_canonical_4.canonicalize(); q_non_canonical_4.canonicalize();
q_canonical_4.canonicalize(); q_canonical_4.canonicalize();
TEST(isEqual(q_non_canonical_4,q_canonical_ref_4)); TEST(isEqual(q_non_canonical_4, q_canonical_ref_4));
TEST(isEqual(q_canonical_4,q_canonical_ref_4)); TEST(isEqual(q_canonical_4, q_canonical_ref_4));
Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f); Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f); Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f); Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f);
TEST(isEqual(q_non_canonical_5.canonical(),q_canonical_ref_5)); TEST(isEqual(q_non_canonical_5.canonical(), q_canonical_ref_5));
TEST(isEqual(q_canonical_5.canonical(),q_canonical_ref_5)); TEST(isEqual(q_canonical_5.canonical(), q_canonical_ref_5));
q_non_canonical_5.canonicalize(); q_non_canonical_5.canonicalize();
q_canonical_5.canonicalize(); q_canonical_5.canonicalize();
TEST(isEqual(q_non_canonical_5,q_canonical_ref_5)); TEST(isEqual(q_non_canonical_5, q_canonical_ref_5));
TEST(isEqual(q_canonical_5,q_canonical_ref_5)); TEST(isEqual(q_canonical_5, q_canonical_ref_5));
// quaternion setIdentity // quaternion setIdentity
Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f); Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f);
@@ -358,7 +361,7 @@ int main()
// non-unit quaternion invese // non-unit quaternion invese
Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f); Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
TEST(isEqual(q_nonunit*q_nonunit.inversed(), Quatf())); TEST(isEqual(q_nonunit * q_nonunit.inversed(), Quatf()));
// rotate quaternion (nonzero rotation) // rotate quaternion (nonzero rotation)
Vector3f rot(1.f, 0.f, 0.f); Vector3f rot(1.f, 0.f, 0.f);
@@ -402,7 +405,7 @@ int main()
TEST(isEqual(q, q_true)); TEST(isEqual(q, q_true));
// from axis angle, with length of vector the rotation // from axis angle, with length of vector the rotation
float n = float(sqrt(4*M_PI*M_PI/3)); float n = float(sqrt(4 * M_PI * M_PI / 3));
q = AxisAnglef(n, n, n); q = AxisAnglef(n, n, n);
TEST(isEqual(q, Quatf(-1, 0, 0, 0))); TEST(isEqual(q, Quatf(-1, 0, 0, 0)));
q = AxisAnglef(0, 0, 0); q = AxisAnglef(0, 0, 0);
@@ -471,13 +474,13 @@ int main()
TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34))); TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34)));
// check corner cases of matrix to quaternion conversion // check corner cases of matrix to quaternion conversion
q = Quatf(0,1,0,0); // 180 degree rotation around the x axis q = Quatf(0, 1, 0, 0); // 180 degree rotation around the x axis
R = Dcmf(q); R = Dcmf(q);
TEST(isEqual(q, Quatf(R))); TEST(isEqual(q, Quatf(R)));
q = Quatf(0,0,1,0); // 180 degree rotation around the y axis q = Quatf(0, 0, 1, 0); // 180 degree rotation around the y axis
R = Dcmf(q); R = Dcmf(q);
TEST(isEqual(q, Quatf(R))); TEST(isEqual(q, Quatf(R)));
q = Quatf(0,0,0,1); // 180 degree rotation around the z axis q = Quatf(0, 0, 0, 1); // 180 degree rotation around the z axis
R = Dcmf(q); R = Dcmf(q);
TEST(isEqual(q, Quatf(R))); TEST(isEqual(q, Quatf(R)));
@@ -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 : */
src/lib/matrix/test/copyto.cpp
+18 -12
View File
@@ -3,8 +3,9 @@
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,21 +28,23 @@ 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);
} }
// Matrix copyTo // Matrix copyTo
Matrix<float, 2, 3> A; Matrix<float, 2, 3> A;
A(0,0) = 1; A(0, 0) = 1;
A(0,1) = 2; A(0, 1) = 2;
A(0,2) = 3; A(0, 2) = 3;
A(1,0) = 4; A(1, 0) = 4;
A(1,1) = 5; A(1, 1) = 5;
A(1,2) = 6; A(1, 2) = 6;
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,20 +52,23 @@ 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);
} }
// 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);
} }
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 : */
src/lib/matrix/test/dual.cpp
+32 -31
View File
@@ -11,19 +11,20 @@ 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)
+ 3.f * point(1) * point(1) + 3.f * point(1) * point(1)
+ point(2); + point(2);
} }
template <typename Scalar> template <typename Scalar>
Vector<Scalar, 3> positionError(const Vector<Scalar, 3>& positionState, Vector<Scalar, 3> positionError(const Vector<Scalar, 3> &positionState,
const Vector<Scalar, 3>& velocityStateBody, const Vector<Scalar, 3> &velocityStateBody,
const Quaternion<Scalar>& bodyOrientation, const Quaternion<Scalar> &bodyOrientation,
const Vector<Scalar, 3>& positionMeasurement, const Vector<Scalar, 3> &positionMeasurement,
Scalar dt Scalar dt
) )
{ {
@@ -32,8 +33,8 @@ Vector<Scalar, 3> positionError(const Vector<Scalar, 3>& positionState,
int main() int main()
{ {
const Dual<float, 1> a(3,0); const Dual<float, 1> a(3, 0);
const Dual<float, 1> b(6,0); const Dual<float, 1> b(6, 0);
{ {
TEST(isEqualF(a.value, 3.f)); TEST(isEqualF(a.value, 3.f));
@@ -82,7 +83,7 @@ int main()
{ {
// multiplication // multiplication
Dual<float, 1> c = a*b; Dual<float, 1> c = a * b;
TEST(isEqualF(c.value, 18.f)); TEST(isEqualF(c.value, 18.f));
TEST(isEqualF(c.derivative(0), 9.f)); TEST(isEqualF(c.derivative(0), 9.f));
@@ -102,9 +103,9 @@ int main()
{ {
// division // division
Dual<float, 1> c = b/a; Dual<float, 1> c = b / a;
TEST(isEqualF(c.value, 2.f)); TEST(isEqualF(c.value, 2.f));
TEST(isEqualF(c.derivative(0), -1.f/3.f)); TEST(isEqualF(c.derivative(0), -1.f / 3.f));
Dual<float, 1> d = b; Dual<float, 1> d = b;
TEST(isEqualAll(d, b)); TEST(isEqualAll(d, b));
@@ -117,7 +118,7 @@ int main()
TEST(isEqual(e.derivative, b.derivative / a.value)); TEST(isEqual(e.derivative, b.derivative / a.value));
Dual<float, 1> f = a.value / b; Dual<float, 1> f = a.value / b;
TEST(isEqualAll(f, 1.f/e)); TEST(isEqualAll(f, 1.f / e));
} }
{ {
@@ -129,7 +130,7 @@ int main()
{ {
// sqrt // sqrt
TEST(isEqualF(sqrt(a).value, sqrt(a.value))); TEST(isEqualF(sqrt(a).value, sqrt(a.value)));
TEST(isEqualF(sqrt(a).derivative(0), 1.f/sqrt(12.f))); TEST(isEqualF(sqrt(a).derivative(0), 1.f / sqrt(12.f)));
} }
{ {
@@ -141,14 +142,14 @@ int main()
{ {
// ceil // ceil
Dual<float, 1> c(1.5,0); Dual<float, 1> c(1.5, 0);
TEST(isEqualF(ceil(c).value, ceil(c.value))); TEST(isEqualF(ceil(c).value, ceil(c.value)));
TEST(isEqualF(ceil(c).derivative(0), 0.f)); TEST(isEqualF(ceil(c).derivative(0), 0.f));
} }
{ {
// floor // floor
Dual<float, 1> c(1.5,0); Dual<float, 1> c(1.5, 0);
TEST(isEqualF(floor(c).value, floor(c.value))); TEST(isEqualF(floor(c).value, floor(c.value)));
TEST(isEqualF(floor(c).derivative(0), 0.f)); TEST(isEqualF(floor(c).derivative(0), 0.f));
} }
@@ -168,7 +169,7 @@ int main()
{ {
// isnan // isnan
TEST(!IsNan(a)); TEST(!IsNan(a));
Dual<float, 1> c(sqrt(-1.f),0); Dual<float, 1> c(sqrt(-1.f), 0);
TEST(IsNan(c)); TEST(IsNan(c));
} }
@@ -176,10 +177,10 @@ int main()
// isfinite/isinf // isfinite/isinf
TEST(IsFinite(a)); TEST(IsFinite(a));
TEST(!IsInf(a)); TEST(!IsInf(a));
Dual<float, 1> c(sqrt(-1.f),0); Dual<float, 1> c(sqrt(-1.f), 0);
TEST(!IsFinite(c)); TEST(!IsFinite(c));
TEST(!IsInf(c)); TEST(!IsInf(c));
Dual<float, 1> d(INFINITY,0); Dual<float, 1> d(INFINITY, 0);
TEST(!IsFinite(d)); TEST(!IsFinite(d));
TEST(IsInf(d)); TEST(IsInf(d));
} }
@@ -200,19 +201,19 @@ int main()
// asin/acos/atan // asin/acos/atan
Dual<float, 1> c(0.3f, 0); Dual<float, 1> c(0.3f, 0);
TEST(isEqualF(asin(c).value, asin(c.value))); TEST(isEqualF(asin(c).value, asin(c.value)));
TEST(isEqualF(asin(c).derivative(0), 1.f/sqrt(1.f - 0.3f*0.3f))); // asin'(x) = 1/sqrt(1-x^2) TEST(isEqualF(asin(c).derivative(0), 1.f / sqrt(1.f - 0.3f * 0.3f))); // asin'(x) = 1/sqrt(1-x^2)
TEST(isEqualF(acos(c).value, acos(c.value))); TEST(isEqualF(acos(c).value, acos(c.value)));
TEST(isEqualF(acos(c).derivative(0), -1.f/sqrt(1.f - 0.3f*0.3f))); // acos'(x) = -1/sqrt(1-x^2) TEST(isEqualF(acos(c).derivative(0), -1.f / sqrt(1.f - 0.3f * 0.3f))); // acos'(x) = -1/sqrt(1-x^2)
TEST(isEqualF(atan(c).value, atan(c.value))); TEST(isEqualF(atan(c).value, atan(c.value)));
TEST(isEqualF(atan(c).derivative(0), 1.f/(1.f + 0.3f*0.3f))); // tan'(x) = 1 + x^2 TEST(isEqualF(atan(c).derivative(0), 1.f / (1.f + 0.3f * 0.3f))); // tan'(x) = 1 + x^2
} }
{ {
// atan2 // atan2
TEST(isEqualF(atan2(a, b).value, atan2(a.value, b.value))); TEST(isEqualF(atan2(a, b).value, atan2(a.value, b.value)));
TEST(isEqualAll(atan2(a, Dual<float,1>(b.value)), atan(a/b.value))); // atan2'(y, x) = atan'(y/x) TEST(isEqualAll(atan2(a, Dual<float, 1>(b.value)), atan(a / b.value))); // atan2'(y, x) = atan'(y/x)
} }
{ {
@@ -239,8 +240,8 @@ int main()
floatPoint_plusDY(1) += h; floatPoint_plusDY(1) += h;
float floatResult_plusDY = testFunction(floatPoint_plusDY); float floatResult_plusDY = testFunction(floatPoint_plusDY);
Vector2f numerical_derivative((floatResult_plusDX - floatResult)/h, Vector2f numerical_derivative((floatResult_plusDX - floatResult) / h,
(floatResult_plusDY - floatResult)/h); (floatResult_plusDY - floatResult) / h);
TEST(isEqualF(dualResult.value, floatResult, 0.0f)); TEST(isEqualF(dualResult.value, floatResult, 0.0f));
TEST(isEqual(dualResult.derivative, numerical_derivative, 1e-2f)); TEST(isEqual(dualResult.derivative, numerical_derivative, 1e-2f));
@@ -254,9 +255,9 @@ int main()
Vector3f direct_error; Vector3f direct_error;
Matrix<float, 3, 4> numerical_jacobian; Matrix<float, 3, 4> numerical_jacobian;
{ {
Vector3f positionState(5,6,7); Vector3f positionState(5, 6, 7);
Vector3f velocityState(-1,0,1); Vector3f velocityState(-1, 0, 1);
Quaternionf velocityOrientation(0.2f,-0.1f,0,1); Quaternionf velocityOrientation(0.2f, -0.1f, 0, 1);
Vector3f positionMeasurement(4.5f, 6.2f, 7.9f); Vector3f positionMeasurement(4.5f, 6.2f, 7.9f);
float dt = 0.1f; float dt = 0.1f;
@@ -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,
@@ -275,7 +276,7 @@ int main()
h4, h4,
positionMeasurement, positionMeasurement,
dt) dt)
- direct_error)/h; - direct_error) / h;
} }
} }
Vector3f auto_error; Vector3f auto_error;
@@ -288,7 +289,7 @@ int main()
// request partial derivatives of velocity orientation // request partial derivatives of velocity orientation
// by setting these variables' derivatives in corresponding columns [0...3] // by setting these variables' derivatives in corresponding columns [0...3]
Quaternion<D4> velocityOrientation(D4(0.2f, 0),D4(-0.1f, 1),D4(0, 2),D4(1, 3)); Quaternion<D4> velocityOrientation(D4(0.2f, 0), D4(-0.1f, 1), D4(0, 2), D4(1, 3));
Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f)); Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f));
D4 dt(0.1f); D4 dt(0.1f);
src/lib/matrix/test/filter.cpp
+2 -3
View File
@@ -11,7 +11,7 @@ int main()
SquareMatrix<float, n_y> R = eye<float, n_y>(); SquareMatrix<float, n_y> R = eye<float, n_y>();
Matrix<float, n_y, n_x> C; Matrix<float, n_y, n_x> C;
C.setIdentity(); C.setIdentity();
float data[] = {1,2,3,4,5}; float data[] = {1, 2, 3, 4, 5};
Vector<float, n_y> r(data); Vector<float, n_y> r(data);
Vector<float, n_x> dx; Vector<float, n_x> dx;
@@ -19,11 +19,10 @@ int main()
float beta = 0; float beta = 0;
kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta); kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
float data_check[] = {0.5,1,1.5,2,2.5,0}; float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0};
Vector<float, n_x> dx_check(data_check); Vector<float, n_x> dx_check(data_check);
TEST(isEqual(dx, dx_check)); TEST(isEqual(dx, dx_check));
return 0; 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; 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; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/integration.cpp
+3 -3
View File
@@ -5,9 +5,10 @@ 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>();
} }
int main() int main()
@@ -23,4 +24,3 @@ int main()
return 0; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/inverse.cpp
+2 -2
View File
@@ -115,9 +115,10 @@ int main()
TEST(isEqual(A3_I, Z3)); TEST(isEqual(A3_I, Z3));
TEST(isEqual(A3.I(), Z3)); TEST(isEqual(A3.I(), Z3));
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 : */
src/lib/matrix/test/least_squares.cpp
+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 : */
src/lib/matrix/test/matrixAssignment.cpp
+25 -15
View File
@@ -23,19 +23,21 @@ int main()
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data); Matrix3f m2(data);
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);
} }
} }
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};
@@ -108,7 +112,7 @@ int main()
Matrix3f m4(data); Matrix3f m4(data);
TEST(isEqual(-m4, m4*(-1))); TEST(isEqual(-m4, m4 * (-1)));
// col swap // col swap
m4.swapCols(0, 2); m4.swapCols(0, 2);
@@ -136,20 +140,20 @@ int main()
TEST(fabs((-m4).min() + 9) < FLT_EPSILON); TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
Scalar<float> s = 1; Scalar<float> s = 1;
const Vector<float, 1> & s_vect = s; const Vector<float, 1> &s_vect = s;
TEST(fabs(s - 1) < FLT_EPSILON); TEST(fabs(s - 1) < FLT_EPSILON);
TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON); TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
Matrix<float, 1, 1> m5 = s; Matrix<float, 1, 1> m5 = s;
TEST(fabs(m5(0,0) - s) < FLT_EPSILON); TEST(fabs(m5(0, 0) - s) < FLT_EPSILON);
Matrix<float, 2, 2> m6; Matrix<float, 2, 2> m6;
m6.setRow(0, Vector2f(1, 2)); m6.setRow(0, Vector2f(1, 2));
float m7_array[] = {1,2,0,0}; float m7_array[] = {1, 2, 0, 0};
Matrix<float, 2, 2> m7(m7_array); Matrix<float, 2, 2> m7(m7_array);
TEST(isEqual(m6, m7)); TEST(isEqual(m6, m7));
m6.setCol(0, Vector2f(3, 4)); m6.setCol(0, Vector2f(3, 4));
float m8_array[] = {3,2,4,0}; float m8_array[] = {3, 2, 4, 0};
Matrix<float, 2, 2> m8(m8_array); Matrix<float, 2, 2> m8(m8_array);
TEST(isEqual(m6, m8)); TEST(isEqual(m6, m8));
@@ -206,7 +210,7 @@ int main()
TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f)); TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f));
Vector2f v1{NAN, 5.0f}; Vector2f v1{NAN, 5.0f};
Vector2f v1_min = min(v1, 1.f); Vector2f v1_min = min(v1, 1.f);
Matrix3f m11 = min(m10_constrained_ref,NAN); Matrix3f m11 = min(m10_constrained_ref, NAN);
TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0)))); TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0))));
TEST(isEqual(m11, m10_constrained_ref)); TEST(isEqual(m11, m10_constrained_ref));
@@ -217,18 +221,21 @@ int main()
12345678910.123456789f, 1234567891011.123456789101112f 12345678910.123456789f, 1234567891011.123456789101112f
}; };
Matrix<float, 3, 2> Comma(comma); Matrix<float, 3, 2> Comma(comma);
const size_t len = 15*2*3 + 2 + 1; const size_t len = 15 * 2 * 3 + 2 + 1;
char buffer[len]; char buffer[len];
Comma.print(); // for debugging in case of failure Comma.print(); // for debugging in case of failure
Comma.write_string(buffer, len); Comma.write_string(buffer, len);
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 : */
src/lib/matrix/test/matrixMult.cpp
+20 -21
View File
@@ -19,12 +19,12 @@ int main()
R2 *= A_I; R2 *= A_I;
TEST(isEqual(R2, I)); TEST(isEqual(R2, I));
TEST(R2==I); TEST(R2 == I);
TEST(A!=A_I); TEST(A != A_I);
Matrix3f A2 = eye<float, 3>()*2; Matrix3f A2 = eye<float, 3>() * 2;
Matrix3f B = A2.emult(A2); Matrix3f B = A2.emult(A2);
Matrix3f B_check = eye<float, 3>()*4; Matrix3f B_check = eye<float, 3>() * 4;
Matrix3f C_check = eye<float, 3>()*2; Matrix3f C_check = eye<float, 3>() * 2;
TEST(isEqual(B, B_check)); TEST(isEqual(B, B_check));
Matrix3f C = B_check.edivide(C_check); Matrix3f C = B_check.edivide(C_check);
@@ -33,14 +33,14 @@ int main()
TEST(C == Matrix3f(off_diagonal_nan)); TEST(C == Matrix3f(off_diagonal_nan));
// Test non-square matrix // Test non-square matrix
float data_43[12] = {1,3,2, float data_43[12] = {1, 3, 2,
2,2,1, 2, 2, 1,
5,2,1, 5, 2, 1,
2,3,4 2, 3, 4
}; };
float data_32[6] = {2,3, float data_32[6] = {2, 3,
1,7, 1, 7,
5,4 5, 4
}; };
Matrix<float, 4, 3> m43(data_43); Matrix<float, 4, 3> m43(data_43);
@@ -48,18 +48,18 @@ int main()
Matrix<float, 4, 2> m42 = m43 * m32; Matrix<float, 4, 2> m42 = m43 * m32;
float data_42[8] = {15,32, float data_42[8] = {15, 32,
11,24, 11, 24,
17,33, 17, 33,
27,43 27, 43
}; };
Matrix<float, 4, 2> m42_check(data_42); Matrix<float, 4, 2> m42_check(data_42);
TEST(isEqual(m42, m42_check)) TEST(isEqual(m42, m42_check))
float data_42_plus2[8] = {17,34, float data_42_plus2[8] = {17, 34,
13,26, 13, 26,
19,35, 19, 35,
29,45 29, 45
}; };
Matrix<float, 4, 2> m42_plus2_check(data_42_plus2); Matrix<float, 4, 2> m42_plus2_check(data_42_plus2);
Matrix<float, 4, 2> m42_plus2 = m42 - (-2); Matrix<float, 4, 2> m42_plus2 = m42 - (-2);
@@ -68,4 +68,3 @@ int main()
return 0; 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; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/pseudoInverse.cpp
+3 -4
View File
@@ -52,9 +52,9 @@ int main()
TEST((A1_I - A1_I_check).abs().max() < 1e-5); TEST((A1_I - A1_I_check).abs().max() < 1e-5);
// Stess test // Stess test
Matrix<float, n_large, n_large - 1> A_large; Matrix < float, n_large, n_large - 1 > A_large;
A_large.setIdentity(); A_large.setIdentity();
Matrix<float, n_large - 1, n_large> A_large_I; Matrix < float, n_large - 1, n_large > A_large_I;
for (size_t i = 0; i < n_large; i++) { for (size_t i = 0; i < n_large; i++) {
ret = geninv(A_large, A_large_I); ret = geninv(A_large, A_large_I);
@@ -130,7 +130,7 @@ int main()
{ 0.159704, 0.159704, 0.159704, 0.159704, -0.2500, -0.2500}, { 0.159704, 0.159704, 0.159704, 0.159704, -0.2500, -0.2500},
{ 0.607814, -0.607814, 0.607814, -0.607814, 1.0000, 1.0000} { 0.607814, -0.607814, 0.607814, -0.607814, 1.0000, 1.0000}
}; };
Matrix<float, 5, 6> real ( real_alloc); Matrix<float, 5, 6> real(real_alloc);
Matrix<float, 6, 5> real_pinv; Matrix<float, 6, 5> real_pinv;
ret = geninv(real, real_pinv); ret = geninv(real, real_pinv);
TEST(ret); TEST(ret);
@@ -150,4 +150,3 @@ int main()
return 0; 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; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/slice.cpp
+45 -44
View File
@@ -61,27 +61,28 @@ int main()
//Test writing to slices //Test writing to slices
Matrix<float, 3, 1> E; Matrix<float, 3, 1> E;
E(0,0) = -1; E(0, 0) = -1;
E(1,0) = 1; E(1, 0) = 1;
E(2,0) = 3; E(2, 0) = 3;
Matrix<float, 2, 1> F; Matrix<float, 2, 1> F;
F(0,0) = 9; F(0, 0) = 9;
F(1,0) = 11; F(1, 0) = 11;
E.slice<2,1>(0,0) = F; E.slice<2, 1>(0, 0) = F;
float data_3_check[3] = {9, 11, 3}; float data_3_check[3] = {9, 11, 3};
Matrix<float, 3, 1> G (data_3_check); Matrix<float, 3, 1> G(data_3_check);
TEST(isEqual(E, G)); TEST(isEqual(E, G));
TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0)))); TEST(isEqual(E, Matrix<float, 3, 1>(E.slice<3, 1>(0, 0))));
Matrix<float, 2, 1> H = E.slice<2,1>(0,0); Matrix<float, 2, 1> H = E.slice<2, 1>(0, 0);
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 {
const Matrix<float, 3, 1> J (data_3_check); // assigning row slices to each other
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);
K.row(1) = J.row(1); K.row(1) = J.row(1);
@@ -90,8 +91,9 @@ 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 {
const Matrix<float, 1, 3> J (data_3_check); // assigning col slices to each other
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);
K.col(1) = J.col(1); K.col(1) = J.col(1);
@@ -103,13 +105,13 @@ int main()
// check that slice of a slice works for reading // check that slice of a slice works for reading
const Matrix<float, 3, 3> cm33(data); const Matrix<float, 3, 3> cm33(data);
Matrix<float, 2, 1> topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2); Matrix<float, 2, 1> topRight = cm33.slice<2, 3>(0, 0).slice<2, 1>(0, 2);
float top_right_check[2] = {3,6}; float top_right_check[2] = {3, 6};
TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check))); TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
// check that slice of a slice works for writing // check that slice of a slice works for writing
Matrix<float, 3, 3> m33(data); Matrix<float, 3, 3> m33(data);
m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix<float, 2, 1>(); m33.slice<2, 3>(0, 0).slice<2, 1>(0, 2) = Matrix<float, 2, 1>();
const float data_check[9] = {0, 2, 0, const float data_check[9] = {0, 2, 0,
4, 5, 0, 4, 5, 0,
7, 8, 10 7, 8, 10
@@ -128,18 +130,18 @@ int main()
// min/max // min/max
TEST(m33.row(1).max() == 5); TEST(m33.row(1).max() == 5);
TEST(m33.col(0).min() == 0); TEST(m33.col(0).min() == 0);
TEST((m33.slice<2,2>(1,1).max()) == 10); TEST((m33.slice<2, 2>(1, 1).max()) == 10);
// assign scalar value to slice // assign scalar value to slice
Matrix<float, 3, 1> L; Matrix<float, 3, 1> L;
L(0,0) = -1; L(0, 0) = -1;
L(1,0) = 1; L(1, 0) = 1;
L(2,0) = 3; L(2, 0) = 3;
L.slice<2,1>(0,0) = 0.0f; L.slice<2, 1>(0, 0) = 0.0f;
float data_5_check[3] = {0, 0, 3}; float data_5_check[3] = {0, 0, 3};
Matrix<float, 3, 1> M (data_5_check); Matrix<float, 3, 1> M(data_5_check);
TEST(isEqual(L, M)); TEST(isEqual(L, M));
// return diagonal elements // return diagonal elements
@@ -149,38 +151,38 @@ int main()
}; };
SquareMatrix<float, 3> N(data_6); SquareMatrix<float, 3> N(data_6);
Vector3f v6 = N.slice<3,3>(0,0).diag(); Vector3f v6 = N.slice<3, 3>(0, 0).diag();
Vector3f v6_check = {0, 5, 10}; Vector3f v6_check = {0, 5, 10};
TEST(isEqual(v6,v6_check)); TEST(isEqual(v6, v6_check));
Vector2f v7 = N.slice<2,3>(1,0).diag(); Vector2f v7 = N.slice<2, 3>(1, 0).diag();
Vector2f v7_check = {4, 8}; Vector2f v7_check = {4, 8};
TEST(isEqual(v7,v7_check)); TEST(isEqual(v7, v7_check));
Vector2f v8 = N.slice<3,2>(0,1).diag(); Vector2f v8 = N.slice<3, 2>(0, 1).diag();
Vector2f v8_check = {2, 6}; Vector2f v8_check = {2, 6};
TEST(isEqual(v8,v8_check)); TEST(isEqual(v8, v8_check));
Vector2f v9(N.slice<1,2>(1,1)); Vector2f v9(N.slice<1, 2>(1, 1));
Vector2f v9_check = {5, 6}; Vector2f v9_check = {5, 6};
TEST(isEqual(v9,v9_check)); TEST(isEqual(v9, v9_check));
Vector3f v10(N.slice<1,3>(1,0)); Vector3f v10(N.slice<1, 3>(1, 0));
Vector3f v10_check = {4, 5, 6}; Vector3f v10_check = {4, 5, 6};
TEST(isEqual(v10,v10_check)); TEST(isEqual(v10, v10_check));
// Different assignment operators // Different assignment operators
SquareMatrix3f O(data); SquareMatrix3f O(data);
float operand_data [4] = {2, 1, -3, -1}; float operand_data [4] = {2, 1, -3, -1};
const SquareMatrix<float, 2> operand(operand_data); const SquareMatrix<float, 2> operand(operand_data);
O.slice<2,2>(1,0) += operand; O.slice<2, 2>(1, 0) += operand;
float O_check_data_1 [9] = {0, 2, 3, 6, 6, 6, 4, 7, 10}; float O_check_data_1 [9] = {0, 2, 3, 6, 6, 6, 4, 7, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_1))); TEST(isEqual(O, SquareMatrix3f(O_check_data_1)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
O.slice<2,1>(1,1) += operand.slice<2,1>(0,0); O.slice<2, 1>(1, 1) += operand.slice<2, 1>(0, 0);
float O_check_data_2 [9] = {0, 2, 3, 4, 7, 6, 7, 5, 10}; float O_check_data_2 [9] = {0, 2, 3, 4, 7, 6, 7, 5, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_2))); TEST(isEqual(O, SquareMatrix3f(O_check_data_2)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
O.slice<3,3>(0,0) += -1; O.slice<3, 3>(0, 0) += -1;
float O_check_data_3 [9] = {-1, 1, 2, 3, 4, 5, 6, 7, 9}; float O_check_data_3 [9] = {-1, 1, 2, 3, 4, 5, 6, 7, 9};
TEST(isEqual(O, SquareMatrix3f(O_check_data_3))); TEST(isEqual(O, SquareMatrix3f(O_check_data_3)));
@@ -190,17 +192,17 @@ int main()
TEST(isEqual(O, SquareMatrix3f(O_check_data_4))); TEST(isEqual(O, SquareMatrix3f(O_check_data_4)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
O.slice<2,2>(1,0) -= operand; O.slice<2, 2>(1, 0) -= operand;
float O_check_data_5 [9] = {0, 2, 3, 2, 4, 6, 10, 9, 10}; float O_check_data_5 [9] = {0, 2, 3, 2, 4, 6, 10, 9, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_5))); TEST(isEqual(O, SquareMatrix3f(O_check_data_5)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
O.slice<2,1>(1,1) -= operand.slice<2,1>(0,0); O.slice<2, 1>(1, 1) -= operand.slice<2, 1>(0, 0);
float O_check_data_6 [9] = {0, 2, 3, 4, 3, 6, 7, 11, 10}; float O_check_data_6 [9] = {0, 2, 3, 4, 3, 6, 7, 11, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_6))); TEST(isEqual(O, SquareMatrix3f(O_check_data_6)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
O.slice<3,3>(0,0) -= -1; O.slice<3, 3>(0, 0) -= -1;
float O_check_data_7 [9] = {1, 3, 4, 5, 6, 7, 8, 9, 11}; float O_check_data_7 [9] = {1, 3, 4, 5, 6, 7, 8, 9, 11};
TEST(isEqual(O, SquareMatrix3f(O_check_data_7))); TEST(isEqual(O, SquareMatrix3f(O_check_data_7)));
@@ -210,26 +212,25 @@ int main()
TEST(isEqual(O, SquareMatrix3f(O_check_data_8))); TEST(isEqual(O, SquareMatrix3f(O_check_data_8)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
O.slice<2,1>(1,1) *= 5.f; O.slice<2, 1>(1, 1) *= 5.f;
float O_check_data_9 [9] = {0, 2, 3, 4, 25, 6, 7, 40, 10}; float O_check_data_9 [9] = {0, 2, 3, 4, 25, 6, 7, 40, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_9))); TEST(isEqual(O, SquareMatrix3f(O_check_data_9)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
O.slice<2,1>(1,1) /= 2.f; O.slice<2, 1>(1, 1) /= 2.f;
float O_check_data_10 [9] = {0, 2, 3, 4, 2.5, 6, 7, 4, 10}; float O_check_data_10 [9] = {0, 2, 3, 4, 2.5, 6, 7, 4, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_10))); TEST(isEqual(O, SquareMatrix3f(O_check_data_10)));
// Different operations // Different operations
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
SquareMatrix<float, 2> res_11(O.slice<2,2>(1,1) * 2.f); SquareMatrix<float, 2> res_11(O.slice<2, 2>(1, 1) * 2.f);
float O_check_data_11 [4] = {10, 12, 16, 20}; float O_check_data_11 [4] = {10, 12, 16, 20};
TEST(isEqual(res_11, SquareMatrix<float, 2>(O_check_data_11))); TEST(isEqual(res_11, SquareMatrix<float, 2>(O_check_data_11)));
O = SquareMatrix3f(data); O = SquareMatrix3f(data);
SquareMatrix<float, 2> res_12(O.slice<2,2>(1,1) / 2.f); SquareMatrix<float, 2> res_12(O.slice<2, 2>(1, 1) / 2.f);
float O_check_data_12 [4] = {2.5, 3, 4, 5}; float O_check_data_12 [4] = {2.5, 3, 4, 5};
TEST(isEqual(res_12, SquareMatrix<float, 2>(O_check_data_12))); TEST(isEqual(res_12, SquareMatrix<float, 2>(O_check_data_12)));
return 0; 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; 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();
src/lib/matrix/test/squareMatrix.cpp
+14 -15
View File
@@ -23,13 +23,13 @@ int main()
}; };
float dt = 0.01f; float dt = 0.01f;
SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A*dt), 5); SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A * dt), 5);
SquareMatrix<float, 3> eA_check(data_check); SquareMatrix<float, 3> eA_check(data_check);
TEST((eA - eA_check).abs().max() < 1e-3f); TEST((eA - eA_check).abs().max() < 1e-3f);
SquareMatrix<float, 2> A_bottomright = A.slice<2,2>(1,1); SquareMatrix<float, 2> A_bottomright = A.slice<2, 2>(1, 1);
SquareMatrix<float, 2> A_bottomright2; SquareMatrix<float, 2> A_bottomright2;
A_bottomright2 = A.slice<2,2>(1,1); A_bottomright2 = A.slice<2, 2>(1, 1);
float data_bottomright[4] = {5, 6, float data_bottomright[4] = {5, 6,
8, 10 8, 10
@@ -42,7 +42,7 @@ int main()
float data_4x4[16] = {1, 2, 3, 4, float data_4x4[16] = {1, 2, 3, 4,
5, 6, 7, 8, 5, 6, 7, 8,
9, 10, 11, 12, 9, 10, 11, 12,
13, 14,15, 16 13, 14, 15, 16
}; };
SquareMatrix<float, 4> B(data_4x4); SquareMatrix<float, 4> B(data_4x4);
@@ -50,7 +50,7 @@ int main()
float data_B_check[16] = {1, 0, 3, 4, float data_B_check[16] = {1, 0, 3, 4,
0, 6, 0, 0, 0, 6, 0, 0,
9, 0, 11, 12, 9, 0, 11, 12,
13, 0,15, 16 13, 0, 15, 16
}; };
SquareMatrix<float, 4> B_check(data_B_check); SquareMatrix<float, 4> B_check(data_B_check);
TEST(isEqual(B, B_check)) TEST(isEqual(B, B_check))
@@ -60,17 +60,17 @@ int main()
float data_C_check[16] = {1, 0, 0, 4, float data_C_check[16] = {1, 0, 0, 4,
0, 6, 0, 0, 0, 6, 0, 0,
0, 0, 11, 0, 0, 0, 11, 0,
13, 0,0, 16 13, 0, 0, 16
}; };
SquareMatrix<float, 4> C_check(data_C_check); SquareMatrix<float, 4> C_check(data_C_check);
TEST(isEqual(C, C_check)) TEST(isEqual(C, C_check))
SquareMatrix<float, 4> D(data_4x4); SquareMatrix<float, 4> D(data_4x4);
D.uncorrelateCovarianceSetVariance<2>(0, Vector2f{20,21}); D.uncorrelateCovarianceSetVariance<2>(0, Vector2f{20, 21});
float data_D_check[16] = {20, 0, 0, 0, float data_D_check[16] = {20, 0, 0, 0,
0, 21, 0, 0, 0, 21, 0, 0,
0, 0, 11, 12, 0, 0, 11, 12,
0, 0,15, 16 0, 0, 15, 16
}; };
SquareMatrix<float, 4> D_check(data_D_check); SquareMatrix<float, 4> D_check(data_D_check);
TEST(isEqual(D, D_check)) TEST(isEqual(D, D_check))
@@ -80,7 +80,7 @@ int main()
float data_E_check[16] = {1, 0, 0, 0, float data_E_check[16] = {1, 0, 0, 0,
0, 33, 0, 0, 0, 33, 0, 0,
0, 0, 33, 0, 0, 0, 33, 0,
0, 0,0, 33 0, 0, 0, 33
}; };
SquareMatrix<float, 4> E_check(data_E_check); SquareMatrix<float, 4> E_check(data_E_check);
TEST(isEqual(E, E_check)) TEST(isEqual(E, E_check))
@@ -91,7 +91,7 @@ int main()
float data_F_check[16] = {1, 2, 3, 4, float data_F_check[16] = {1, 2, 3, 4,
5, 6, 8.5, 8, 5, 6, 8.5, 8,
9, 8.5, 11, 12, 9, 8.5, 11, 12,
13, 14,15, 16 13, 14, 15, 16
}; };
SquareMatrix<float, 4> F_check(data_F_check); SquareMatrix<float, 4> F_check(data_F_check);
TEST(isEqual(F, F_check)) TEST(isEqual(F, F_check))
@@ -103,7 +103,7 @@ int main()
float data_G_check[16] = {1, 3.5, 6, 4, float data_G_check[16] = {1, 3.5, 6, 4,
3.5, 6, 8.5, 11, 3.5, 6, 8.5, 11,
6, 8.5, 11, 13.5, 6, 8.5, 11, 13.5,
13, 11,13.5, 16 13, 11, 13.5, 16
}; };
SquareMatrix<float, 4> G_check(data_G_check); SquareMatrix<float, 4> G_check(data_G_check);
TEST(isEqual(G, G_check)); TEST(isEqual(G, G_check));
@@ -115,7 +115,7 @@ int main()
float data_H_check[16] = {1, 2, 3, 4, float data_H_check[16] = {1, 2, 3, 4,
5, 6, 7, 8, 5, 6, 7, 8,
9, 10, 11, 12, 9, 10, 11, 12,
13, 14,15, 16 13, 14, 15, 16
}; };
SquareMatrix<float, 4> H_check(data_H_check); SquareMatrix<float, 4> H_check(data_H_check);
TEST(isEqual(H, H_check)) TEST(isEqual(H, H_check))
@@ -127,7 +127,7 @@ int main()
float data_J_check[16] = {1, 3.5, 3, 4, float data_J_check[16] = {1, 3.5, 3, 4,
3.5, 6, 8.5, 11, 3.5, 6, 8.5, 11,
9, 8.5, 11, 12, 9, 8.5, 11, 12,
13, 11,15, 16 13, 11, 15, 16
}; };
SquareMatrix<float, 4> J_check(data_J_check); SquareMatrix<float, 4> J_check(data_J_check);
TEST(isEqual(J, J_check)); TEST(isEqual(J, J_check));
@@ -138,11 +138,10 @@ int main()
float data_K[16] = {1, 2, 3, 4, float data_K[16] = {1, 2, 3, 4,
2, 3, 4, 11, 2, 3, 4, 11,
3, 4, 11, 12, 3, 4, 11, 12,
4, 11,15, 16 4, 11, 15, 16
}; };
SquareMatrix<float, 4> K(data_K); SquareMatrix<float, 4> K(data_K);
TEST(!K.isRowColSymmetric<1>(2)); TEST(!K.isRowColSymmetric<1>(2));
return 0; 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) 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 :
src/lib/matrix/test/transpose.cpp
-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 : */
src/lib/matrix/test/upperRightTriangle.cpp
+1 -2
View File
@@ -13,11 +13,10 @@ int main()
SquareMatrix<float, 3> A(data); SquareMatrix<float, 3> A(data);
for(size_t i=0; i<6; i++) { for (size_t i = 0; i < 6; i++) {
TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON); TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON);
} }
return 0; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vector.cpp
+3 -4
View File
@@ -7,8 +7,8 @@ using namespace matrix;
int main() int main()
{ {
// test data // test data
float data1[] = {1,2,3,4,5}; float data1[] = {1, 2, 3, 4, 5};
float data2[] = {6,7,8,9,10}; float data2[] = {6, 7, 8, 9, 10};
Vector<float, 5> v1(data1); Vector<float, 5> v1(data1);
Vector<float, 5> v2(data2); Vector<float, 5> v2(data2);
@@ -31,7 +31,7 @@ int main()
TEST(isEqualF(v2.norm(), 1.f)); TEST(isEqualF(v2.norm(), 1.f));
// sqrt // sqrt
float data1_sq[] = {1,4,9,16,25}; float data1_sq[] = {1, 4, 9, 16, 25};
Vector<float, 5> v4(data1_sq); Vector<float, 5> v4(data1_sq);
TEST(isEqual(v1, v4.sqrt())); TEST(isEqual(v1, v4.sqrt()));
@@ -45,4 +45,3 @@ int main()
return 0; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vector2.cpp
+3 -4
View File
@@ -17,14 +17,14 @@ int main()
TEST(fabs(c(1) - 0) < FLT_EPSILON); TEST(fabs(c(1) - 0) < FLT_EPSILON);
Matrix<float, 2, 1> d(a); Matrix<float, 2, 1> d(a);
TEST(fabs(d(0,0) - 1) < FLT_EPSILON); TEST(fabs(d(0, 0) - 1) < FLT_EPSILON);
TEST(fabs(d(1,0) - 0) < FLT_EPSILON); TEST(fabs(d(1, 0) - 0) < FLT_EPSILON);
Vector2f e(d); Vector2f e(d);
TEST(fabs(e(0) - 1) < FLT_EPSILON); TEST(fabs(e(0) - 1) < FLT_EPSILON);
TEST(fabs(e(1) - 0) < FLT_EPSILON); TEST(fabs(e(1) - 0) < FLT_EPSILON);
float data[] = {4,5}; float data[] = {4, 5};
Vector2f f(data); Vector2f f(data);
TEST(fabs(f(0) - 4) < FLT_EPSILON); TEST(fabs(f(0) - 4) < FLT_EPSILON);
TEST(fabs(f(1) - 5) < FLT_EPSILON); TEST(fabs(f(1) - 5) < FLT_EPSILON);
@@ -37,4 +37,3 @@ int main()
return 0; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vector3.cpp
+10 -11
View File
@@ -9,9 +9,9 @@ int main()
Vector3f a(1, 0, 0); Vector3f a(1, 0, 0);
Vector3f b(0, 1, 0); Vector3f b(0, 1, 0);
Vector3f c = a.cross(b); Vector3f c = a.cross(b);
TEST(isEqual(c, Vector3f(0,0,1))); TEST(isEqual(c, Vector3f(0, 0, 1)));
c = a % b; c = a % b;
TEST(isEqual(c, Vector3f(0,0,1))); TEST(isEqual(c, Vector3f(0, 0, 1)));
Matrix<float, 3, 1> d(c); Matrix<float, 3, 1> d(c);
Vector3f e(d); Vector3f e(d);
TEST(isEqual(e, d)); TEST(isEqual(e, d));
@@ -25,20 +25,20 @@ int main()
TEST(isEqual(-a, Vector3f(-1, 0, 0))); TEST(isEqual(-a, Vector3f(-1, 0, 0)));
TEST(isEqual(a.unit(), a)); TEST(isEqual(a.unit(), a));
TEST(isEqual(a.unit(), a.normalized())); TEST(isEqual(a.unit(), a.normalized()));
TEST(isEqual(a*2.0, Vector3f(2, 0, 0))); TEST(isEqual(a * 2.0, Vector3f(2, 0, 0)));
Vector2f g2(1,3); Vector2f g2(1, 3);
Vector3f g3(7, 11, 17); Vector3f g3(7, 11, 17);
g3.xy() = g2; g3.xy() = g2;
TEST(isEqual(g3, Vector3f(1, 3, 17))); TEST(isEqual(g3, Vector3f(1, 3, 17)));
const Vector3f g4(g3); const Vector3f g4(g3);
Vector2f g5 = g4.xy(); Vector2f g5 = g4.xy();
TEST(isEqual(g5,g2)); TEST(isEqual(g5, g2));
TEST(isEqual(g2,Vector2f(g4.xy()))); TEST(isEqual(g2, Vector2f(g4.xy())));
Vector3f h; Vector3f h;
TEST(isEqual(h,Vector3f(0,0,0))); TEST(isEqual(h, Vector3f(0, 0, 0)));
Vector<float, 4> j; Vector<float, 4> j;
j(0) = 1; j(0) = 1;
@@ -46,9 +46,9 @@ int main()
j(2) = 3; j(2) = 3;
j(3) = 4; j(3) = 4;
Vector3f k = j.slice<3,1>(0,0); Vector3f k = j.slice<3, 1>(0, 0);
Vector3f k_test(1,2,3); Vector3f k_test(1, 2, 3);
TEST(isEqual(k,k_test)); TEST(isEqual(k, k_test));
Vector3f m1(1, 2, 3); Vector3f m1(1, 2, 3);
Vector3f m2(3.1f, 4.1f, 5.1f); Vector3f m2(3.1f, 4.1f, 5.1f);
@@ -58,4 +58,3 @@ int main()
return 0; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
src/lib/matrix/test/vectorAssignment.cpp
+1 -2
View File
@@ -23,7 +23,7 @@ int main()
TEST(fabs(v2(1) - 5) < eps); TEST(fabs(v2(1) - 5) < eps);
TEST(fabs(v2(2) - 6) < eps); TEST(fabs(v2(2) - 6) < eps);
SquareMatrix<float, 3> m = diag(Vector3f(1,2,3)); SquareMatrix<float, 3> m = diag(Vector3f(1, 2, 3));
TEST(fabs(m(0, 0) - 1) < eps); TEST(fabs(m(0, 0) - 1) < eps);
TEST(fabs(m(1, 1) - 2) < eps); TEST(fabs(m(1, 1) - 2) < eps);
TEST(fabs(m(2, 2) - 3) < eps); TEST(fabs(m(2, 2) - 3) < eps);
@@ -31,4 +31,3 @@ int main()
return 0; return 0;
} }
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */