mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-12 22:43:34 +08:00
Array::packSquareMatrix(), Array::unpackSquareMatrix()
This commit is contained in:
Pavel Kirienko
@@ -10,6 +10,7 @@
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
#include <uavcan/impl_constants.hpp>
|
||||
@@ -336,6 +337,108 @@ class Array : public ArrayImpl<T, ArrayMode, MaxSize_>
|
||||
return -ErrLogic;
|
||||
}
|
||||
|
||||
template <typename InputIter>
|
||||
void packSquareMatrixImpl(const InputIter src_row_major)
|
||||
{
|
||||
StaticAssert<IsDynamic>::check();
|
||||
enum { 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);
|
||||
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
|
||||
const bool nan = std::isnan(*it);
|
||||
#else
|
||||
const bool nan = (*it) != (*it);
|
||||
#endif
|
||||
if (!nan)
|
||||
{
|
||||
all_nans = false;
|
||||
}
|
||||
if (!on_diagonal && (*it) != 0) // TODO: Proper float comparison
|
||||
{
|
||||
scalar_matrix = false; // This matrix cannot be compressed.
|
||||
diagonal_matrix = false;
|
||||
break;
|
||||
}
|
||||
if (on_diagonal && (*it) != (*src_row_major)) // TODO: Proper float comparison
|
||||
{
|
||||
scalar_matrix = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
/*
|
||||
* Actual packing is performed here.
|
||||
*/
|
||||
this->clear();
|
||||
if (!all_nans)
|
||||
{
|
||||
unsigned index = 0;
|
||||
for (InputIter it = src_row_major; index < MaxSize; ++it, ++index)
|
||||
{
|
||||
const bool on_diagonal = (index / Width) == (index % Width);
|
||||
if (diagonal_matrix && !on_diagonal)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
this->push_back(ValueType(*it));
|
||||
if (scalar_matrix)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename OutputIter>
|
||||
void unpackSquareMatrixImpl(OutputIter it) const
|
||||
{
|
||||
StaticAssert<IsDynamic>::check();
|
||||
enum { 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)
|
||||
{
|
||||
for (unsigned index = 0; index < MaxSize; index++)
|
||||
{
|
||||
const bool on_diagonal = (index / Width) == (index % Width);
|
||||
if (on_diagonal)
|
||||
{
|
||||
const SizeType source_index = (this->size() == 1) ? 0 : (index / Width);
|
||||
*it++ = at(source_index);
|
||||
}
|
||||
else
|
||||
{
|
||||
*it++ = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (this->size() == MaxSize)
|
||||
{
|
||||
std::copy(this->begin(), this->end(), it);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::fill_n(it, MaxSize, 0);
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
typedef T RawValueType;
|
||||
typedef typename StorageType<T>::Type ValueType;
|
||||
@@ -517,6 +620,72 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Fills this array as a packed square matrix from a static array.
|
||||
*/
|
||||
template <typename ScalarType>
|
||||
void packSquareMatrix(const ScalarType (&src_row_major)[MaxSize])
|
||||
{
|
||||
packSquareMatrixImpl<const ScalarType*>(src_row_major);
|
||||
}
|
||||
|
||||
/**
|
||||
* Fills this array as a packed square matrix from any container that implements the methods begin() and size().
|
||||
*/
|
||||
template <typename R>
|
||||
typename EnableIf<sizeof(((const R*)(0U))->begin()) && sizeof(((const R*)(0U))->size())>::Type
|
||||
packSquareMatrix(const R& src_row_major)
|
||||
{
|
||||
if (src_row_major.size() == MaxSize)
|
||||
{
|
||||
packSquareMatrixImpl(src_row_major.begin());
|
||||
}
|
||||
else if (src_row_major.size() == 0)
|
||||
{
|
||||
this->clear();
|
||||
}
|
||||
else
|
||||
{
|
||||
#if UAVCAN_EXCEPTIONS
|
||||
throw std::out_of_range("uavcan::Array::packSquareMatrix()");
|
||||
#else
|
||||
assert(0);
|
||||
this->clear();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Reconstructs full matrix, result will be saved into a static array.
|
||||
*/
|
||||
template <typename ScalarType>
|
||||
void unpackSquareMatrix(ScalarType (&dst_row_major)[MaxSize]) const
|
||||
{
|
||||
unpackSquareMatrixImpl<ScalarType*>(dst_row_major);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reconstructs full matrix, result will be saved into container that implements begin() and size().
|
||||
*/
|
||||
template <typename R>
|
||||
typename EnableIf<sizeof(((const R*)(0U))->begin()) && sizeof(((const R*)(0U))->size())>::Type
|
||||
unpackSquareMatrix(R& dst_row_major) const
|
||||
{
|
||||
if (dst_row_major.size() == MaxSize)
|
||||
{
|
||||
unpackSquareMatrixImpl(dst_row_major.begin());
|
||||
}
|
||||
else
|
||||
{
|
||||
#if UAVCAN_EXCEPTIONS
|
||||
throw std::out_of_range("uavcan::Array::unpackSquareMatrix()");
|
||||
#else
|
||||
assert(0);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
typedef ValueType value_type;
|
||||
typedef SizeType size_type;
|
||||
};
|
||||
|
||||
@@ -118,4 +118,17 @@ To try_implicit_cast(const From& from)
|
||||
BooleanType<IsImplicitlyConvertibleFromTo<From, To>::Result>());
|
||||
}
|
||||
|
||||
/**
|
||||
* Some arithmetics
|
||||
*/
|
||||
template <unsigned Value> struct CompileTimeIntSqrt;
|
||||
template <> struct CompileTimeIntSqrt<4> { enum { Result = 2 }; };
|
||||
template <> struct CompileTimeIntSqrt<9> { enum { Result = 3 }; };
|
||||
template <> struct CompileTimeIntSqrt<16> { enum { Result = 4 }; };
|
||||
template <> struct CompileTimeIntSqrt<25> { enum { Result = 5 }; };
|
||||
template <> struct CompileTimeIntSqrt<36> { enum { Result = 6 }; };
|
||||
template <> struct CompileTimeIntSqrt<49> { enum { Result = 7 }; };
|
||||
template <> struct CompileTimeIntSqrt<64> { enum { Result = 8 }; };
|
||||
template <> struct CompileTimeIntSqrt<81> { enum { Result = 9 }; };
|
||||
|
||||
}
|
||||
|
||||
@@ -832,3 +832,240 @@ TEST(Array, MultidimensionalStreaming)
|
||||
uavcan::YamlStreamer<ThreeDimensional>::stream(std::cout, threedee, 0);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
||||
TEST(Array, SquareMatrixPacking)
|
||||
{
|
||||
Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s;
|
||||
Array<FloatSpec<32, CastModeSaturate>, ArrayModeDynamic, 4> m2x2f;
|
||||
Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 36> m6x6d;
|
||||
|
||||
// NAN will be reduced to empty array
|
||||
{
|
||||
const double nans3x3[] =
|
||||
{
|
||||
NAN, NAN, NAN,
|
||||
NAN, NAN, NAN,
|
||||
NAN, NAN, NAN
|
||||
};
|
||||
m3x3s.packSquareMatrix(nans3x3);
|
||||
ASSERT_EQ(0, m3x3s.size());
|
||||
|
||||
// Empty array will be decoded as zero matrix
|
||||
double nans3x3_out[9];
|
||||
m3x3s.unpackSquareMatrix(nans3x3_out);
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(0, nans3x3_out[i]);
|
||||
}
|
||||
}
|
||||
{
|
||||
std::vector<double> empty;
|
||||
m3x3s.packSquareMatrix(empty);
|
||||
ASSERT_EQ(0, m3x3s.size());
|
||||
|
||||
empty.resize(9);
|
||||
m3x3s.unpackSquareMatrix(empty);
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(0, empty.at(i));
|
||||
}
|
||||
}
|
||||
|
||||
// Scalar matrix will be reduced to a single value
|
||||
{
|
||||
std::vector<float> scalar2x2(4);
|
||||
scalar2x2[0] = scalar2x2[3] = 3.14;
|
||||
m2x2f.packSquareMatrix(scalar2x2);
|
||||
ASSERT_EQ(1, m2x2f.size());
|
||||
ASSERT_FLOAT_EQ(3.14, m2x2f[0]);
|
||||
|
||||
m2x2f.unpackSquareMatrix(scalar2x2);
|
||||
const float reference[] =
|
||||
{
|
||||
3.14, 0,
|
||||
0, 3.14
|
||||
};
|
||||
ASSERT_TRUE(std::equal(scalar2x2.begin(), scalar2x2.end(), reference));
|
||||
}
|
||||
{
|
||||
const float scalar6x6[] =
|
||||
{
|
||||
-18, 0, 0, 0, 0, 0,
|
||||
0, -18, 0, 0, 0, 0,
|
||||
0, 0, -18, 0, 0, 0,
|
||||
0, 0, 0, -18, 0, 0,
|
||||
0, 0, 0, 0, -18, 0,
|
||||
0, 0, 0, 0, 0, -18
|
||||
};
|
||||
m6x6d.packSquareMatrix(scalar6x6);
|
||||
ASSERT_EQ(1, m6x6d.size());
|
||||
ASSERT_FLOAT_EQ(-18, m6x6d[0]);
|
||||
|
||||
std::vector<long double> output(36);
|
||||
m6x6d.unpackSquareMatrix(output);
|
||||
ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar6x6));
|
||||
}
|
||||
|
||||
// Diagonal matrix will be reduced to an array of length Width
|
||||
{
|
||||
const float diagonal6x6[] =
|
||||
{
|
||||
1, 0, 0, 0, 0, 0,
|
||||
0, -2, 0, 0, 0, 0,
|
||||
0, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, -4, 0, 0,
|
||||
0, 0, 0, 0, 5, 0,
|
||||
0, 0, 0, 0, 0, -6
|
||||
};
|
||||
m6x6d.packSquareMatrix(diagonal6x6);
|
||||
ASSERT_EQ(6, m6x6d.size());
|
||||
ASSERT_FLOAT_EQ(1, m6x6d[0]);
|
||||
ASSERT_FLOAT_EQ(-2, m6x6d[1]);
|
||||
ASSERT_FLOAT_EQ(3, m6x6d[2]);
|
||||
ASSERT_FLOAT_EQ(-4, m6x6d[3]);
|
||||
ASSERT_FLOAT_EQ(5, m6x6d[4]);
|
||||
ASSERT_FLOAT_EQ(-6, m6x6d[5]);
|
||||
|
||||
std::vector<long double> output(36);
|
||||
m6x6d.unpackSquareMatrix(output);
|
||||
ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6));
|
||||
}
|
||||
|
||||
// A matrix filled with random values will not be compressed
|
||||
{
|
||||
std::vector<float> full3x3(9);
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
full3x3[i] = i;
|
||||
}
|
||||
m3x3s.packSquareMatrix(full3x3);
|
||||
ASSERT_EQ(9, m3x3s.size());
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(i, m3x3s[i]);
|
||||
}
|
||||
|
||||
long output[9];
|
||||
m3x3s.unpackSquareMatrix(output);
|
||||
ASSERT_TRUE(std::equal(full3x3.begin(), full3x3.end(), output));
|
||||
}
|
||||
|
||||
// This will be represented as diagonal - NANs are exceptional
|
||||
{
|
||||
const double scalarnan3x3[] =
|
||||
{
|
||||
NAN, 0, 0,
|
||||
0, NAN, 0,
|
||||
0, 0, NAN
|
||||
};
|
||||
m3x3s.packSquareMatrix(scalarnan3x3);
|
||||
ASSERT_EQ(3, m3x3s.size());
|
||||
ASSERT_FALSE(m3x3s[0] == m3x3s[0]); // NAN
|
||||
ASSERT_FALSE(m3x3s[1] == m3x3s[1]); // NAN
|
||||
ASSERT_FALSE(m3x3s[2] == m3x3s[2]); // NAN
|
||||
|
||||
float output[9];
|
||||
m3x3s.unpackSquareMatrix(output);
|
||||
ASSERT_FALSE(output[0] == output[0]); // NAN
|
||||
ASSERT_EQ(0, output[1]);
|
||||
ASSERT_EQ(0, output[2]);
|
||||
ASSERT_EQ(0, output[3]);
|
||||
ASSERT_FALSE(output[4] == output[4]); // NAN
|
||||
ASSERT_EQ(0, output[5]);
|
||||
ASSERT_EQ(0, output[6]);
|
||||
ASSERT_EQ(0, output[7]);
|
||||
ASSERT_FALSE(output[8] == output[8]); // NAN
|
||||
}
|
||||
|
||||
// This is a full matrix too (notice the NAN)
|
||||
{
|
||||
const float full2x2[] =
|
||||
{
|
||||
1, NAN,
|
||||
0, -2
|
||||
};
|
||||
m2x2f.packSquareMatrix(full2x2);
|
||||
ASSERT_EQ(4, m2x2f.size());
|
||||
ASSERT_FLOAT_EQ(1, m2x2f[0]);
|
||||
ASSERT_FALSE(m2x2f[1] == m2x2f[1]); // NAN
|
||||
ASSERT_FLOAT_EQ(0, m2x2f[2]);
|
||||
ASSERT_FLOAT_EQ(-2, m2x2f[3]);
|
||||
|
||||
float output[4];
|
||||
m2x2f.unpackSquareMatrix(output);
|
||||
ASSERT_EQ(1, output[0]);
|
||||
ASSERT_FALSE(output[1] == output[1]); // NAN
|
||||
ASSERT_EQ(0, output[2]);
|
||||
ASSERT_EQ(-2, output[3]);
|
||||
}
|
||||
|
||||
// Zero matrix will be represented as scalar matrix
|
||||
{
|
||||
const float zero2x2[] =
|
||||
{
|
||||
0, 0,
|
||||
0, 0
|
||||
};
|
||||
m2x2f.packSquareMatrix(zero2x2);
|
||||
ASSERT_EQ(1, m2x2f.size());
|
||||
ASSERT_FLOAT_EQ(0, m2x2f[0]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST(Array, SquareMatrixPackingIntegers)
|
||||
{
|
||||
Array<IntegerSpec<30, SignednessSigned, CastModeSaturate>, ArrayModeDynamic, 9> m3x3int;
|
||||
{
|
||||
const long scalar[] =
|
||||
{
|
||||
42, 0, 0,
|
||||
0, 42, 0,
|
||||
0, 0, 42
|
||||
};
|
||||
m3x3int.packSquareMatrix(scalar);
|
||||
ASSERT_EQ(1, m3x3int.size());
|
||||
ASSERT_EQ(42, m3x3int[0]);
|
||||
|
||||
std::vector<int> output(9);
|
||||
m3x3int.unpackSquareMatrix(output);
|
||||
ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar));
|
||||
}
|
||||
{
|
||||
std::vector<short> diagonal(9);
|
||||
diagonal[0] = 6;
|
||||
diagonal[4] = -57;
|
||||
diagonal[8] = 1139;
|
||||
m3x3int.packSquareMatrix(diagonal);
|
||||
ASSERT_EQ(3, m3x3int.size());
|
||||
ASSERT_EQ(6, m3x3int[0]);
|
||||
ASSERT_EQ(-57, m3x3int[1]);
|
||||
ASSERT_EQ(1139, m3x3int[2]);
|
||||
}
|
||||
{
|
||||
std::vector<long double> full(9);
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
full[i] = i;
|
||||
}
|
||||
m3x3int.packSquareMatrix(full);
|
||||
ASSERT_EQ(9, m3x3int.size());
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
ASSERT_EQ(i, m3x3int[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if UAVCAN_EXCEPTIONS
|
||||
TEST(Array, SquareMatrixPackingErrors)
|
||||
{
|
||||
Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s;
|
||||
|
||||
std::vector<float> ill_formed_row_major(8);
|
||||
ASSERT_THROW(m3x3s.packSquareMatrix(ill_formed_row_major), std::out_of_range);
|
||||
|
||||
ASSERT_THROW(m3x3s.unpackSquareMatrix(ill_formed_row_major), std::out_of_range);
|
||||
}
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user