69 lines
1.9 KiB
C++
69 lines
1.9 KiB
C++
|
#pragma once
|
||
|
|
#ifndef __CVTT_COVARIANCEMATRIX_H__
|
||
|
|
#define __CVTT_COVARIANCEMATRIX_H__
|
||
|
|
|
||
|
|
namespace cvtt
|
||
|
|
{
|
||
|
|
namespace Internal
|
||
|
|
{
|
||
|
|
|
||
|
|
template<int TMatrixSize>
|
||
|
|
class PackedCovarianceMatrix
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
// 0: xx,
|
||
|
|
// 1: xy, yy
|
||
|
|
// 3: xz, yz, zz
|
||
|
|
// 6: xw, yw, zw, ww
|
||
|
|
// ... etc.
|
||
|
|
static const int PyramidSize = (TMatrixSize * (TMatrixSize + 1)) / 2;
|
||
|
|
|
||
|
|
typedef ParallelMath::Float MFloat;
|
||
|
|
|
||
|
|
PackedCovarianceMatrix()
|
||
|
|
{
|
||
|
|
for (int i = 0; i < PyramidSize; i++)
|
||
|
|
m_values[i] = ParallelMath::MakeFloatZero();
|
||
|
|
}
|
||
|
|
|
||
|
|
void Add(const ParallelMath::Float *vec, const ParallelMath::Float &weight)
|
||
|
|
{
|
||
|
|
int index = 0;
|
||
|
|
for (int row = 0; row < TMatrixSize; row++)
|
||
|
|
{
|
||
|
|
for (int col = 0; col <= row; col++)
|
||
|
|
{
|
||
|
|
m_values[index] = m_values[index] + vec[row] * vec[col] * weight;
|
||
|
|
index++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void Product(MFloat *outVec, const MFloat *inVec)
|
||
|
|
{
|
||
|
|
for (int row = 0; row < TMatrixSize; row++)
|
||
|
|
{
|
||
|
|
MFloat sum = ParallelMath::MakeFloatZero();
|
||
|
|
|
||
|
|
int index = (row * (row + 1)) >> 1;
|
||
|
|
for (int col = 0; col < TMatrixSize; col++)
|
||
|
|
{
|
||
|
|
sum = sum + inVec[col] * m_values[index];
|
||
|
|
if (col >= row)
|
||
|
|
index += col + 1;
|
||
|
|
else
|
||
|
|
index++;
|
||
|
|
}
|
||
|
|
|
||
|
|
outVec[row] = sum;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
ParallelMath::Float m_values[PyramidSize];
|
||
|
|
};
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|