fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 17:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Symmetric square matrix packing

Browse Source
This commit is contained in:
Pavel Kirienko
2015-04-27 10:12:50 +03:00
parent f2209c214a
commit bfc4b5336c
2 changed files with 319 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/marshal/array.hpp
+209 -62
View File
@@ -34,6 +34,138 @@ namespace uavcan
enum ArrayMode { ArrayModeStatic, ArrayModeDynamic };
/**
* Properties of a square matrix; assuming row-major representation.
*/
template <unsigned NumElements_>
struct SquareMatrixTraits
{
enum { NumElements = NumElements_ };
enum { NumRowsCols = CompileTimeIntSqrt<NumElements>::Result };
enum { NumElementsInTriangle = ((1 + NumRowsCols) * NumRowsCols) / 2 };
static inline bool isIndexOnDiagonal(unsigned index) { return (index / NumRowsCols) == (index % NumRowsCols); }
static inline int computeElementIndexAtRowCol(int row, int col) { return row * NumRowsCols + col; }
};
/**
* This class can be used to detect properties of square matrices.
* Element iterator is a random access forward constant iterator.
*/
template <typename ElementIterator, unsigned NumElements>
class SquareMatrixAnalyzer : public SquareMatrixTraits<NumElements>
{
typedef SquareMatrixTraits<NumElements> Traits;
const ElementIterator first_;
public:
enum PackingMode
{
PackingModeEmpty,
PackingModeScalar,
PackingModeDiagonal,
PackingModeSymmetric,
PackingModeFull
};
SquareMatrixAnalyzer(ElementIterator first_element_iterator)
: first_(first_element_iterator)
{
StaticAssert<(NumElements > 0)>::check();
}
ElementIterator accessElementAtRowCol(int row, int col) const
{
return first_ + Traits::computeElementIndexAtRowCol(row, col);
}
bool areAllElementsNan() const
{
unsigned index = 0;
for (ElementIterator it = first_; index < NumElements; ++it, ++index)
{
if (!isNaN(*it))
{
return false;
}
}
return true;
}
bool isScalar() const
{
unsigned index = 0;
for (ElementIterator it = first_; index < NumElements; ++it, ++index)
{
if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it))
{
return false;
}
if (Traits::isIndexOnDiagonal(index) && !areClose(*it, *first_))
{
return false;
}
}
return true;
}
bool isDiagonal() const
{
unsigned index = 0;
for (ElementIterator it = first_; index < NumElements; ++it, ++index)
{
if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it))
{
return false;
}
}
return true;
}
bool isSymmetric() const
{
for (int i = 0; i < Traits::NumRowsCols; ++i)
{
for (int k = 0; k < Traits::NumRowsCols; ++k)
{
// On diagonal comparison is pointless
if ((i != k) &&
!areClose(*accessElementAtRowCol(i, k),
*accessElementAtRowCol(k, i)))
{
return false;
}
}
}
return true;
}
PackingMode detectOptimalPackingMode() const
{
if (areAllElementsNan())
{
return PackingModeEmpty;
}
if (isScalar())
{
return PackingModeScalar;
}
if (isDiagonal())
{
return PackingModeDiagonal;
}
if (isSymmetric())
{
return PackingModeSymmetric;
}
return PackingModeFull;
}
};
template <unsigned Size>
class UAVCAN_EXPORT StaticArrayBase
@@ -400,82 +532,74 @@ class UAVCAN_EXPORT Array : public ArrayImpl<T, ArrayMode, MaxSize_>
void packSquareMatrixImpl(const InputIter src_row_major)
{
StaticAssert<IsDynamic>::check();
const unsigned Width = CompileTimeIntSqrt<MaxSize>::Result;
bool all_nans = true;
bool scalar_matrix = true;
bool diagonal_matrix = true;
/*
* Detecting how the matrix can be compressed:
* - Matrix that consists only of NANs will be eliminated completely;
* - Scalar matrix will be reduced to one value;
* - Diagonal matrix will be reduced to array of length Width.
*/
{
unsigned index = 0;
for (InputIter it = src_row_major; index < MaxSize; ++it, ++index)
{
const bool on_diagonal = (index / Width) == (index % Width);
const bool nan = isNaN(*it);
if (!nan)
{
all_nans = false;
}
if (!on_diagonal && !isCloseToZero(*it))
{
scalar_matrix = false; // This matrix cannot be compressed.
diagonal_matrix = false;
break;
}
if (on_diagonal && !areClose(*it, *src_row_major))
{
scalar_matrix = false;
}
}
}
/*
* Actual packing is performed here.
*/
this->clear();
if (!all_nans)
typedef SquareMatrixAnalyzer<InputIter, MaxSize> Analyzer;
const Analyzer analyzer(src_row_major);
switch (analyzer.detectOptimalPackingMode())
{
unsigned index = 0;
for (InputIter it = src_row_major; index < MaxSize; ++it, ++index)
case Analyzer::PackingModeEmpty:
{
break; // Nothing to insert
}
case Analyzer::PackingModeScalar:
{
this->push_back(ValueType(*src_row_major));
break;
}
case Analyzer::PackingModeDiagonal:
{
for (int i = 0; i < Analyzer::NumRowsCols; i++)
{
const bool on_diagonal = (index / Width) == (index % Width);
if (diagonal_matrix && !on_diagonal)
this->push_back(ValueType(*analyzer.accessElementAtRowCol(i, i)));
}
break;
}
case Analyzer::PackingModeSymmetric:
{
for (int row = 0; row < Analyzer::NumRowsCols; row++)
{
for (int col = row; col < Analyzer::NumRowsCols; col++)
{
continue;
}
this->push_back(ValueType(*it));
if (scalar_matrix)
{
break;
this->push_back(ValueType(*analyzer.accessElementAtRowCol(row, col)));
}
}
UAVCAN_ASSERT(this->size() == Analyzer::NumElementsInTriangle);
break;
}
case Analyzer::PackingModeFull:
{
InputIter it = src_row_major;
for (unsigned index = 0; index < MaxSize; index++, it++)
{
this->push_back(ValueType(*it));
}
break;
}
default:
{
UAVCAN_ASSERT(0);
break;
}
}
}
template <typename ScalarType, typename OutputIter>
void unpackSquareMatrixImpl(OutputIter it) const
void unpackSquareMatrixImpl(const OutputIter dst_row_major) const
{
StaticAssert<IsDynamic>::check();
const unsigned Width = CompileTimeIntSqrt<MaxSize>::Result;
/*
* Unpacking as follows:
* - Array of length 1 will be unpacked to scalar matrix
* - Array of length Width will be unpacked to diagonal matrix
* - Array of length MaxSize will be unpacked to full matrix
* - All other length values will yield zero matrix
*/
if (this->size() == Width || this->size() == 1)
typedef SquareMatrixTraits<MaxSize> Traits;
if (this->size() == Traits::NumRowsCols || this->size() == 1) // Scalar or diagonal
{
OutputIter it = dst_row_major;
for (unsigned index = 0; index < MaxSize; index++)
{
const bool on_diagonal = (index / Width) == (index % Width);
if (on_diagonal)
if (Traits::isIndexOnDiagonal(index))
{
const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Width));
const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Traits::NumRowsCols));
*it++ = ScalarType(this->at(source_index));
}
else
@@ -484,17 +608,40 @@ class UAVCAN_EXPORT Array : public ArrayImpl<T, ArrayMode, MaxSize_>
}
}
}
else if (this->size() == MaxSize)
else if (this->size() == Traits::NumElementsInTriangle) // Symmetric
{
OutputIter it = dst_row_major;
SizeType source_index = 0;
for (int row = 0; row < Traits::NumRowsCols; row++)
{
for (int col = 0; col < Traits::NumRowsCols; col++)
{
if (col >= row) // Diagonal or upper-right triangle
{
*it++ = ScalarType(this->at(source_index));
source_index++;
}
else // Lower-left triangle
{
// Transposing one element
*it++ = *(dst_row_major + Traits::computeElementIndexAtRowCol(col, row));
}
}
}
UAVCAN_ASSERT(source_index == Traits::NumElementsInTriangle);
}
else if (this->size() == MaxSize) // Full - no packing whatsoever
{
OutputIter it = dst_row_major;
for (SizeType index = 0; index < MaxSize; index++)
{
*it++ = ScalarType(this->at(index));
}
}
else
else // Everything else
{
// coverity[suspicious_sizeof : FALSE]
::uavcan::fill_n(it, MaxSize, ScalarType(0));
::uavcan::fill_n(dst_row_major, MaxSize, ScalarType(0));
}
}
libuavcan/test/marshal/array.cpp
+110
View File
@@ -1024,6 +1024,73 @@ TEST(Array, SquareMatrixPacking)
ASSERT_EQ(1, m2x2f.size());
ASSERT_FLOAT_EQ(0, m2x2f[0]);
}
// Symmetric matrix will contain only upper-right triangle
{
const float sym2x2[] =
{
1, 2,
2, 1
};
m2x2f.packSquareMatrix(sym2x2);
ASSERT_EQ(3, m2x2f.size());
float sym2x2_out[4];
m2x2f.unpackSquareMatrix(sym2x2_out);
ASSERT_FLOAT_EQ(1, sym2x2_out[0]);
ASSERT_FLOAT_EQ(2, sym2x2_out[1]);
ASSERT_FLOAT_EQ(2, sym2x2_out[2]);
ASSERT_FLOAT_EQ(1, sym2x2_out[3]);
}
{
const float sym3x3[] =
{
1, 2, 3,
2, 4, 5,
3, 5, 6
};
m3x3s.packSquareMatrix(sym3x3);
ASSERT_EQ(6, m3x3s.size());
ASSERT_EQ(1, m3x3s[0]);
ASSERT_EQ(2, m3x3s[1]);
ASSERT_EQ(3, m3x3s[2]);
ASSERT_EQ(4, m3x3s[3]);
ASSERT_EQ(5, m3x3s[4]);
ASSERT_EQ(6, m3x3s[5]);
float sym3x3_out[9];
m3x3s.unpackSquareMatrix(sym3x3_out);
for (int i = 0; i < 9; i++)
{
ASSERT_FLOAT_EQ(sym3x3[i], sym3x3_out[i]);
}
}
{
const double sym6x6[] =
{
1, 2, 3, 4, 5, 6,
2, 7, 8, 9, 10, 11,
3, 8, 12, 13, 14, 15,
4, 9, 13, 16, 17, 18,
5, 10, 14, 17, 19, 20,
6, 11, 15, 18, 20, 21
};
m6x6d.packSquareMatrix(sym6x6);
ASSERT_EQ(21, m6x6d.size());
for (uavcan::uint8_t i = 0; i < 21; i++)
{
ASSERT_DOUBLE_EQ(double(i + 1), m6x6d[i]);
}
double sym6x6_out[36];
m6x6d.unpackSquareMatrix(sym6x6_out);
for (int i = 0; i < 36; i++)
{
ASSERT_DOUBLE_EQ(sym6x6[i], sym6x6_out[i]);
}
}
}
@@ -1195,6 +1262,49 @@ TEST(Array, SquareMatrixPackingInPlace)
const bool diagonal = (i == 0) || (i == 4) || (i == 8);
ASSERT_EQ((diagonal ? 123 : 0), m3x3s[i]);
}
// Try again with symmetric matrix
/*
* Full matrix:
* 1 2 3
* 2 4 5
* 3 5 6
* Compressed triangle:
* 1 2 3
* 4 5
* 6
*/
m3x3s.clear();
m3x3s.push_back(1);
m3x3s.push_back(2);
m3x3s.push_back(3);
m3x3s.push_back(4);
m3x3s.push_back(5);
m3x3s.push_back(6);
// Unpacking
ASSERT_EQ(6, m3x3s.size());
m3x3s.unpackSquareMatrix();
ASSERT_EQ(9, m3x3s.size());
// Validating
ASSERT_EQ(1, m3x3s[0]);
ASSERT_EQ(2, m3x3s[1]);
ASSERT_EQ(3, m3x3s[2]);
ASSERT_EQ(2, m3x3s[3]);
ASSERT_EQ(4, m3x3s[4]);
ASSERT_EQ(5, m3x3s[5]);
ASSERT_EQ(3, m3x3s[6]);
ASSERT_EQ(5, m3x3s[7]);
ASSERT_EQ(6, m3x3s[8]);
// Packing back
m3x3s.packSquareMatrix();
ASSERT_EQ(6, m3x3s.size());
// Validating
ASSERT_EQ(1, m3x3s[0]);
ASSERT_EQ(2, m3x3s[1]);
ASSERT_EQ(3, m3x3s[2]);
ASSERT_EQ(4, m3x3s[3]);
ASSERT_EQ(5, m3x3s[4]);
ASSERT_EQ(6, m3x3s[5]);
}
TEST(Array, FuzzyComparison)