fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 02:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add asserts (#115)

Browse Source 
* Add asserts

* Type cast literals

* asserts for indexing vectors

* include assert

* Fix accessing elements outside of slice
This commit is contained in:
kritz
2019-12-09 10:21:27 +01:00
committed by Julian Kent
parent 2f6398168d
commit 4f3565da94
6 changed files with 87 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/SquareMatrix.hpp
+34 -6
View File
@@ -135,6 +135,10 @@ public:
template <size_t Width>
void uncorrelateCovariance(size_t first)
{
static_assert(Width <= M, "Width bigger than matrix");
assert(first >= 0);
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
Vector<Type, Width> diag_elements = self.slice<Width, Width>(first, first).diag();
self.uncorrelateCovarianceSetVariance(first, diag_elements);
@@ -143,10 +147,14 @@ public:
template <size_t Width>
void uncorrelateCovarianceSetVariance(size_t first, const Vector<Type, Width> &vec)
{
static_assert(Width <= M, "Width bigger than matrix");
assert(first >= 0);
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
// zero rows and columns
self.slice<Width, M>(first, 0) = 0;
self.slice<M, Width>(0, first) = 0;
self.slice<Width, M>(first, 0) = Type(0);
self.slice<M, Width>(0, first) = Type(0);
// set diagonals
unsigned vec_idx = 0;
@@ -159,10 +167,14 @@ public:
template <size_t Width>
void uncorrelateCovarianceSetVariance(size_t first, Type val)
{
static_assert(Width <= M, "Width bigger than matrix");
assert(first >= 0);
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
// zero rows and columns
self.slice<Width, M>(first, 0) = 0;
self.slice<M, Width>(0, first) = 0;
self.slice<Width, M>(first, 0) = Type(0);
self.slice<M, Width>(0, first) = Type(0);
// set diagonals
for (size_t idx = first; idx < first+Width; idx++) {
@@ -174,11 +186,15 @@ public:
template <size_t Width>
void makeBlockSymmetric(size_t first)
{
static_assert(Width <= M, "Width bigger than matrix");
assert(first >= 0);
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
if(Width>1) {
for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
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 @@ public:
template <size_t Width>
void makeRowColSymmetric(size_t first)
{
static_assert(Width <= M, "Width bigger than matrix");
assert(first >= 0);
assert(first + Width <= M);
SquareMatrix<Type, M> &self = *this;
self.makeBlockSymmetric<Width>(first);
for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
for (size_t col_idx = 0; col_idx < first; col_idx++) {
Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - 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 @@ public:
template <size_t Width>
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<Type, M> &self = *this;
if(Width>1) {
for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
@@ -227,6 +251,10 @@ public:
template <size_t Width>
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<Type, M> &self = *this;
for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
for (size_t col_idx = 0; col_idx < first; col_idx++) {