From 4f3565da94d00c4cd1feb560f1f72a81296522f8 Mon Sep 17 00:00:00 2001 From: kritz Date: Mon, 9 Dec 2019 10:21:27 +0100 Subject: [PATCH] Add asserts (#115) * Add asserts * Type cast literals * asserts for indexing vectors * include assert * Fix accessing elements outside of slice --- CMakeLists.txt | 6 +++++- matrix/Matrix.hpp | 20 ++++++++++++++++++++ matrix/Slice.hpp | 27 +++++++++++++++++++++------ matrix/SquareMatrix.hpp | 40 ++++++++++++++++++++++++++++++++++------ matrix/Vector.hpp | 6 ++++++ matrix/math.hpp | 1 + 6 files changed, 87 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fae908d9aab..909683df3292 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,11 @@ set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE) + if(TESTING) + set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE) + else() + set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE) + endif() message(STATUS "set build type to ${CMAKE_BUILD_TYPE}") endif() diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index c3b47ce83935..188b5be619d9 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -96,11 +96,21 @@ class Matrix inline Type operator()(size_t i, size_t j) const { + assert(i >= 0); + assert(i < M); + assert(j >= 0); + assert(j < N); + return _data[i][j]; } inline Type &operator()(size_t i, size_t j) { + assert(i >= 0); + assert(i < M); + assert(j >= 0); + assert(j < N); + return _data[i][j]; } @@ -470,6 +480,11 @@ class Matrix inline void swapRows(size_t a, size_t b) { + assert(a >= 0); + assert(a < M); + assert(b >= 0); + assert(b < M); + if (a == b) { return; } @@ -485,6 +500,11 @@ class Matrix inline void swapCols(size_t a, size_t b) { + assert(a >= 0); + assert(a < N); + assert(b >= 0); + assert(b < N); + if (a == b) { return; } diff --git a/matrix/Slice.hpp b/matrix/Slice.hpp index 21b2a4f09ad5..f61a00905463 100644 --- a/matrix/Slice.hpp +++ b/matrix/Slice.hpp @@ -28,15 +28,30 @@ class Slice { _data(const_cast*>(data)) { static_assert(P <= M, "Slice rows bigger than backing matrix"); static_assert(Q <= N, "Slice cols bigger than backing matrix"); + assert(x0 >= 0); + assert(x0 + P <= M); + assert(y0 >= 0); + assert(y0 + Q <= N); } Type operator()(size_t i, size_t j) const { + assert(i >= 0); + assert(i < P); + assert(j >= 0); + assert(j < Q); + return (*_data)(_x0 + i, _y0 + j); } Type &operator()(size_t i, size_t j) + { + assert(i >= 0); + assert(i < P); + assert(j >= 0); + assert(j < Q); + return (*_data)(_x0 + i, _y0 + j); } @@ -97,23 +112,23 @@ class Slice { return Slice(x0 + _x0, y0 + _y0, _data); } - void copyTo(Type dst[M*N]) const + void copyTo(Type dst[P*Q]) const { const Slice &self = *this; - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { dst[i*N+j] = self(i, j); } } } - void copyToColumnMajor(Type dst[M*N]) const + void copyToColumnMajor(Type dst[P*Q]) const { const Slice &self = *this; - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { dst[i+(j*M)] = self(i, j); } } diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp index fcb225d32419..d343bcfbfbe9 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -135,6 +135,10 @@ class SquareMatrix : public Matrix template void uncorrelateCovariance(size_t first) { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first >= 0); + assert(first + Width <= M); + SquareMatrix &self = *this; Vector diag_elements = self.slice(first, first).diag(); self.uncorrelateCovarianceSetVariance(first, diag_elements); @@ -143,10 +147,14 @@ class SquareMatrix : public Matrix template void uncorrelateCovarianceSetVariance(size_t first, const Vector &vec) { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first >= 0); + assert(first + Width <= M); + SquareMatrix &self = *this; // zero rows and columns - self.slice(first, 0) = 0; - self.slice(0, first) = 0; + self.slice(first, 0) = Type(0); + self.slice(0, first) = Type(0); // set diagonals unsigned vec_idx = 0; @@ -159,10 +167,14 @@ class SquareMatrix : public Matrix template void uncorrelateCovarianceSetVariance(size_t first, Type val) { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first >= 0); + assert(first + Width <= M); + SquareMatrix &self = *this; // zero rows and columns - self.slice(first, 0) = 0; - self.slice(0, first) = 0; + self.slice(first, 0) = Type(0); + self.slice(0, first) = Type(0); // set diagonals for (size_t idx = first; idx < first+Width; idx++) { @@ -174,11 +186,15 @@ class SquareMatrix : public Matrix template void makeBlockSymmetric(size_t first) { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first >= 0); + assert(first + Width <= M); + SquareMatrix &self = *this; if(Width>1) { for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) { for (size_t col_idx = first; col_idx < row_idx; col_idx++) { - Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / 2; + Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / Type(2); self(row_idx,col_idx) = tmp; self(col_idx,row_idx) = tmp; } @@ -190,11 +206,15 @@ class SquareMatrix : public Matrix template void makeRowColSymmetric(size_t first) { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first >= 0); + assert(first + Width <= M); + SquareMatrix &self = *this; self.makeBlockSymmetric(first); for (size_t row_idx = first; row_idx < first+Width; row_idx++) { for (size_t col_idx = 0; col_idx < first; col_idx++) { - Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / 2; + Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / Type(2); self(row_idx,col_idx) = tmp; self(col_idx,row_idx) = tmp; } @@ -210,6 +230,10 @@ class SquareMatrix : public Matrix template bool isBlockSymmetric(size_t first, const Type eps = 1e-8f) { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first >= 0); + assert(first + Width <= M); + SquareMatrix &self = *this; if(Width>1) { for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) { @@ -227,6 +251,10 @@ class SquareMatrix : public Matrix template bool isRowColSymmetric(size_t first, const Type eps = 1e-8f) { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first >= 0); + assert(first + Width <= M); + SquareMatrix &self = *this; for (size_t row_idx = first; row_idx < first+Width; row_idx++) { for (size_t col_idx = 0; col_idx < first; col_idx++) { diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp index fe83505af9dd..40803ba8cce9 100644 --- a/matrix/Vector.hpp +++ b/matrix/Vector.hpp @@ -42,12 +42,18 @@ class Vector : public Matrix inline Type operator()(size_t i) const { + assert(i >= 0); + assert(i < M); + const MatrixM1 &v = *this; return v(i, 0); } inline Type &operator()(size_t i) { + assert(i >= 0); + assert(i < M); + MatrixM1 &v = *this; return v(i, 0); } diff --git a/matrix/math.hpp b/matrix/math.hpp index 1bb9d6724376..edfc1591fcc2 100644 --- a/matrix/math.hpp +++ b/matrix/math.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include "stdlib_imports.hpp" #ifdef __PX4_QURT #include "dspal_math.h"