From af50ae73cfe6f1533e5390e5131e25c6d322dc8a Mon Sep 17 00:00:00 2001 From: cam Date: Wed, 4 Jan 2017 23:09:30 -0700 Subject: removed .cpp and .h files --- video_input/consistencyChecker/CFilter.h | 2210 ---------------------------- video_input/consistencyChecker/CMatrix.h | 1396 ------------------ video_input/consistencyChecker/CTensor.h | 1205 --------------- video_input/consistencyChecker/CTensor4D.h | 656 --------- video_input/consistencyChecker/CVector.h | 519 ------- video_input/consistencyChecker/Makefile | 3 - video_input/consistencyChecker/NMath.cpp | 664 --------- video_input/consistencyChecker/NMath.h | 170 --- 8 files changed, 6823 deletions(-) delete mode 100644 video_input/consistencyChecker/CFilter.h delete mode 100644 video_input/consistencyChecker/CMatrix.h delete mode 100644 video_input/consistencyChecker/CTensor.h delete mode 100644 video_input/consistencyChecker/CTensor4D.h delete mode 100644 video_input/consistencyChecker/CVector.h delete mode 100644 video_input/consistencyChecker/Makefile delete mode 100644 video_input/consistencyChecker/NMath.cpp delete mode 100644 video_input/consistencyChecker/NMath.h diff --git a/video_input/consistencyChecker/CFilter.h b/video_input/consistencyChecker/CFilter.h deleted file mode 100644 index 29bef9e..0000000 --- a/video_input/consistencyChecker/CFilter.h +++ /dev/null @@ -1,2210 +0,0 @@ -// - Classes for 1D and 2D convolution stencils -// - Pre-defined convolution stencils for binomial filters -// - Pre-defined convolution stencils for 1st, 2nd, 3rd and 4th derivatives up to order 10 -// - Functions for convolution -// -// Author: Thomas Brox - -#ifndef CFILTER -#define CFILTER - -#include -#include -#include -#include -#include -#include - -// CFilter is an extention of CVector. It has an additional property Delta -// which shifts the data to the left (a vector always begins with index 0). -// This enables a filter's range to go from A to B where A can also -// be less than zero. -// -// Example: -// CFilter filter(3,1); -// filter = 1.0; -// cout << filter(-1) << ", " << filter(0) << ", " << filter(1) << endl; -// -// CFilter2D behaves the same way as CFilter but is an extension of CMatrix - -template -class CFilter : public CVector { -public: - // constructor - inline CFilter(const int aSize, const int aDelta = 0); - // copy constructor - CFilter(const CFilter& aCopyFrom); - // constructor initialized by a vector - CFilter(const CVector& aCopyFrom, const int aDelta = 0); - - // Access to the filter's values - inline T& operator()(const int aIndex) const; - inline T& operator[](const int aIndex) const; - // Copies a filter into this filter - CFilter& operator=(const CFilter& aCopyFrom); - - // Access to the filter's delta - inline int delta() const; - // Access to the filter's range A<=i -class CFilter2D : public CMatrix { -public: - // constructor - inline CFilter2D(); - inline CFilter2D(const int aXSize, const int aYSize, const int aXDelta = 0, const int aYDelta = 0); - // copy contructor - CFilter2D(const CFilter2D& aCopyFrom); - // constructor initialized by a matrix - CFilter2D(const CMatrix& aCopyFrom, const int aXDelta = 0, const int aYDelta = 0); - // Normalize sum of values to 1.0 - void normalizeSum(); - // Moves the filter's center - void shift(int aXDelta, int aYDelta); - - // Access to filter's values - inline T& operator()(const int ax, const int ay) const; - // Copies a filter into this filter - CFilter2D& operator=(const CFilter2D& aCopyFrom); - - // Access to the filter's delta - inline int deltaX() const; - inline int deltaY() const; - // Access to the filter's range A<=i inline void filter(CVector& aVector, const CFilter& aFilter); - // Convolution of the vector aVector with aFilter, the initial values of aVector will persist. - template void filter(const CVector& aVector, CVector& aResult, const CFilter& aFilter); - - // Convolution with a rectangle -> approximation of Gaussian - template inline void boxFilter(CVector& aVector, int aWidth); - template void boxFilter(const CVector& aVector, CVector& aResult, int aWidth); - - // Linear 2D filtering - - // Convolution of the matrix aMatrix with aFilter, aFilter should be a separable filter - // The result will be written into aMatrix, so its initial values will get lost - template inline void filter(CMatrix& aMatrix, const CFilter& aFilterX, const CFilter& aFilterY); - template inline void filterMin(CMatrix& aMatrix, const CFilter& aFilterX, const CFilter& aFilterY); - // Convolution of the matrix aMatrix with aFilter, aFilter must be separable - // The initial values of aMatrix will persist. - template inline void filter(const CMatrix& aMatrix, CMatrix& aResult, const CFilter& aFilterX, const CFilter& aFilterY); - template inline void filterMin(const CMatrix& aMatrix, CMatrix& aResult, const CFilter& aFilterX, const CFilter& aFilterY); - - // Convolution of the matrix aMatrix with aFilter only in x-direction, aDummy can be set to 1 - // The result will be written into aMatrix, so its initial values will get lost - template inline void filter(CMatrix& aMatrix, const CFilter& aFilter, const int aDummy); - template inline void filterMin(CMatrix& aMatrix, const CFilter& aFilter, const int aDummy); - // Convolution of the matrix aMatrix with aFilter only in x-direction, aDummy can be set to 1 - // The initial values of aMatrix will persist. - template void filter(const CMatrix& aMatrix, CMatrix& aResult, const CFilter& aFilter, const int aDummy); - template void filterMin(const CMatrix& aMatrix, const CMatrix& aOrig, CMatrix& aResult, const CFilter& aFilter, const int aDummy); - // Convolution of the matrix aMatrix with aFilter only in y-direction, aDummy can be set to 1 - // The result will be written into aMatrix, so its initial values will get lost - template inline void filter(CMatrix& aMatrix, const int aDummy, const CFilter& aFilter); - template inline void filterMin(CMatrix& aMatrix, const int aDummy, const CFilter& aFilter); - // Convolution of the matrix aMatrix with aFilter only in y-direction, aDummy can be set to 1 - // The initial values of aMatrix will persist. - template void filter(const CMatrix& aMatrix, CMatrix& aResult, const int aDummy, const CFilter& aFilter); - template void filterMin(const CMatrix& aMatrix, const CMatrix& aOrig, CMatrix& aResult, const int aDummy, const CFilter& aFilter); - - // Convolution of the matrix aMatrix with aFilter - // The result will be written to aMatrix, so its initial values will get lost - template inline void filter(CMatrix& aMatrix, const CFilter2D& aFilter); - // Convolution of the matrix aMatrix with aFilter, the initial values of aMatrix will persist - template void filter(const CMatrix& aMatrix, CMatrix& aResult, const CFilter2D& aFilter); - - // Convolution with a rectangle -> approximation of Gaussian - template inline void boxFilterX(CMatrix& aMatrix, int aWidth); - template void boxFilterX(const CMatrix& aMatrix, CMatrix& aResult, int aWidth); - template inline void boxFilterY(CMatrix& aMatrix, int aWidth); - template void boxFilterY(const CMatrix& aMatrix, CMatrix& aResult, int aWidth); - - // Recursive filter -> approximation of Gaussian - template void recursiveSmoothX(CMatrix& aMatrix, float aSigma); - template void recursiveSmoothY(CMatrix& aMatrix, float aSigma); - template inline void recursiveSmooth(CMatrix& aMatrix, float aSigma); - - // Linear 3D filtering - - // Convolution of the 3D Tensor aTensor with aFilter, aFilter must be separable - // The result will be written back to aTensor so its initial values will get lost - template inline void filter(CTensor& aTensor, const CFilter& aFilterX, const CFilter& aFilterY, const CFilter& aFilterZ); - // Convolution of the 3D Tensor aTensor with aFilter, aFilter must be separable - // The initial values of aTensor will persist - template inline void filter(const CTensor& aTensor, CTensor& aResult, const CFilter& aFilterX, const CFilter& aFilterY, const CFilter& aFilterZ); - - // Convolution of the 3D Tensor aTensor with aFilter only in x-Direction - template inline void filter(CTensor& aTensor, const CFilter& aFilter, const int aDummy1, const int aDummy2); - template void filter(const CTensor& aTensor, CTensor& aResult, const CFilter& aFilter, const int aDummy1, const int aDummy2); - // Convolution of the 3D Tensor aTensor with aFilter only in y-Direction - template inline void filter(CTensor& aTensor, const int aDummy1, const CFilter& aFilter, const int aDummy2); - template void filter(const CTensor& aTensor, CTensor& aResult, const int aDummy1, const CFilter& aFilter, const int aDummy2); - // Convolution of the 3D Tensor aTensor with aFilter only in z-Direction - template inline void filter(CTensor& aTensor, const int aDummy1, const int aDummy2, const CFilter& aFilter); - template void filter(const CTensor& aTensor, CTensor& aResult, const int aDummy1, const int aDummy2, const CFilter& aFilter); - - // Convolution with a rectangle -> approximation of Gaussian - template inline void boxFilterX(CTensor& aTensor, int aWidth); - template void boxFilterX(const CTensor& aTensor, CTensor& aResult, int aWidth); - template inline void boxFilterY(CTensor& aTensor, int aWidth); - template void boxFilterY(const CTensor& aTensor, CTensor& aResult, int aWidth); - template inline void boxFilterZ(CTensor& aTensor, int aWidth); - template void boxFilterZ(const CTensor& aTensor, CTensor& aResult, int aWidth); - - // Recursive filter -> approximation of Gaussian - template void recursiveSmoothX(CTensor& aTensor, float aSigma); - template void recursiveSmoothY(CTensor& aTensor, float aSigma); - template void recursiveSmoothZ(CTensor& aTensor, float aSigma); - - // Linear 4D filtering - - // Convolution of the 4D Tensor aTensor with aFilter, aFilter must be separable - // The result will be written back to aTensor so its initial values will get lost - template inline void filter(CTensor4D& aTensor, const CFilter& aFilterX, const CFilter& aFilterY, const CFilter& aFilterZ, const CFilter& aFilterA); - - // Convolution of the 4D Tensor aTensor with aFilter only in x-Direction - template inline void filter(CTensor4D& aTensor, const CFilter& aFilter, const int aDummy1, const int aDummy2, const int aDummy3); - template void filter(const CTensor4D& aTensor, CTensor4D& aResult, const CFilter& aFilter, const int aDummy1, const int aDummy2, const int aDummy3); - // Convolution of the 4D Tensor aTensor with aFilter only in y-Direction - template inline void filter(CTensor4D& aTensor, const int aDummy1, const CFilter& aFilter, const int aDummy2, const int aDummy3); - template void filter(const CTensor4D& aTensor, CTensor4D& aResult, const int aDummy1, const CFilter& aFilter, const int aDummy2, const int aDummy3); - // Convolution of the 4D Tensor aTensor with aFilter only in z-Direction - template inline void filter(CTensor4D& aTensor, const int aDummy1, const int aDummy2, const CFilter& aFilter, const int aDummy3); - template void filter(const CTensor4D& aTensor, CTensor4D& aResult, const int aDummy1, const int aDummy2, const CFilter& aFilter, const int aDummy3); - // Convolution of the 4D Tensor aTensor with aFilter only in a-Direction - template inline void filter(CTensor4D& aTensor, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter& aFilter); - template void filter(const CTensor4D& aTensor, CTensor4D& aResult, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter& aFilter); - - // Recursive filter -> approximation of Gaussian - template void recursiveSmoothX(CTensor4D& aTensor, float aSigma); - template void recursiveSmoothY(CTensor4D& aTensor, float aSigma); - template void recursiveSmoothZ(CTensor4D& aTensor, float aSigma); - template void recursiveSmoothA(CTensor4D& aTensor, float aSigma); - - // Nonlinear filtering: Osher shock filter - template void osher(CMatrix& aData, int aIterations = 20); - template inline void osher(const CMatrix& aData, CMatrix& aResult, int aIterations = 20); -} - -// Common filters - -template -class CGauss : public CFilter { -public: - CGauss(const int aSize, const int aDegreeOfDerivative); -}; - -template -class CSmooth : public CFilter { -public: - CSmooth(float aSigma, float aPrecision); -}; - -template -class CGaussianFirstDerivative : public CFilter { -public: - CGaussianFirstDerivative(float aSigma, float aPrecision); -}; - -template -class CGaussianSecondDerivative : public CFilter { -public: - CGaussianSecondDerivative(float aSigma, float aPrecision); -}; - -template -class CDerivative : public CFilter { -public: - CDerivative(const int aSize); -}; - -template -class CHighOrderDerivative : public CFilter { -public: - CHighOrderDerivative(int aOrder, int aSize); -}; - -template -class CGaborReal : public CFilter2D { -public: - CGaborReal(float aFrequency, float aAngle, float aSigma1 = 3.0, float aSigma2 = 3.0); -}; - -template -class CGaborImaginary : public CFilter2D { -public: - CGaborImaginary(float aFrequency, float aAngle, float aSigma1 = 3.0, float aSigma2 = 3.0); -}; - -// Exceptions ----------------------------------------------------------------- - -// Thrown if one tries to access an element of a filter which is out of the filter's bounds -struct EFilterRangeOverflow { - EFilterRangeOverflow(const int aIndex, const int aA, const int aB) { - using namespace std; - cerr << "Exception EFilterRangeOverflow: i = " << aIndex; - cerr << " Allowed Range: " << aA << " <= i < " << aB << endl; - } - EFilterRangeOverflow(const int ax, const int ay, const int aAX, const int aBX, const int aAY, const int aBY) { - using namespace std; - cerr << "Exception EFilterRangeOverflow: (x,y) = (" << ax << "," << ay << ") "; - cerr << "Allowed Range: " << aAX << " <= x < " << aBX << " " << aAY << " <= y < " << aBY << endl; - } -}; - -// Thrown if the resulting container has not the same size as the initial container -struct EFilterIncompatibleSize { - EFilterIncompatibleSize(const int aSize1, const int aSize2) { - using namespace std; - cerr << "Exception EFilterIncompatibleSize: Initial container size: " << aSize1; - cerr << " Resulting container size: " << aSize2 << endl; - } -}; - -// Thrown if the demanded filter is not available -struct EFilterNotAvailable { - EFilterNotAvailable(int aSize, int aOrder) { - using namespace std; - cerr << "Exception EFilterNotAvailable: Mask size: " << aSize; - if (aOrder >= 0) cerr << " Derivative order: " << aOrder; - cerr << endl; - } -}; - -// I M P L E M E N T A T I O N ------------------------------------------------ -// -// You might wonder why there is implementation code in a header file. -// The reason is that not all C++ compilers yet manage separate compilation -// of templates. Inline functions cannot be compiled separately anyway. -// So in this case the whole implementation code is added to the header -// file. -// Users should ignore everything that's beyond this line :) -// ---------------------------------------------------------------------------- - -// C F I L T E R -------------------------------------------------------------- -// P U B L I C ---------------------------------------------------------------- -// constructor -template -inline CFilter::CFilter(const int aSize, const int aDelta) - : CVector(aSize),mDelta(aDelta) { -} - -// copy constructor -template -CFilter::CFilter(const CFilter& aCopyFrom) - : CVector(aCopyFrom.mSize),mDelta(aCopyFrom.mDelta) { - for (register int i = 0; i < this->mSize; i++) - this->mData[i] = aCopyFrom.mData[i]; -} - -// constructor initialized by a vector -template -CFilter::CFilter(const CVector& aCopyFrom, const int aDelta) - : CVector(aCopyFrom.size()),mDelta(aDelta) { - for (register int i = 0; i < this->mSize; i++) - this->mData[i] = aCopyFrom(i); -} - -// operator() -template -inline T& CFilter::operator()(const int aIndex) const { - #ifdef DEBUG - if (aIndex < A() || aIndex >= B()) - throw EFilterRangeOverflow(aIndex,A(),B()); - #endif - return this->mData[aIndex+mDelta]; -} - -// operator[] -template -inline T& CFilter::operator[](const int aIndex) const { - return operator()(aIndex); -} - -// operator= -template -CFilter& CFilter::operator=(const CFilter& aCopyFrom) { - if (this != &aCopyFrom) { - delete[] this->mData; - this->mSize = aCopyFrom.mSize; - mDelta = aCopyFrom.mDelta; - this->mData = new T[this->mSize]; - for (register int i = 0; i < this->mSize; i++) - this->mData[i] = aCopyFrom.mData[i]; - } - return *this; -} - -// delta -template -inline int CFilter::delta() const { - return mDelta; -} - -// A -template -inline int CFilter::A() const { - return -mDelta; -} - -// B -template -inline int CFilter::B() const { - return this->mSize-mDelta; -} - -// sum -template -T CFilter::sum() const { - T aResult = 0; - for (int i = 0; i < this->mSize; i++) - aResult += fabs(this->mData[i]); - return aResult; -} - -// shift -template -inline void CFilter::shift(int aDelta) { - mDelta += aDelta; -} - -// C F I L T E R 2 D ----------------------------------------------------------- -// P U B L I C ---------------------------------------------------------------- -// constructor -template -inline CFilter2D::CFilter2D() - : CMatrix(),mDeltaX(0),mDeltaY(0) { -} - -template -inline CFilter2D::CFilter2D(const int aXSize, const int aYSize, const int aDeltaX, const int aDeltaY) - : CMatrix(aXSize,aYSize),mDeltaX(aDeltaX),mDeltaY(aDeltaY) { -} - -// copy constructor -template -CFilter2D::CFilter2D(const CFilter2D& aCopyFrom) - : CMatrix(aCopyFrom.mXSize,aCopyFrom.mYSize),mDeltaX(aCopyFrom.mDeltaX,aCopyFrom.mDeltaY) { - for (int i = 0; i < this->mXSize*this->mYSize; i++) - this->mData[i] = aCopyFrom.mData[i]; -} - -// constructor initialized by a matrix -template -CFilter2D::CFilter2D(const CMatrix& aCopyFrom, const int aDeltaX, const int aDeltaY) - : CMatrix(aCopyFrom.xSize(),aCopyFrom.ySize()),mDeltaX(aDeltaX),mDeltaY(aDeltaY) { - for (register int i = 0; i < this->mXSize*this->mYSize; i++) - this->mData[i] = aCopyFrom.data()[i]; -} - -// normalizeSum -template -void CFilter2D::normalizeSum() { - int aSize = this->size(); - T aSum = 0; - for (int i = 0; i < aSize; i++) - aSum += this->mData[i]; - T invSum = 1.0/aSum; - for (int i = 0; i < aSize; i++) - this->mData[i] *= invSum; -} - -// shift -template -void CFilter2D::shift(int aXDelta, int aYDelta) { - mDeltaX = aXDelta; - mDeltaY = aYDelta; -} - -// operator() -template -inline T& CFilter2D::operator()(const int ax, const int ay) const { - #ifdef DEBUG - if (ax < AX() || ax >= BX() || ay < AY() || ay >= BY) - throw EFilterRangeOverflow(ax,ay,AX(),BX(),AY(),BY()); - #endif - return this->mData[(ay+mDeltaY)*this->mXSize+ax+mDeltaX]; -} - -// operator= -template -CFilter2D& CFilter2D::operator=(const CFilter2D& aCopyFrom) { - if (this != &aCopyFrom) { - delete[] this->mData; - this->mXSize = aCopyFrom.mXSize; - this->mYSize = aCopyFrom.mYSize; - mDeltaX = aCopyFrom.mDeltaX; - mDeltaY = aCopyFrom.mDeltaY; - this->mData = new T[this->mXSize*this->mYSize]; - for (register int i = 0; i < this->mXSize*this->mYSize; i++) - this->mData[i] = aCopyFrom.mData[i]; - } - return *this; -} - -// deltaX -template -inline int CFilter2D::deltaX() const { - return mDeltaX; -} - -// deltaY -template -inline int CFilter2D::deltaY() const { - return mDeltaY; -} - -// AX -template -inline int CFilter2D::AX() const { - return -mDeltaX; -} - -// AY -template -inline int CFilter2D::AY() const { - return -mDeltaY; -} - -// BX -template -inline int CFilter2D::BX() const { - return this->mXSize-mDeltaX; -} - -// BY -template -inline int CFilter2D::BY() const { - return this->mYSize-mDeltaY; -} - -// sum -template -T CFilter2D::sum() const { - T aResult = 0; - for (int i = 0; i < this->mXSize*this->mYSize; i++) - aResult += abs(this->mData[i]); - return aResult; -} - -// C G A U S S ----------------------------------------------------------------- -template -CGauss::CGauss(const int aSize, const int aDegreeOfDerivative) - : CFilter(aSize,aSize >> 1) { - CVector *oldData; - CVector *newData; - CVector *temp; - oldData = new CVector(aSize); - newData = new CVector(aSize); - - (*oldData)(0) = 1; - (*oldData)(1) = 1; - - for (int i = 2; i < aSize-aDegreeOfDerivative; i++) { - (*newData)(0) = 1; - for (int j = 1; j < i; j++) - (*newData)(j) = (*oldData)(j)+(*oldData)(j-1); - (*newData)(i) = 1; - temp = oldData; - oldData = newData; - newData = temp; - } - for (int i = aSize-aDegreeOfDerivative; i < aSize; i++) { - (*newData)(0) = 1; - for (int j = 1; j < i; j++) - (*newData)(j) = (*oldData)(j)-(*oldData)(j-1); - (*newData)(i) = -(*oldData)(i-1); - temp = oldData; - oldData = newData; - newData = temp; - } - - int aSum = 0; - for (int i = 0; i < aSize; i++) - aSum += abs((*oldData)(i)); - double aInvSum = 1.0/aSum; - for (int i = 0; i < aSize; i++) - this->mData[aSize-1-i] = (*oldData)(i)*aInvSum; - - delete newData; - delete oldData; -} - -// C S M O O T H --------------------------------------------------------------- -template -CSmooth::CSmooth(float aSigma, float aPrecision) - : CFilter(2*(int)ceil(aPrecision*aSigma)+1,(int)ceil(aPrecision*aSigma)) { - float aSqrSigma = aSigma*aSigma; - for (int i = 0; i <= (this->mSize >> 1); i++) { - T aTemp = exp(i*i/(-2.0*aSqrSigma))/(aSigma*sqrt(2.0*NMath::Pi)); - this->operator()(i) = aTemp; - this->operator()(-i) = aTemp; - } - T invSum = 1.0/this->sum(); - for (int i = 0; i < this->mSize; i++) - this->mData[i] *= invSum; -} - -template -CGaussianFirstDerivative::CGaussianFirstDerivative(float aSigma, float aPrecision) - : CFilter(2*(int)ceil(aPrecision*aSigma)+1,(int)ceil(aPrecision*aSigma)) { - float aSqrSigma = aSigma*aSigma; - float aPreFactor = 1.0/(aSqrSigma*aSigma*sqrt(2.0*NMath::Pi)); - for (int i = 0; i <= (this->mSize >> 1); i++) { - T aTemp = exp(i*i/(-2.0*aSqrSigma))*i*aPreFactor; - this->operator()(i) = aTemp; - this->operator()(-i) = -aTemp; - } -} - -template -CGaussianSecondDerivative::CGaussianSecondDerivative(float aSigma, float aPrecision) - : CFilter(2*(int)ceil(aPrecision*aSigma)+1,(int)ceil(aPrecision*aSigma)) { - float aSqrSigma = aSigma*aSigma; - float aPreFactor = 1.0/(aSqrSigma*aSigma*sqrt(2.0*NMath::Pi)); - for (int i = 0; i <= (this->mSize >> 1); i++) { - T aTemp = exp(i*i/(-2.0*aSqrSigma))*(i*i/aSqrSigma-1.0)*aPreFactor; - this->operator()(i) = aTemp; - this->operator()(-i) = aTemp; - } -} - -// C D E R I V A T I V E ------------------------------------------------------- -template -CDerivative::CDerivative(const int aSize) - : CFilter(aSize,(aSize-1) >> 1) { - switch (aSize) { - case 2: - this->mData[0] = -1; - this->mData[1] = 1; - break; - case 3: - this->mData[0] = -0.5; - this->mData[1] = 0; - this->mData[2] = 0.5; - break; - case 4: - this->mData[0] = 0.041666666666666666666666666666667; - this->mData[1] = -1.125; - this->mData[2] = 1.125; - this->mData[3] = -0.041666666666666666666666666666667; - break; - case 5: - this->mData[0] = 0.083333333333; - this->mData[1] = -0.66666666666; - this->mData[2] = 0; - this->mData[3] = 0.66666666666; - this->mData[4] = -0.083333333333; - break; - case 6: - this->mData[0] = -0.0046875; - this->mData[1] = 0.0651041666666666666666666666666667; - this->mData[2] = -1.171875; - this->mData[3] = 1.171875; - this->mData[4] = -0.0651041666666666666666666666666667; - this->mData[5] = 0.0046875; - break; - case 7: - this->mData[0] = -0.016666666666666666666666666666667; - this->mData[1] = 0.15; - this->mData[2] = -0.75; - this->mData[3] = 0; - this->mData[4] = 0.75; - this->mData[5] = -0.15; - this->mData[6] = 0.016666666666666666666666666666667; - break; - case 8: - this->mData[0] = 6.9754464285714285714285714285714e-4; - this->mData[1] = -0.0095703125; - this->mData[2] = 0.079752604166666666666666666666667; - this->mData[3] = -1.1962890625; - this->mData[4] = 1.1962890625; - this->mData[5] = -0.079752604166666666666666666666667; - this->mData[6] = 0.0095703125; - this->mData[7] = -6.9754464285714285714285714285714e-4; - break; - case 9: - this->mData[0] = 0.0035714285714285714285714285714286; - this->mData[1] = -0.038095238095238095238095238095238; - this->mData[2] = 0.2; - this->mData[3] = -0.8; - this->mData[4] = 0; - this->mData[5] = 0.8; - this->mData[6] = -0.2; - this->mData[7] = 0.038095238095238095238095238095238; - - this->mData[8] = -0.0035714285714285714285714285714286; - break; - case 10: - this->mData[0] = -1.1867947048611111111111111111111e-4; - this->mData[1] = 0.0017656598772321428571428571428571; - this->mData[2] = -0.0138427734375; - this->mData[3] = 0.0897216796875; - this->mData[4] = -1.21124267578125; - this->mData[5] = 1.21124267578125; - this->mData[6] = -0.0897216796875; - this->mData[7] = 0.0138427734375; - this->mData[8] = -0.0017656598772321428571428571428571; - this->mData[9] = 1.1867947048611111111111111111111e-4; - break; - default: - throw EFilterNotAvailable(aSize,-1); - } -} - -// C H I G H O R D E R D E R I V A T I V E ------------------------------------- -template -CHighOrderDerivative::CHighOrderDerivative(int aOrder, int aSize) - : CFilter(aSize,(aSize-1) >> 1) { - switch (aSize) { - case 3: - switch (aOrder) { - case 2: - this->mData[0] = 1; - this->mData[1] = -2; - this->mData[2] = 1; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - case 4: - switch (aOrder) { - case 2: - this->mData[0] = 0.25; - this->mData[1] = -0.25; - this->mData[2] = -0.25; - this->mData[3] = 0.25; - break; - case 3: - this->mData[0] = -0.25; - this->mData[1] = 0.75; - this->mData[2] = -0.75; - this->mData[3] = 0.25; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - case 5: - switch (aOrder) { - case 2: - this->mData[0] = -0.083333333333333333333333333333333; - this->mData[1] = 1.3333333333333333333333333333333; - this->mData[2] = -2.5; - this->mData[3] = 1.3333333333333333333333333333333; - this->mData[4] = -0.083333333333333333333333333333333; - break; - case 3: - this->mData[0] = -0.5; - this->mData[1] = 1; - this->mData[2] = 0; - this->mData[3] = -1; - this->mData[4] = 0.5; - break; - case 4: - this->mData[0] = 1; - this->mData[1] = -4; - this->mData[2] = 6; - this->mData[3] = -4; - this->mData[4] = 1; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - case 6: - switch (aOrder) { - case 2: - this->mData[0] = -0.052083333333333333333333333333333; - this->mData[1] = 0.40625; - this->mData[2] = -0.35416666666666666666666666666667; - this->mData[3] = -0.35416666666666666666666666666667; - this->mData[4] = 0.40625; - this->mData[5] = -0.052083333333333333333333333333333; - break; - case 3: - this->mData[0] = 0.03125; - this->mData[1] = -0.40625; - this->mData[2] = 1.0625; - this->mData[3] = -1.0625; - this->mData[4] = 0.40625; - this->mData[5] = -0.03125; - break; - case 4: - this->mData[0] = 0.0625; - this->mData[1] = -0.1875; - this->mData[2] = 0.125; - this->mData[3] = 0.125; - this->mData[4] = -0.1875; - this->mData[5] = 0.0625; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - case 7: - switch (aOrder) { - case 2: - this->mData[0] = 0.011111111111111111111111111111111; - this->mData[1] = -0.15; - this->mData[2] = 1.5; - this->mData[3] = -2.6666666666666666666666666666667; - this->mData[4] = 1.5; - this->mData[5] = -0.15; - this->mData[6] = 0.011111111111111111111111111111111; - break; - case 3: - this->mData[0] = 0.125; - this->mData[1] = -1; - this->mData[2] = 1.625; - this->mData[3] = 0; - this->mData[4] = -1.625; - this->mData[5] = 1; - this->mData[6] = -0.125; - break; - case 4: - this->mData[0] = -0.16666666666666666666666666666667; - this->mData[1] = 2; - this->mData[2] = -6.5; - this->mData[3] = 9.3333333333333333333333333333333; - this->mData[4] = -6.5; - this->mData[5] = 2; - this->mData[6] = -0.16666666666666666666666666666667; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - case 8: - switch (aOrder) { - case 2: - this->mData[0] = 0.011241319444444444444444444444444; - this->mData[1] = -0.10828993055555555555555555555556; - this->mData[2] = 0.507421875; - this->mData[3] = -0.41037326388888888888888888888889; - this->mData[4] = -0.41037326388888888888888888888889; - this->mData[5] = 0.507421875; - this->mData[6] = -0.10828993055555555555555555555556; - this->mData[7] = 0.011241319444444444444444444444444; - break; - case 3: - this->mData[0] = -0.0048177083333333333333333333333333; - this->mData[1] = 0.064973958333333333333333333333333; - this->mData[2] = -0.507421875; - this->mData[3] = 1.2311197916666666666666666666667; - this->mData[4] = -1.2311197916666666666666666666667; - this->mData[5] = 0.507421875; - this->mData[6] = -0.064973958333333333333333333333333; - this->mData[7] = 0.0048177083333333333333333333333333; - break; - case 4: - this->mData[0] = -0.018229166666666666666666666666667; - this->mData[1] = 0.15364583333333333333333333333333; - this->mData[2] = -0.3515625; - this->mData[3] = 0.21614583333333333333333333333333; - this->mData[4] = 0.21614583333333333333333333333333; - this->mData[5] = -0.3515625; - this->mData[6] = 0.15364583333333333333333333333333; - this->mData[7] = -0.018229166666666666666666666666667; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - case 9: - switch (aOrder) { - case 2: - this->mData[0] = -0.0017857142857142857142857142857143; - this->mData[1] = 0.025396825396825396825396825396825; - this->mData[2] = -0.2; - this->mData[3] = 1.6; - this->mData[4] = -2.8472222222222222222222222222222; - this->mData[5] = 1.6; - this->mData[6] = -0.2; - this->mData[7] = 0.025396825396825396825396825396825; - this->mData[8] = -0.0017857142857142857142857142857143; - break; - case 3: - this->mData[0] = -0.029166666666666666666666666666667; - this->mData[1] = 0.3; - this->mData[2] = -1.4083333333333333333333333333333; - this->mData[3] = 2.0333333333333333333333333333333; - this->mData[4] = 0; - this->mData[5] = -2.0333333333333333333333333333333; - this->mData[6] = 1.4083333333333333333333333333333; - this->mData[7] = -0.3; - this->mData[8] = 0.029166666666666666666666666666667; - break; - case 4: - this->mData[0] = 0.029166666666666666666666666666667; - this->mData[1] = -0.4; - this->mData[2] = 2.8166666666666666666666666666667; - this->mData[3] = -8.1333333333333333333333333333333; - this->mData[4] = 11.375; - this->mData[5] = -8.1333333333333333333333333333333; - this->mData[6] = 2.8166666666666666666666666666667; - this->mData[7] = -0.4; - this->mData[8] = 0.029166666666666666666666666666667; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - case 10: - switch (aOrder) { - case 2: - this->mData[0] = -0.0025026351686507936507936507936508; - this->mData[1] = 0.028759765625; - this->mData[2] = -0.15834263392857142857142857142857; - this->mData[3] = 0.57749565972222222222222222222222; - this->mData[4] = -0.44541015625; - this->mData[5] = -0.44541015625; - this->mData[6] = 0.57749565972222222222222222222222; - this->mData[7] = -0.15834263392857142857142857142857; - this->mData[8] = 0.028759765625; - this->mData[9] = -0.0025026351686507936507936507936508; - break; - case 3: - this->mData[0] = 0.0008342117228835978835978835978836; - this->mData[1] = -0.012325613839285714285714285714286; - this->mData[2] = 0.095005580357142857142857142857143; - this->mData[3] = -0.57749565972222222222222222222222; - this->mData[4] = 1.33623046875; - this->mData[5] = -1.33623046875; - this->mData[6] = 0.57749565972222222222222222222222; - this->mData[7] = -0.095005580357142857142857142857143; - this->mData[8] = 0.012325613839285714285714285714286; - this->mData[9] = -0.0008342117228835978835978835978836; - break; - case 4: - this->mData[0] = 0.00458984375; - this->mData[1] = -0.050358072916666666666666666666667; - this->mData[2] = 0.24544270833333333333333333333333; - this->mData[3] = -0.480078125; - this->mData[4] = 0.28040364583333333333333333333333; - this->mData[5] = 0.28040364583333333333333333333333; - this->mData[6] = -0.480078125; - this->mData[7] = 0.24544270833333333333333333333333; - this->mData[8] = -0.050358072916666666666666666666667; - this->mData[9] = 0.00458984375; - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } - break; - default: - throw EFilterNotAvailable(aSize,aOrder); - } -} - -// C G A B O R ----------------------------------------------------------------- -template -CGaborReal::CGaborReal(float aFrequency, float aAngle, float aSigma1, float aSigma2) - : CFilter2D() { - // sqrt(2.0*log(2.0))/(2.0*NMath::Pi) = 0.18739 - float sigma1Sqr2 = aSigma1*0.18739/aFrequency; - sigma1Sqr2 = 0.5/(sigma1Sqr2*sigma1Sqr2); - float sigma2Sqr2 = aSigma2*0.18739/aFrequency; - sigma2Sqr2 = 0.5/(sigma2Sqr2*sigma2Sqr2); - float aCos = cos(aAngle); - float aSin = sin(aAngle); - float a = 0.6*aSigma1/aFrequency; - float b = 0.6*aSigma2/aFrequency; - float aXSize = fabs(a*aCos)+fabs(b*aSin); - float aYSize = fabs(b*aCos)+fabs(a*aSin); - this->setSize(1+2.0*floor(aXSize),1+2.0*floor(aYSize)); - this->shift(floor(aXSize),floor(aYSize)); - for (int y = this->AY(); y < this->BY(); y++) - for (int x = this->AX(); x < this->BX(); x++) { - float a = x*aCos+y*aSin; - float b = y*aCos-x*aSin; - float aGauss = exp(-sigma1Sqr2*a*a-sigma2Sqr2*b*b); - float aHelp = 2.0*NMath::Pi*aFrequency*(x*aCos+y*aSin); - this->operator()(x,y) = aGauss*cos(aHelp); - } -} - -template -CGaborImaginary::CGaborImaginary(float aFrequency, float aAngle, float aSigma1, float aSigma2) - : CFilter2D() { - // sqrt(2.0*log(2.0))/(2.0*NMath::Pi) = 0.18739 - float sigma1Sqr2 = aSigma1*0.18739/aFrequency; - sigma1Sqr2 = 0.5/(sigma1Sqr2*sigma1Sqr2); - float sigma2Sqr2 = aSigma2*0.18739/aFrequency; - sigma2Sqr2 = 0.5/(sigma2Sqr2*sigma2Sqr2); - float aCos = cos(aAngle); - float aSin = sin(aAngle); - float a = 0.6*aSigma1/aFrequency; - float b = 0.6*aSigma2/aFrequency; - float aXSize = fabs(a*aCos)+fabs(b*aSin); - float aYSize = fabs(b*aCos)+fabs(a*aSin); - this->setSize(1+2.0*floor(aXSize),1+2.0*floor(aYSize)); - this->shift(floor(aXSize),floor(aYSize)); - for (int y = this->AY(); y < this->BY(); y++) - for (int x = this->AX(); x < this->BX(); x++) { - float a = x*aCos+y*aSin; - float b = y*aCos-x*aSin; - float aGauss = exp(-sigma1Sqr2*a*a-sigma2Sqr2*b*b); - float aHelp = 2.0*NMath::Pi*aFrequency*(x*aCos+y*aSin); - this->operator()(x,y) = aGauss*sin(aHelp); - } -} - -// F I L T E R ----------------------------------------------------------------- - -namespace NFilter { - -// 1D linear filtering --------------------------------------------------------- - -template -inline void filter(CVector& aVector, const CFilter& aFilter) { - CVector oldVector(aVector); - filter(oldVector,aVector,aFilter); -} - -template -void filter(const CVector& aVector, CVector& aResult, const CFilter& aFilter) { - if (aResult.size() != aVector.size()) throw EFilterIncompatibleSize(aVector.size(),aResult.size()); - int x1 = -aFilter.A(); - int x2 = aVector.size()-aFilter.B(); - int a2Size = 2*aVector.size()-1; - // Left rim - for (int i = 0; i < x1; i++) { - aResult[i] = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) - if (j+i < 0) aResult(i) += aFilter(j)*aVector(-1-j-i); - else aResult(i) += aFilter(j)*aVector(j+i); - } - // Middle - for (int i = x1; i < x2; i++) { - aResult[i] = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) - aResult(i) += aFilter(j)*aVector(j+i); - } - // Right rim - for (int i = x2; i < aResult.size(); i++) { - aResult[i] = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) - if (j+i >= aVector.size()) aResult(i) += aFilter(j)*aVector(a2Size-j-i); - else aResult(i) += aFilter(j)*aVector(j+i); - } -} - -// boxfilter -template -inline void boxFilter(CVector& aVector, int aWidth) { - CVector aTemp(aVector); - boxFilter(aTemp,aVector,aWidth); -} - -template -void boxFilter(const CVector& aVector, CVector& aResult, int aWidth) { - if (aWidth % 2 == 0) aWidth += 1; - T* invWidth = new T[aWidth+1]; - invWidth[0] = 1.0f; - for (int i = 1; i <= aWidth; i++) - invWidth[i] = 1.0/i; - int halfWidth = (aWidth >> 1); - int aRight = halfWidth; - if (aRight >= aVector.size()) aRight = aVector.size()-1; - // Initialize - T aSum = 0.0f; - for (int x = 0; x <= aRight; x++) - aSum += aVector(x); - int aNum = aRight+1; - // Shift - for (int x = 0; x < aVector.size(); x++) { - aResult(x) = aSum*invWidth[aNum]; - if (x-halfWidth >= 0) { - aSum -= aVector(x-halfWidth); aNum--; - } - if (x+halfWidth+1 < aVector.size()) { - aSum += aVector(x+halfWidth+1); aNum++; - } - } - delete[] invWidth; -} - -// 2D linear filtering --------------------------------------------------------- - -template -inline void filter(CMatrix& aMatrix, const CFilter& aFilterX, const CFilter& aFilterY) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filter(aMatrix,tempMatrix,aFilterX,1); - filter(tempMatrix,aMatrix,1,aFilterY); -} - -template -inline void filter(const CMatrix& aMatrix, CMatrix& aResult, const CFilter& aFilterX, const CFilter& aFilterY) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filter(aMatrix,tempMatrix,aFilterX,1); - filter(tempMatrix,aResult,1,aFilterY); -} - -template -inline void filter(CMatrix& aMatrix, const CFilter& aFilter, const int aDummy) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filter(aMatrix,tempMatrix,aFilter,1); - aMatrix = tempMatrix; -} - -template -void filter(const CMatrix& aMatrix, CMatrix& aResult, const CFilter& aFilter, const int aDummy) { - if (aResult.xSize() != aMatrix.xSize() || aResult.ySize() != aMatrix.ySize()) - throw EFilterIncompatibleSize(aMatrix.xSize()*aMatrix.ySize(),aResult.xSize()*aResult.ySize()); - int x1 = -aFilter.A(); - int x2 = aMatrix.xSize()-aFilter.B(); - int a2Size = 2*aMatrix.xSize()-1; - aResult = 0; - for (int y = 0; y < aMatrix.ySize(); y++) { - int aOffset = y*aMatrix.xSize(); - // Left rim - for (int x = 0; x < x1; x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (x+i < 0) aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset-1-x-i]; - else if (x+i >= aMatrix.xSize()) aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset+a2Size-x-i]; - else aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset+x+i]; - } - // Center - for (int x = x1; x < x2; x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset+x+i]; - // Right rim - for (int x = x2; x < aMatrix.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (x+i < 0) aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset-1-x-i]; - else if (x+i >= aMatrix.xSize()) aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset+a2Size-x-i]; - else aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset+x+i]; - } - } -} - -template -inline void filter(CMatrix& aMatrix, const int aDummy, const CFilter& aFilter) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filter(aMatrix,tempMatrix,1,aFilter); - aMatrix = tempMatrix; -} - -template -void filter(const CMatrix& aMatrix, CMatrix& aResult, const int aDummy, const CFilter& aFilter) { - if (aResult.xSize() != aMatrix.xSize() || aResult.ySize() != aMatrix.ySize()) - throw EFilterIncompatibleSize(aMatrix.xSize()*aMatrix.ySize(),aResult.xSize()*aResult.ySize()); - int y1 = -aFilter.A(); - int y2 = aMatrix.ySize()-aFilter.B(); - int a2Size = 2*aMatrix.ySize()-1; - // Upper rim - for (int y = 0; y < y1; y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) { - if (y+j < 0) aResult(x,y) += aFilter[j]*aMatrix(x,-1-y-j); - else if (y+j >= aMatrix.ySize()) aResult(x,y) += aFilter[j]*aMatrix(x,a2Size-y-j); - else aResult(x,y) += aFilter[j]*aMatrix(x,y+j); - } - } - // Lower rim - for (int y = y2; y < aMatrix.ySize(); y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) { - if (y+j < 0) aResult(x,y) += aFilter[j]*aMatrix(x,-1-y-j); - else if (y+j >= aMatrix.ySize()) aResult(x,y) += aFilter[j]*aMatrix(x,a2Size-y-j); - else aResult(x,y) += aFilter[j]*aMatrix(x,y+j); - } - } - // Center - for (int y = y1; y < y2; y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) - aResult(x,y) += aFilter[j]*aMatrix(x,y+j); - } -} - -template -inline void filter(CMatrix& aMatrix, const CFilter2D& aFilter) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filter(aMatrix,tempMatrix,aFilter); - aMatrix = tempMatrix; -} - -template -void filter(const CMatrix& aMatrix, CMatrix& aResult, const CFilter2D& aFilter) { - if (aResult.xSize() != aMatrix.xSize() || aResult.ySize() != aMatrix.ySize()) - throw EFilterIncompatibleSize(aMatrix.xSize()*aMatrix.ySize(),aResult.xSize()*aResult.ySize()); - int x1 = -aFilter.AX(); - int y1 = -aFilter.AY(); - int x2 = aMatrix.xSize()-aFilter.BX(); - int y2 = aMatrix.ySize()-aFilter.BY(); - int a2XSize = 2*aMatrix.xSize()-1; - int a2YSize = 2*aMatrix.ySize()-1; - // Upper rim - for (int y = 0; y < y1; y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.AY(); j < aFilter.BY(); j++) { - int tempY; - if (y+j < 0) tempY = -1-y-j; - else if (y+j >= aMatrix.ySize()) tempY = a2YSize-y-j; - else tempY = y+j; - for (int i = aFilter.AX(); i < aFilter.BX(); i++) { - if (x+i < 0) aResult(x,y) += aFilter(i,j)*aMatrix(-1-x-i,tempY); - else if (x+i >= aMatrix.xSize()) aResult(x,y) += aFilter(i,j)*aMatrix(a2XSize-x-i,tempY); - else aResult(x,y) += aFilter(i,j)*aMatrix(x+i,tempY); - } - } - } - // Lower rim - for (int y = y2; y < aMatrix.ySize(); y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.AY(); j < aFilter.BY(); j++) { - int tempY; - if (y+j < 0) tempY = -1-y-j; - else if (y+j >= aMatrix.ySize()) tempY = a2YSize-y-j; - else tempY = y+j; - for (int i = aFilter.AX(); i < aFilter.BX(); i++) { - if (x+i < 0) aResult(x,y) += aFilter(i,j)*aMatrix(-1-x-i,tempY); - else if (x+i >= aMatrix.xSize()) aResult(x,y) += aFilter(i,j)*aMatrix(a2XSize-x-i,tempY); - else aResult(x,y) += aFilter(i,j)*aMatrix(x+i,tempY); - } - } - } - for (int y = y1; y < y2; y++) { - // Left rim - for (int x = 0; x < x1; x++) { - aResult(x,y) = 0; - for (int j = aFilter.AY(); j < aFilter.BY(); j++) { - for (int i = aFilter.AX(); i < aFilter.BX(); i++) { - if (x+i < 0) aResult(x,y) += aFilter(i,j)*aMatrix(-1-x-i,y+j); - else if (x+i >= aMatrix.xSize()) aResult(x,y) += aFilter(i,j)*aMatrix(a2XSize-x-i,y+j); - else aResult(x,y) += aFilter(i,j)*aMatrix(x+i,y+j); - } - } - } - // Right rim - for (int x = x2; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.AY(); j < aFilter.BY(); j++) { - for (int i = aFilter.AX(); i < aFilter.BX(); i++) { - if (x+i < 0) aResult(x,y) += aFilter(i,j)*aMatrix(-1-x-i,y+j); - else if (x+i >= aMatrix.xSize()) aResult(x,y) += aFilter(i,j)*aMatrix(a2XSize-x-i,y+j); - else aResult(x,y) += aFilter(i,j)*aMatrix(x+i,y+j); - } - } - } - } - // Center - for (int y = y1; y < y2; y++) - for (int x = x1; x < x2; x++) { - aResult(x,y) = 0; - for (int j = aFilter.AY(); j < aFilter.BY(); j++) - for (int i = aFilter.AX(); i < aFilter.BX(); i++) - aResult(x,y) += aFilter(i,j)*aMatrix(x+i,y+j); - } -} - - - -template -inline void filterMin(CMatrix& aMatrix, const CFilter& aFilterX, const CFilter& aFilterY) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filterMin(aMatrix,aMatrix,tempMatrix,aFilterX,1); - CMatrix tmp(aMatrix); - filterMin(tempMatrix,tmp,aMatrix,1,aFilterY); -} - -template -inline void filterMin(const CMatrix& aMatrix, CMatrix& aResult, const CFilter& aFilterX, const CFilter& aFilterY) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filterMin(aMatrix,aMatrix,tempMatrix,aFilterX,1); - filterMin(tempMatrix,aMatrix,aResult,1,aFilterY); -} - -template -inline void filterMin(CMatrix& aMatrix, const CFilter& aFilter, const int aDummy) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filterMin(aMatrix,aMatrix,tempMatrix,aFilter,1); - aMatrix = tempMatrix; -} - -template -void filterMin(const CMatrix& aMatrix, const CMatrix& aOrig, CMatrix& aResult, const CFilter& aFilter, const int aDummy) { - if (aResult.xSize() != aMatrix.xSize() || aResult.ySize() != aMatrix.ySize()) - throw EFilterIncompatibleSize(aMatrix.xSize()*aMatrix.ySize(),aResult.xSize()*aResult.ySize()); - int x1 = -aFilter.A(); - int x2 = aMatrix.xSize()-aFilter.B(); - int a2Size = 2*aMatrix.xSize()-1; - aResult = 0; - for (int y = 0; y < aMatrix.ySize(); y++) { - int aOffset = y*aMatrix.xSize(); - // Left rim - for (int x = 0; x < x1; x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - int matrixIdx; - if (x+i < 0) matrixIdx = aOffset-1-x-i; - else if (x+i >= aMatrix.xSize()) matrixIdx = aOffset+a2Size-x-i; - else matrixIdx = aOffset+x+i; - if (matrixIdx == aOffset+x || aOrig.data()[matrixIdx] - 1e-5 <= aOrig.data()[aOffset+x]) - aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[matrixIdx]; - } - // Center - for (int x = x1; x < x2; x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) - if (i == 0 || aOrig.data()[aOffset+x+i] - 1e-5 <= aOrig.data()[aOffset+x]) - aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[aOffset+x+i]; - // Right rim - for (int x = x2; x < aMatrix.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - int matrixIdx; - if (x+i < 0) matrixIdx = aOffset-1-x-i; - else if (x+i >= aMatrix.xSize()) matrixIdx = aOffset+a2Size-x-i; - else matrixIdx = aOffset+x+i; - if (matrixIdx == aOffset+x || aOrig.data()[matrixIdx] - 1e-5 <= aOrig.data()[aOffset+x]) - aResult.data()[aOffset+x] += aFilter[i]*aMatrix.data()[matrixIdx]; - } - } -} - -template -inline void filterMin(CMatrix& aMatrix, const int aDummy, const CFilter& aFilter) { - CMatrix tempMatrix(aMatrix.xSize(),aMatrix.ySize()); - filterMin(aMatrix, aMatrix,tempMatrix,1,aFilter); - aMatrix = tempMatrix; -} - -template -void filterMin(const CMatrix& aMatrix, const CMatrix& aOrig, CMatrix& aResult, const int aDummy, const CFilter& aFilter) { - if (aResult.xSize() != aMatrix.xSize() || aResult.ySize() != aMatrix.ySize()) - throw EFilterIncompatibleSize(aMatrix.xSize()*aMatrix.ySize(),aResult.xSize()*aResult.ySize()); - int y1 = -aFilter.A(); - int y2 = aMatrix.ySize()-aFilter.B(); - int a2Size = 2*aMatrix.ySize()-1; - // Upper rim - for (int y = 0; y < y1; y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) { - int matrixIdx; - if (y+j < 0) matrixIdx = -1-y-j; - else if (y+j >= aMatrix.ySize()) matrixIdx = a2Size-y-j; - else matrixIdx = y+j; - if (matrixIdx == y || aOrig(x, matrixIdx) - 1e-5 <= aOrig(x, y)) - aResult(x,y) += aFilter[j]*aMatrix(x,matrixIdx); - } - } - // Lower rim - for (int y = y2; y < aMatrix.ySize(); y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) { - int matrixIdx; - if (y+j < 0) matrixIdx = -1-y-j; - else if (y+j >= aMatrix.ySize()) matrixIdx = a2Size-y-j; - else matrixIdx = y+j; - if (matrixIdx == y || aOrig(x, matrixIdx) - 1e-5 <= aOrig(x, y)) - aResult(x,y) += aFilter[j]*aMatrix(x,matrixIdx); - } - } - // Center - for (int y = y1; y < y2; y++) - for (int x = 0; x < aMatrix.xSize(); x++) { - aResult(x,y) = 0; - for (int j = aFilter.A(); j < aFilter.B(); j++) - if (j == 0 || aOrig(x,y+j) - 1e-5 <= aOrig(x,y)) - aResult(x,y) += aFilter[j]*aMatrix(x,y+j); - } -} - - - -// boxfilterX -template -inline void boxFilterX(CMatrix& aMatrix, int aWidth) { - CMatrix aTemp(aMatrix); - boxFilterX(aTemp,aMatrix,aWidth); -} - -template -void boxFilterX(const CMatrix& aMatrix, CMatrix& aResult, int aWidth) { - if (aWidth & 1 == 0) aWidth += 1; - T invWidth = 1.0/aWidth; - int halfWidth = (aWidth >> 1); - int aRight = halfWidth; - if (aRight >= aMatrix.xSize()) aRight = aMatrix.xSize()-1; - for (int y = 0; y < aMatrix.ySize(); y++) { - int aOffset = y*aMatrix.xSize(); - // Initialize - T aSum = 0.0f; - for (int x = aRight-aWidth+1; x <= aRight; x++) - if (x < 0) aSum += aMatrix.data()[aOffset-x-1]; - else aSum += aMatrix.data()[aOffset+x]; - // Shift - int xm = -halfWidth; - int xp = halfWidth+1; - for (int x = 0; x < aMatrix.xSize(); x++,xm++,xp++) { - aResult.data()[aOffset+x] = aSum*invWidth; - if (xm < 0) aSum -= aMatrix.data()[aOffset-xm-1]; - else aSum -= aMatrix.data()[aOffset+xm]; - if (xp >= aMatrix.xSize()) aSum += aMatrix.data()[aOffset+2*aMatrix.xSize()-1-xp]; - else aSum += aMatrix.data()[aOffset+xp]; - } - } -} - -// boxfilterY -template -inline void boxFilterY(CMatrix& aMatrix, int aWidth) { - CMatrix aTemp(aMatrix); - boxFilterY(aTemp,aMatrix,aWidth); -} - -template -void boxFilterY(const CMatrix& aMatrix, CMatrix& aResult, int aWidth) { - if (aWidth & 1 == 0) aWidth += 1; - T invWidth = 1.0/aWidth; - int halfWidth = (aWidth >> 1); - int aBottom = halfWidth; - if (aBottom >= aMatrix.ySize()) aBottom = aMatrix.xSize()-1; - for (int x = 0; x < aMatrix.xSize(); x++) { - // Initialize - T aSum = 0.0f; - for (int y = aBottom-aWidth+1; y <= aBottom; y++) - if (y < 0) aSum += aMatrix(x,-1-y); - else aSum += aMatrix(x,y); - // Shift - int ym = -halfWidth; - int yp = halfWidth+1; - for (int y = 0; y < aMatrix.ySize(); y++,ym++,yp++) { - aResult(x,y) = aSum*invWidth; - if (ym < 0) aSum -= aMatrix(x,-1-ym); - else aSum -= aMatrix(x,ym); - if (yp >= aMatrix.ySize()) aSum += aMatrix(x,2*aMatrix.ySize()-1-yp); - else aSum += aMatrix(x,yp); - } - } -} - -template -void recursiveSmoothX(CMatrix& aMatrix, float aSigma) { - CVector aVals1(aMatrix.xSize()); - CVector aVals2(aMatrix.xSize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int y = 0; y < aMatrix.ySize(); y++) { - aVals1(0) = (0.5f-k*aPreMinus)*aMatrix(0,y); - aVals1(1) = k*(aMatrix(1,y)+aPreMinus*aMatrix(0,y))+(a2Exp-aExpSqr)*aVals1(0); - for (int x = 2; x < aMatrix.xSize(); x++) - aVals1(x) = k*(aMatrix(x,y)+aPreMinus*aMatrix(x-1,y))+a2Exp*aVals1(x-1)-aExpSqr*aVals1(x-2); - aVals2(aMatrix.xSize()-1) = (0.5f+k*aPreMinus)*aMatrix(aMatrix.xSize()-1,y); - aVals2(aMatrix.xSize()-2) = k*((aPrePlus-aExpSqr)*aMatrix(aMatrix.xSize()-1,y))+(a2Exp-aExpSqr)*aVals2(aMatrix.xSize()-1); - for (int x = aMatrix.xSize()-3; x >= 0; x--) - aVals2(x) = k*(aPrePlus*aMatrix(x+1,y)-aExpSqr*aMatrix(x+2,y))+a2Exp*aVals2(x+1)-aExpSqr*aVals2(x+2); - for (int x = 0; x < aMatrix.xSize(); x++) - aMatrix(x,y) = aVals1(x)+aVals2(x); - } -} - -template -void recursiveSmoothY(CMatrix& aMatrix, float aSigma) { - CVector aVals1(aMatrix.ySize()); - CVector aVals2(aMatrix.ySize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int x = 0; x < aMatrix.xSize(); x++) { - aVals1(0) = (0.5f-k*aPreMinus)*aMatrix(x,0); - aVals1(1) = k*(aMatrix(x,1)+aPreMinus*aMatrix(x,0))+(a2Exp-aExpSqr)*aVals1(0); - for (int y = 2; y < aMatrix.ySize(); y++) - aVals1(y) = k*(aMatrix(x,y)+aPreMinus*aMatrix(x,y-1))+a2Exp*aVals1(y-1)-aExpSqr*aVals1(y-2); - aVals2(aMatrix.ySize()-1) = (0.5f+k*aPreMinus)*aMatrix(x,aMatrix.ySize()-1); - aVals2(aMatrix.ySize()-2) = k*((aPrePlus-aExpSqr)*aMatrix(x,aMatrix.ySize()-1))+(a2Exp-aExpSqr)*aVals2(aMatrix.ySize()-1); - for (int y = aMatrix.ySize()-3; y >= 0; y--) - aVals2(y) = k*(aPrePlus*aMatrix(x,y+1)-aExpSqr*aMatrix(x,y+2))+a2Exp*aVals2(y+1)-aExpSqr*aVals2(y+2); - for (int y = 0; y < aMatrix.ySize(); y++) - aMatrix(x,y) = aVals1(y)+aVals2(y); - } -} - -template -inline void recursiveSmooth(CMatrix& aMatrix, float aSigma) { - recursiveSmoothX(aMatrix,aSigma); - recursiveSmoothY(aMatrix,aSigma); -} - -// Linear 3D filtering --------------------------------------------------------- - -template -inline void filter(CTensor& aTensor, const CFilter& aFilterX, const CFilter& aFilterY, const CFilter& aFilterZ) { - CTensor tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize()); - filter(aTensor,tempTensor,aFilterX,1,1); - filter(tempTensor,aTensor,1,aFilterY,1); - filter(aTensor,tempTensor,1,1,aFilterZ); - aTensor = tempTensor; -} - -template -inline void filter(const CTensor& aTensor, CTensor& aResult, const CFilter& aFilterX, const CFilter& aFilterY, const CFilter& aFilterZ) { - CTensor tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize()); - filter(aTensor,aResult,aFilterX,1,1); - filter(aResult,tempTensor,1,aFilterY,1); - filter(tempTensor,aResult,1,1,aFilterZ); -} - -template -inline void filter(CTensor& aTensor, const CFilter& aFilter, const int aDummy1, const int aDummy2) { - CTensor tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize()); - filter(aTensor,tempTensor,aFilter,1,1); - aTensor = tempTensor; -} - -template -void filter(const CTensor& aTensor, CTensor& aResult, const CFilter& aFilter, const int aDummy1, const int aDummy2) { - if (aResult.xSize() != aTensor.xSize() || aResult.ySize() != aTensor.ySize() || aResult.zSize() != aTensor.zSize()) - throw EFilterIncompatibleSize(aTensor.xSize()*aTensor.ySize()*aTensor.zSize(),aResult.xSize()*aResult.ySize()*aResult.zSize()); - int x1 = -aFilter.A(); - int x2 = aTensor.xSize()-aFilter.B(); - int a2Size = 2*aTensor.xSize()-1; - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) { - // Left rim - for (int x = 0; x < x1; x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (x+i < 0) aResult(x,y,z) += aFilter[i]*aTensor(-1-x-i,y,z); - else if (x+i >= aTensor.xSize()) aResult(x,y,z) += aFilter[i]*aTensor(a2Size-x-i,y,z); - else aResult(x,y,z) += aFilter[i]*aTensor(x+i,y,z); - } - } - // Center - for (int x = x1; x < x2; x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult(x,y,z) += aFilter[i]*aTensor(x+i,y,z); - } - // Right rim - for (int x = x2; x < aTensor.xSize(); x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (x+i < 0) aResult(x,y,z) += aFilter[i]*aTensor(-1-x-i,y,z); - else if (x+i >= aTensor.xSize()) aResult(x,y,z) += aFilter[i]*aTensor(a2Size-x-i,y,z); - else aResult(x,y,z) += aFilter[i]*aTensor(x+i,y,z); - } - } - } -} - -template -inline void filter(CTensor& aTensor, const int aDummy1, const CFilter& aFilter, const int aDummy2) { - CTensor tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize()); - filter(aTensor,tempTensor,1,aFilter,1); - aTensor = tempTensor; -} - -template -void filter(const CTensor& aTensor, CTensor& aResult, const int aDummy1, const CFilter& aFilter, const int aDummy2) { - if (aResult.xSize() != aTensor.xSize() || aResult.ySize() != aTensor.ySize() || aResult.zSize() != aTensor.zSize()) - throw EFilterIncompatibleSize(aTensor.xSize()*aTensor.ySize()*aTensor.zSize(),aResult.xSize()*aResult.ySize()*aResult.zSize()); - int y1 = -aFilter.A(); - int y2 = aTensor.ySize()-aFilter.B(); - int a2Size = 2*aTensor.ySize()-1; - for (int z = 0; z < aTensor.zSize(); z++) { - // Upper rim - for (int y = 0; y < y1; y++) - for (int x = 0; x < aTensor.xSize(); x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (y+i < 0) aResult(x,y,z) += aFilter[i]*aTensor(x,-1-y-i,z); - else if (y+i >= aTensor.ySize()) aResult(x,y,z) += aFilter[i]*aTensor(x,a2Size-y-i,z); - else aResult(x,y,z) += aFilter[i]*aTensor(x,y+i,z); - } - } - // Lower rim - for (int y = y2; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (y+i < 0) aResult(x,y,z) += aFilter[i]*aTensor(x,-1-y-i,z); - else if (y+i >= aTensor.ySize()) aResult(x,y,z) += aFilter[i]*aTensor(x,a2Size-y-i,z); - else aResult(x,y,z) += aFilter[i]*aTensor(x,y+i,z); - } - } - } - // Center - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = y1; y < y2; y++) - for (int x = 0; x < aTensor.xSize(); x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult(x,y,z) += aFilter[i]*aTensor(x,y+i,z); - } -} - -template -inline void filter(CTensor& aTensor, const int aDummy1, const int aDummy2, const CFilter& aFilter) { - CTensor tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize()); - filter(aTensor,tempTensor,1,1,aFilter); - aTensor = tempTensor; -} - -template -void filter(const CTensor& aTensor, CTensor& aResult, const int aDummy1, const int aDummy2, const CFilter& aFilter) { - if (aResult.xSize() != aTensor.xSize() || aResult.ySize() != aTensor.ySize() || aResult.zSize() != aTensor.zSize()) - throw EFilterIncompatibleSize(aTensor.xSize()*aTensor.ySize()*aTensor.zSize(),aResult.xSize()*aResult.ySize()*aResult.zSize()); - int z1 = -aFilter.A(); - int z2 = aTensor.zSize()-aFilter.B(); - if (z2 < 0) z2 = 0; - int a2Size = 2*aTensor.zSize()-1; - // Front rim - for (int z = 0; z < z1; z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (z+i < 0) aResult(x,y,z) += aFilter[i]*aTensor(x,y,-1-z-i); - else if (z+i >= aTensor.zSize()) aResult(x,y,z) += aFilter[i]*aTensor(x,y,a2Size-z-i); - else aResult(x,y,z) += aFilter[i]*aTensor(x,y,z+i); - } - } - // Back rim - for (int z = z2; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (z+i < 0) aResult(x,y,z) += aFilter[i]*aTensor(x,y,-1-z-i); - else if (z+i >= aTensor.zSize()) aResult(x,y,z) += aFilter[i]*aTensor(x,y,a2Size-z-i); - else aResult(x,y,z) += aFilter[i]*aTensor(x,y,z+i); - } - } - // Center - for (int z = z1; z < z2; z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - aResult(x,y,z) = 0; - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult(x,y,z) += aFilter[i]*aTensor(x,y,z+i); - } -} - -// boxfilterX -template -inline void boxFilterX(CTensor& aTensor, int aWidth) { - CTensor aTemp(aTensor); - boxFilterX(aTemp,aTensor,aWidth); -} - -template -void boxFilterX(const CTensor& aTensor, CTensor& aResult, int aWidth) { - if (aWidth % 2 == 0) aWidth += 1; - T* invWidth = new T[aWidth+1]; - invWidth[0] = 1.0f; - for (int i = 1; i <= aWidth; i++) - invWidth[i] = 1.0/i; - int halfWidth = (aWidth >> 1); - int aRight = halfWidth; - if (aRight >= aTensor.xSize()) aRight = aTensor.xSize()-1; - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) { - int aOffset = (z*aTensor.ySize()+y)*aTensor.xSize(); - // Initialize - int aNum = 0; - T aSum = 0.0f; - for (int x = 0; x <= aRight; x++) { - aSum += aTensor.data()[aOffset+x]; aNum++; - } - // Shift - for (int x = 0; x < aTensor.xSize(); x++) { - aResult.data()[aOffset+x] = aSum*invWidth[aNum]; - if (x-halfWidth >= 0) { - aSum -= aTensor.data()[aOffset+x-halfWidth]; aNum--; - } - if (x+halfWidth+1 < aTensor.xSize()) { - aSum += aTensor.data()[aOffset+x+halfWidth+1]; aNum++; - } - } - } - delete[] invWidth; -} - -// boxfilterY -template -inline void boxFilterY(CTensor& aTensor, int aWidth) { - CTensor aTemp(aTensor); - boxFilterY(aTemp,aTensor,aWidth); -} - -template -void boxFilterY(const CTensor& aTensor, CTensor& aResult, int aWidth) { - if (aWidth % 2 == 0) aWidth += 1; - T* invWidth = new T[aWidth+1]; - invWidth[0] = 1.0f; - for (int i = 1; i <= aWidth; i++) - invWidth[i] = 1.0/i; - int halfWidth = (aWidth >> 1); - int aBottom = halfWidth; - if (aBottom >= aTensor.ySize()) aBottom = aTensor.ySize()-1; - for (int z = 0; z < aTensor.zSize(); z++) - for (int x = 0; x < aTensor.xSize(); x++) { - // Initialize - int aNum = 0; - T aSum = 0.0f; - for (int y = 0; y <= aBottom; y++) { - aSum += aTensor(x,y,z); aNum++; - } - // Shift - for (int y = 0; y < aTensor.ySize(); y++) { - aResult(x,y,z) = aSum*invWidth[aNum]; - if (y-halfWidth >= 0) { - aSum -= aTensor(x,y-halfWidth,z); aNum--; - } - if (y+halfWidth+1 < aTensor.ySize()) { - aSum += aTensor(x,y+halfWidth+1,z); aNum++; - } - } - } - delete[] invWidth; -} - -// boxfilterZ -template -inline void boxFilterZ(CTensor& aTensor, int aWidth) { - CTensor aTemp(aTensor); - boxFilterZ(aTemp,aTensor,aWidth); -} - -template -void boxFilterZ(const CTensor& aTensor, CTensor& aResult, int aWidth) { - if (aWidth % 2 == 0) aWidth += 1; - T* invWidth = new T[aWidth+1]; - invWidth[0] = 1.0f; - for (int i = 1; i <= aWidth; i++) - invWidth[i] = 1.0/i; - int halfWidth = (aWidth >> 1); - int aBottom = halfWidth; - if (aBottom >= aTensor.zSize()) aBottom = aTensor.zSize()-1; - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - // Initialize - int aNum = 0; - T aSum = 0.0f; - for (int z = 0; z <= aBottom; z++) { - aSum += aTensor(x,y,z); aNum++; - } - // Shift - for (int z = 0; z < aTensor.zSize(); z++) { - aResult(x,y,z) = aSum*invWidth[aNum]; - if (z-halfWidth >= 0) { - aSum -= aTensor(x,y,z-halfWidth); aNum--; - } - if (z+halfWidth+1 < aTensor.zSize()) { - aSum += aTensor(x,y,z+halfWidth+1); aNum++; - } - } - } - delete[] invWidth; -} - -template -void recursiveSmoothX(CTensor& aTensor, float aSigma) { - CVector aVals1(aTensor.xSize()); - CVector aVals2(aTensor.xSize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) { - int aOffset = (z*aTensor.ySize()+y)*aTensor.xSize(); - aVals1(0) = (0.5-k*aPreMinus)*aTensor.data()[aOffset]; - aVals1(1) = k*(aTensor.data()[aOffset+1]+aPreMinus*aTensor.data()[aOffset])+(2.0*aExp-aExpSqr)*aVals1(0); - for (int x = 2; x < aTensor.xSize(); x++) - aVals1(x) = k*(aTensor.data()[aOffset+x]+aPreMinus*aTensor.data()[aOffset+x-1])+a2Exp*aVals1(x-1)-aExpSqr*aVals1(x-2); - aVals2(aTensor.xSize()-1) = (0.5+k*aPreMinus)*aTensor.data()[aOffset+aTensor.xSize()-1]; - aVals2(aTensor.xSize()-2) = k*((aPrePlus-aExpSqr)*aTensor.data()[aOffset+aTensor.xSize()-1])+(a2Exp-aExpSqr)*aVals2(aTensor.xSize()-1); - for (int x = aTensor.xSize()-3; x >= 0; x--) - aVals2(x) = k*(aPrePlus*aTensor.data()[aOffset+x+1]-aExpSqr*aTensor.data()[aOffset+x+2])+a2Exp*aVals2(x+1)-aExpSqr*aVals2(x+2); - for (int x = 0; x < aTensor.xSize(); x++) - aTensor.data()[aOffset+x] = aVals1(x)+aVals2(x); - } -} - -template -void recursiveSmoothY(CTensor& aTensor, float aSigma) { - CVector aVals1(aTensor.ySize()); - CVector aVals2(aTensor.ySize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int z = 0; z < aTensor.zSize(); z++) - for (int x = 0; x < aTensor.xSize(); x++) { - aVals1(0) = (0.5-k*aPreMinus)*aTensor(x,0,z); - aVals1(1) = k*(aTensor(x,1,z)+aPreMinus*aTensor(x,0,z))+(2.0*aExp-aExpSqr)*aVals1(0); - for (int y = 2; y < aTensor.ySize(); y++) - aVals1(y) = k*(aTensor(x,y,z)+aPreMinus*aTensor(x,y-1,z))+a2Exp*aVals1(y-1)-aExpSqr*aVals1(y-2); - aVals2(aTensor.ySize()-1) = (0.5+k*aPreMinus)*aTensor(x,aTensor.ySize()-1,z); - aVals2(aTensor.ySize()-2) = k*((aPrePlus-aExpSqr)*aTensor(x,aTensor.ySize()-1,z))+(a2Exp-aExpSqr)*aVals2(aTensor.ySize()-1); - for (int y = aTensor.ySize()-3; y >= 0; y--) - aVals2(y) = k*(aPrePlus*aTensor(x,y+1,z)-aExpSqr*aTensor(x,y+2,z))+a2Exp*aVals2(y+1)-aExpSqr*aVals2(y+2); - for (int y = 0; y < aTensor.ySize(); y++) - aTensor(x,y,z) = aVals1(y)+aVals2(y); - } -} - -template -void recursiveSmoothZ(CTensor& aTensor, float aSigma) { - CVector aVals1(aTensor.zSize()); - CVector aVals2(aTensor.zSize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - aVals1(0) = (0.5-k*aPreMinus)*aTensor(x,y,0); - aVals1(1) = k*(aTensor(x,y,1)+aPreMinus*aTensor(x,y,0))+(2.0*aExp-aExpSqr)*aVals1(0); - for (int z = 2; z < aTensor.zSize(); z++) - aVals1(z) = k*(aTensor(x,y,z)+aPreMinus*aTensor(x,y,z-1))+a2Exp*aVals1(z-1)-aExpSqr*aVals1(z-2); - aVals2(aTensor.zSize()-1) = (0.5+k*aPreMinus)*aTensor(x,y,aTensor.zSize()-1); - aVals2(aTensor.zSize()-2) = k*((aPrePlus-aExpSqr)*aTensor(x,y,aTensor.zSize()-1))+(a2Exp-aExpSqr)*aVals2(aTensor.zSize()-1); - for (int z = aTensor.zSize()-3; z >= 0; z--) - aVals2(z) = k*(aPrePlus*aTensor(x,y,z+1)-aExpSqr*aTensor(x,y,z+2))+a2Exp*aVals2(z+1)-aExpSqr*aVals2(z+2); - for (int z = 0; z < aTensor.zSize(); z++) - aTensor(x,y,z) = aVals1(z)+aVals2(z); - } -} - -// Linear 4D filtering --------------------------------------------------------- - -template -inline void filter(CTensor4D& aTensor, const CFilter& aFilterX, const CFilter& aFilterY, const CFilter& aFilterZ, const CFilter& aFilterA) { - CTensor4D tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize()); - filter(aTensor,tempTensor,aFilterX,1,1,1); - filter(tempTensor,aTensor,1,aFilterY,1,1); - filter(aTensor,tempTensor,1,1,aFilterZ,1); - filter(tempTensor,aTensor,1,1,1,aFilterA); -} - -template -inline void filter(CTensor4D& aTensor, const CFilter& aFilter, const int aDummy1, const int aDummy2, const int aDummy3) { - CTensor4D tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize()); - filter(aTensor,tempTensor,aFilter,1,1,1); - aTensor = tempTensor; -} - -template -void filter(const CTensor4D& aTensor, CTensor4D& aResult, const CFilter& aFilter, const int aDummy1, const int aDummy2, const int aDummy3) { - if (aResult.xSize() != aTensor.xSize() || aResult.ySize() != aTensor.ySize() || aResult.zSize() != aTensor.zSize() || aResult.aSize() != aTensor.aSize()) - throw EFilterIncompatibleSize(aTensor.xSize()*aTensor.ySize()*aTensor.zSize()*aTensor.aSize(),aResult.xSize()*aResult.ySize()*aResult.zSize()*aResult.aSize()); - int x1 = -aFilter.A(); - int x2 = aTensor.xSize()-aFilter.B(); - int a2Size = 2*aTensor.xSize()-1; - aResult = 0; - for (int a = 0; a < aTensor.aSize(); a++) - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) { - int aOffset = aTensor.xSize()*(y+aTensor.ySize()*(z+aTensor.zSize()*a)); - // Left rim - for (int x = 0; x < x1; x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (x+i < 0) aResult.data()[aOffset+x] += aFilter[i]*aTensor.data()[aOffset-1-x-i]; - else if (x+i >= aTensor.xSize()) aResult.data()[aOffset+x] += aFilter[i]*aTensor.data()[aOffset+a2Size-x-i]; - else aResult.data()[aOffset+x] += aFilter[i]*aTensor.data()[x+i+aOffset]; - } - // Center - for (int x = x1; x < x2; x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult.data()[aOffset+x] += aFilter[i]*aTensor.data()[aOffset+x+i]; - // Right rim - for (int x = x2; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (x+i < 0) aResult.data()[aOffset+x] += aFilter[i]*aTensor.data()[aOffset-1-x-i]; - else if (x+i >= aTensor.xSize()) aResult.data()[aOffset+x] += aFilter[i]*aTensor.data()[aOffset+a2Size-x-i]; - else aResult.data()[aOffset+x] += aFilter[i]*aTensor.data()[x+i+aOffset]; - } - } -} - -template -inline void filter(CTensor4D& aTensor, const int aDummy1, const CFilter& aFilter, const int aDummy2, const int aDummy3) { - CTensor4D tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize()); - filter(aTensor,tempTensor,1,aFilter,1,1); - aTensor = tempTensor; -} - -template -void filter(const CTensor4D& aTensor, CTensor4D& aResult, const int aDummy1, const CFilter& aFilter, const int aDummy2, const int aDummy3) { - if (aResult.xSize() != aTensor.xSize() || aResult.ySize() != aTensor.ySize() || aResult.zSize() != aTensor.zSize() || aResult.aSize() != aTensor.aSize()) - throw EFilterIncompatibleSize(aTensor.xSize()*aTensor.ySize()*aTensor.zSize()*aTensor.aSize(),aResult.xSize()*aResult.ySize()*aResult.zSize()*aResult.aSize()); - int y1 = -aFilter.A(); - int y2 = aTensor.ySize()-aFilter.B(); - int a2Size = 2*aTensor.ySize()-1; - aResult = 0; - for (int a = 0; a < aTensor.aSize(); a++) { - for (int z = 0; z < aTensor.zSize(); z++) { - // Upper rim - for (int y = 0; y < y1; y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (y+i < 0) aResult(x,y,z,a) += aFilter[i]*aTensor(x,-1-y-i,z,a); - else if (y+i >= aTensor.ySize()) aResult(x,y,z,a) += aFilter[i]*aTensor(x,a2Size-y-i,z,a); - else aResult(x,y,z,a) += aFilter[i]*aTensor(x,y+i,z,a); - } - // Lower rim - for (int y = y2; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (y+i < 0) aResult(x,y,z,a) += aFilter[i]*aTensor(x,-1-y-i,z,a); - else if (y+i >= aTensor.ySize()) aResult(x,y,z,a) += aFilter[i]*aTensor(x,a2Size-y-i,z,a); - else aResult(x,y,z,a) += aFilter[i]*aTensor(x,y+i,z,a); - } - } - // Center - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = y1; y < y2; y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult(x,y,z,a) += aFilter[i]*aTensor(x,y+i,z,a); - } -} - -template -inline void filter(CTensor4D& aTensor, const int aDummy1, const int aDummy2, const CFilter& aFilter, const int aDummy3) { - CTensor4D tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize()); - filter(aTensor,tempTensor,1,1,aFilter,1); - aTensor = tempTensor; -} - -template -void filter(const CTensor4D& aTensor, CTensor4D& aResult, const int aDummy1, const int aDummy2, const CFilter& aFilter, const int aDummy3) { - if (aResult.xSize() != aTensor.xSize() || aResult.ySize() != aTensor.ySize() || aResult.zSize() != aTensor.zSize() || aResult.aSize() != aTensor.aSize()) - throw EFilterIncompatibleSize(aTensor.xSize()*aTensor.ySize()*aTensor.zSize()*aTensor.aSize(),aResult.xSize()*aResult.ySize()*aResult.zSize()*aResult.aSize()); - int z1 = -aFilter.A(); - int z2 = aTensor.zSize()-aFilter.B(); - int a2Size = 2*aTensor.zSize()-1; - aResult = 0; - for (int a = 0; a < aTensor.aSize(); a++) { - // Front rim - for (int z = 0; z < z1; z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (z+i < 0) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,-1-z-i,a); - else if (z+i >= aTensor.zSize()) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,a2Size-z-i,a); - else aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z+i,a); - } - // Back rim - for (int z = z2; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (z+i < 0) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,-1-z-i,a); - else if (z+i >= aTensor.zSize()) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,a2Size-z-i,a); - else aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z+i,a); - } - // Center - for (int z = z1; z < z2; z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z+i,a); - } -} - -template -inline void filter(CTensor4D& aTensor, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter& aFilter) { - CTensor4D tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize()); - filter(aTensor,tempTensor,1,1,1,aFilter); - aTensor = tempTensor; -} - -template -void filter(const CTensor4D& aTensor, CTensor4D& aResult, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter& aFilter) { - if (aResult.xSize() != aTensor.xSize() || aResult.ySize() != aTensor.ySize() || aResult.zSize() != aTensor.zSize() || aResult.aSize() != aTensor.aSize()) - throw EFilterIncompatibleSize(aTensor.xSize()*aTensor.ySize()*aTensor.zSize()*aTensor.aSize(),aResult.xSize()*aResult.ySize()*aResult.zSize()*aResult.aSize()); - int a1 = -aFilter.A(); - int a2 = aTensor.aSize()-aFilter.B(); - int a2Size = 2*aTensor.aSize()-1; - aResult = 0; - // Front rim - for (int a = 0; a < a1; a++) - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (a+i < 0) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z,-1-a-i); - else if (a+i >= aTensor.aSize()) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z,a2Size-a-i); - else aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z,a+i); - } - // Back rim - for (int a = a2; a < aTensor.aSize(); a++) - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) { - if (a+i < 0) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z,-1-a-i); - else if (a+i >= aTensor.aSize()) aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z,a2Size-a-i); - else aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z,a+i); - } - // Center - for (int a = a1; a < a2; a++) - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) - for (int i = aFilter.A(); i < aFilter.B(); i++) - aResult(x,y,z,a) += aFilter[i]*aTensor(x,y,z,a+i); -} - -template -void recursiveSmoothX(CTensor4D& aTensor, float aSigma) { - CVector aVals1(aTensor.xSize()); - CVector aVals2(aTensor.xSize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int a = 0; a < aTensor.aSize(); a++) - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) { - int aOffset = ((a*aTensor.zSize()+z)*aTensor.ySize()+y)*aTensor.xSize(); - aVals1(0) = (0.5-k*aPreMinus)*aTensor.data()[aOffset]; - aVals1(1) = k*(aTensor.data()[aOffset+1]+aPreMinus*aTensor.data()[aOffset])+(2.0*aExp-aExpSqr)*aVals1(0); - for (int x = 2; x < aTensor.xSize(); x++) - aVals1(x) = k*(aTensor.data()[aOffset+x]+aPreMinus*aTensor.data()[aOffset+x-1])+a2Exp*aVals1(x-1)-aExpSqr*aVals1(x-2); - aVals2(aTensor.xSize()-1) = (0.5+k*aPreMinus)*aTensor.data()[aOffset+aTensor.xSize()-1]; - aVals2(aTensor.xSize()-2) = k*((aPrePlus-aExpSqr)*aTensor.data()[aOffset+aTensor.xSize()-1])+(a2Exp-aExpSqr)*aVals2(aTensor.xSize()-1); - for (int x = aTensor.xSize()-3; x >= 0; x--) - aVals2(x) = k*(aPrePlus*aTensor.data()[aOffset+x+1]-aExpSqr*aTensor.data()[aOffset+x+2])+a2Exp*aVals2(x+1)-aExpSqr*aVals2(x+2); - for (int x = 0; x < aTensor.xSize(); x++) - aTensor.data()[aOffset+x] = aVals1(x)+aVals2(x); - } -} - -template -void recursiveSmoothY(CTensor4D& aTensor, float aSigma) { - CVector aVals1(aTensor.ySize()); - CVector aVals2(aTensor.ySize()); - CVector aVals3(aTensor.ySize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int a = 0; a < aTensor.aSize(); a++) - for (int z = 0; z < aTensor.zSize(); z++) - for (int x = 0; x < aTensor.xSize(); x++) { - for (int y = 0; y < aTensor.ySize(); y++) - aVals3(y) = aTensor(x,y,z,a); - aVals1(0) = (0.5-k*aPreMinus)*aVals3(0); - aVals1(1) = k*(aVals3(1)+aPreMinus*aVals3(0))+(2.0*aExp-aExpSqr)*aVals1(0); - for (int y = 2; y < aTensor.ySize(); y++) - aVals1(y) = k*(aVals3(y)+aPreMinus*aVals3(y-1))+a2Exp*aVals1(y-1)-aExpSqr*aVals1(y-2); - aVals2(aTensor.ySize()-1) = (0.5+k*aPreMinus)*aVals3(aTensor.ySize()-1); - aVals2(aTensor.ySize()-2) = k*((aPrePlus-aExpSqr)*aVals3(aTensor.ySize()-1))+(a2Exp-aExpSqr)*aVals2(aTensor.ySize()-1); - for (int y = aTensor.ySize()-3; y >= 0; y--) - aVals2(y) = k*(aPrePlus*aVals3(y+1)-aExpSqr*aVals3(y+2))+a2Exp*aVals2(y+1)-aExpSqr*aVals2(y+2); - for (int y = 0; y < aTensor.ySize(); y++) - aTensor(x,y,z,a) = aVals1(y)+aVals2(y); - } -} - -template -void recursiveSmoothZ(CTensor4D& aTensor, float aSigma) { - CVector aVals1(aTensor.zSize()); - CVector aVals2(aTensor.zSize()); - CVector aVals3(aTensor.zSize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int a = 0; a < aTensor.aSize(); a++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - for (int z = 0; z < aTensor.zSize(); z++) - aVals3(z) = aTensor(x,y,z,a); - aVals1(0) = (0.5-k*aPreMinus)*aVals3(0); - aVals1(1) = k*(aVals3(1)+aPreMinus*aVals3(0))+(2.0*aExp-aExpSqr)*aVals1(0); - for (int z = 2; z < aTensor.zSize(); z++) - aVals1(z) = k*(aVals3(z)+aPreMinus*aVals3(z-1))+a2Exp*aVals1(z-1)-aExpSqr*aVals1(z-2); - aVals2(aTensor.zSize()-1) = (0.5+k*aPreMinus)*aVals3(aTensor.zSize()-1); - aVals2(aTensor.zSize()-2) = k*((aPrePlus-aExpSqr)*aVals3(aTensor.zSize()-1))+(a2Exp-aExpSqr)*aVals2(aTensor.zSize()-1); - for (int z = aTensor.zSize()-3; z >= 0; z--) - aVals2(z) = k*(aPrePlus*aVals3(z+1)-aExpSqr*aVals3(z+2))+a2Exp*aVals2(z+1)-aExpSqr*aVals2(z+2); - for (int z = 0; z < aTensor.zSize(); z++) - aTensor(x,y,z,a) = aVals1(z)+aVals2(z); - } -} - -template -void recursiveSmoothA(CTensor4D& aTensor, float aSigma) { - CVector aVals1(aTensor.aSize()); - CVector aVals2(aTensor.aSize()); - CVector aVals3(aTensor.aSize()); - float aAlpha = 2.5/(sqrt(NMath::Pi)*aSigma); - float aExp = exp(-aAlpha); - float aExpSqr = aExp*aExp; - float a2Exp = 2.0*aExp; - float k = (1.0-aExp)*(1.0-aExp)/(1.0+2.0*aAlpha*aExp-aExpSqr); - float aPreMinus = aExp*(aAlpha-1.0); - float aPrePlus = aExp*(aAlpha+1.0); - for (int z = 0; z < aTensor.zSize(); z++) - for (int y = 0; y < aTensor.ySize(); y++) - for (int x = 0; x < aTensor.xSize(); x++) { - for (int a = 0; a < aTensor.aSize(); a++) - aVals3(a) = aTensor(x,y,z,a); - aVals1(0) = (0.5-k*aPreMinus)*aVals3(0); - aVals1(1) = k*(aVals3(1)+aPreMinus*aVals3(0))+(2.0*aExp-aExpSqr)*aVals1(0); - for (int a = 2; a < aTensor.aSize(); a++) - aVals1(a) = k*(aVals3(a)+aPreMinus*aVals3(a-1))+a2Exp*aVals1(a-1)-aExpSqr*aVals1(a-2); - aVals2(aTensor.aSize()-1) = (0.5+k*aPreMinus)*aVals3(aTensor.aSize()-1); - aVals2(aTensor.aSize()-2) = k*((aPrePlus-aExpSqr)*aVals3(aTensor.aSize()-1))+(a2Exp-aExpSqr)*aVals2(aTensor.aSize()-1); - for (int a = aTensor.aSize()-3; a >= 0; a--) - aVals2(a) = k*(aPrePlus*aVals3(a+1)-aExpSqr*aVals3(a+2))+a2Exp*aVals2(a+1)-aExpSqr*aVals2(a+2); - for (int a = 0; a < aTensor.aSize(); a++) - aTensor(x,y,z,a) = aVals1(a)+aVals2(a); - } -} - -// Nonlinear filters ----------------------------------------------------------- - -// osher (2D) -template -void osher(CMatrix& aData, int aIterations) { - CMatrix aDiff(aData.xSize(),aData.ySize()); - for (int t = 0; t < aIterations; t++) { - for (int y = 0; y < aData.ySize(); y++) - for (int x = 0; x < aData.xSize(); x++) { - T u00,u01,u02,u10,u11,u12,u20,u21,u22; - if (x > 0) { - if (y > 0) u00 = aData(x-1,y-1); - else u00 = aData(x-1,0); - u01 = aData(x-1,y); - if (y < aData.ySize()-1) u02 = aData(x-1,y+1); - else u02 = aData(x-1,y); - } - else { - if (y > 0) u00 = aData(0,y-1); - else u00 = aData(0,0); - u01 = aData(0,y); - if (y < aData.ySize()-1) u02 = aData(0,y+1); - else u02 = aData(0,y); - } - if (y > 0) u10 = aData(x,y-1); - else u10 = aData(x,y); - u11 = aData(x,y); - if (y < aData.ySize()-1) u12 = aData(x,y+1); - else u12 = aData(x,y); - if (x < aData.xSize()-1) { - if (y > 0) u20 = aData(x+1,y-1); - else u20 = aData(x+1,y); - u21 = aData(x+1,y); - if (y < aData.ySize()-1) u22 = aData(x+1,y+1); - else u22 = aData(x+1,y); - } - else { - if (y > 0) u20 = aData(x,y-1); - else u20 = aData(x,y); - u21 = aData(x,y); - if (y < aData.ySize()-1) u22 = aData(x,y+1); - else u22 = aData(x,y); - } - T ux = 0.5*(u21-u01); - T uy = 0.5*(u12-u10); - T uxuy = ux*uy; - T uxx = u01-2.0*u11+u21; - T uyy = u10-2.0*u11+u12; - T uxy; - if (uxuy < 0) uxy = 2.0*u11+u00+u22-u10-u12-u01-u21; - else uxy = u10+u12+u01+u21-2.0*u11-u02-u20; - T laPlace = uyy*uy*uy+uxy*uxuy+uxx*ux*ux; - T uxLeft = u11-u01; - T uxRight = u21-u11; - T uyUp = u11-u10; - T uyDown = u12-u11; - if (laPlace < 0) { - T aSum = 0; - if (uxRight > 0) aSum += uxRight*uxRight; - if (uxLeft < 0) aSum += uxLeft*uxLeft; - if (uyDown > 0) aSum += uyDown*uyDown; - if (uyUp < 0) aSum += uyUp*uyUp; - aDiff(x,y) = sqrt(aSum); - } - else if (laPlace > 0) { - T aSum = 0; - if (uxRight < 0) aSum += uxRight*uxRight; - if (uxLeft > 0) aSum += uxLeft*uxLeft; - if (uyDown < 0) aSum += uyDown*uyDown; - if (uyUp > 0) aSum += uyUp*uyUp; - aDiff(x,y) = -sqrt(aSum); - } - } - for (int i = 0; i < aData.size(); i++) - aData.data()[i] += 0.25*aDiff.data()[i]; - } -} - -template -inline void osher(const CMatrix& aData, CMatrix& aResult, int aIterations) { - aResult = aData; - osher(aResult,aIterations); -} - -} - -#endif - diff --git a/video_input/consistencyChecker/CMatrix.h b/video_input/consistencyChecker/CMatrix.h deleted file mode 100644 index a49553e..0000000 --- a/video_input/consistencyChecker/CMatrix.h +++ /dev/null @@ -1,1396 +0,0 @@ -// CMatrix -// A two-dimensional array including basic matrix operations -// -// Author: Thomas Brox -//------------------------------------------------------------------------- - -#ifndef CMATRIX_H -#define CMATRIX_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef GNU_COMPILER - #include -#else - #include -#endif -#include - -template -class CMatrix { -public: - // standard constructor - inline CMatrix(); - // constructor - inline CMatrix(const int aXSize, const int aYSize); - // copy constructor - CMatrix(const CMatrix& aCopyFrom); - // constructor with implicit filling - CMatrix(const int aXSize, const int aYSize, const T aFillValue); - // destructor - virtual ~CMatrix(); - - // Changes the size of the matrix, data will be lost - void setSize(int aXSize, int aYSize); - // Downsamples the matrix - void downsampleBool(int aNewXSize, int aNewYSize, float aThreshold = 0.5); - void downsampleInt(int aNewXSize, int aNewYSize); - void downsample(int aNewXSize, int aNewYSize); - void downsample(int aNewXSize, int aNewYSize, CMatrix& aConfidence); - void downsampleBilinear(int aNewXSize, int aNewYSize); - // Upsamples the matrix - void upsample(int aNewXSize, int aNewYSize); - void upsampleBilinear(int aNewXSize, int aNewYSize); -// void upsampleBicubic(int aNewXSize, int aNewYSize); - // Scales the matrix (includes upsampling and downsampling) - void rescale(int aNewXSize, int aNewYSize); - // Creates an identity matrix - void identity(int aSize); - // Fills the matrix with the value aValue (see also operator =) - void fill(const T aValue); - // Fills a rectangular area with the value aValue - void fillRect(const T aValue, int ax1, int ay1, int ax2, int ay2); - // Copies a rectangular part from the matrix into aResult, the size of aResult will be adjusted - void cut(CMatrix& aResult,const int x1, const int y1, const int x2, const int y2); - // Copies aCopyFrom at a certain position of the matrix - void paste(CMatrix& aCopyFrom, int ax, int ay); - // Mirrors the boundaries, aFrom is the distance from the boundaries where the pixels are copied from, - // aTo is the distance from the boundaries they are copied to - void mirror(int aFrom, int aTo); - // Transforms the values so that they are all between aMin and aMax - // aInitialMin/Max are initializations for seeking the minimum and maximum, change if your - // data is not in this range or the data type T cannot hold these values - void normalize(T aMin, T aMax, T aInitialMin = -30000, T aInitialMax = 30000); - // Clips values that exceed the given range - void clip(T aMin, T aMax); - - // Applies a similarity transform (translation, rotation, scaling) to the image - void applySimilarityTransform(CMatrix& aWarped, CMatrix& aOutside, float tx, float ty, float cx, float cy, float phi, float scale); - // Applies a homography (linear projective transformation) to the image - void applyHomography(CMatrix& aWarped, CMatrix& aOutside, const CMatrix& H); - - // Draws a line into the image - void drawLine(int dStartX, int dStartY, int dEndX, int dEndY, T aValue = 255); - // Inverts a gray value image - void invertImage(); - // Extracts the connected component starting from (x,y) - // Component -> 255, Remaining area -> 0 - void connectedComponent(int x, int y); - - // Appends another matrix with the same column number - void append(CMatrix& aMatrix); - // Inverts a square matrix with Gauss elimination - void inv(); - // Transposes a square matrix - void trans(); - // Multiplies with two vectors (from left and from right) - float scalar(CVector& aLeft, CVector& aRight); - - // Reads a picture from a pgm-File - void readFromPGM(const char* aFilename); - // Saves the matrix as a picture in pgm-Format - void writeToPGM(const char *aFilename); - // Read matrix from text file - void readFromTXT(const char* aFilename, bool aHeader = true, int aXSize = 0, int aYSize = 0); - // Read matrix from Matlab ascii file - void readFromMatlabTXT(const char* aFilename, bool aHeader = true, int aXSize = 0, int aYSize = 0); - // Save matrix as text file - void writeToTXT(const char* aFilename, bool aHeader = true); - // Reads a projection matrix in a format used by Bodo Rosenhahn - void readBodoProjectionMatrix(const char* aFilename); - - // Gives full access to matrix values - inline T& operator()(const int ax, const int ay) const; - // Fills the matrix with the value aValue (equivalent to fill()) - inline CMatrix& operator=(const T aValue); - // Copies the matrix aCopyFrom to this matrix (size of matrix might change) - CMatrix& operator=(const CMatrix& aCopyFrom); - // matrix sum - CMatrix& operator+=(const CMatrix& aMatrix); - // Adds a constant to the matrix - CMatrix& operator+=(const T aValue); - // matrix difference - CMatrix& operator-=(const CMatrix& aMatrix); - // matrix product - CMatrix& operator*=(const CMatrix& aMatrix); - // Multiplication with a scalar - CMatrix& operator*=(const T aValue); - - // Comparison of two matrices - bool operator==(const CMatrix& aMatrix); - - // Returns the minimum value - T min() const; - // Returns the maximum value - T max() const; - // Returns the average value - T avg() const; - // Gives access to the matrix' size - inline int xSize() const; - inline int ySize() const; - inline int size() const; - // Returns one row from the matrix - void getVector(CVector& aVector, int ay); - // Gives access to the internal data representation - inline T* data() const; -protected: - int mXSize,mYSize; - T *mData; -}; - -// Returns a matrix where all negative elements are turned positive -template CMatrix abs(const CMatrix& aMatrix); -// Returns the tranposed matrix -template CMatrix trans(const CMatrix& aMatrix); -// matrix sum -template CMatrix operator+(const CMatrix& aM1, const CMatrix& aM2); -// matrix difference -template CMatrix operator-(const CMatrix& aM1, const CMatrix& aM2); -// matrix product -template CMatrix operator*(const CMatrix& aM1, const CMatrix& aM2); -// Multiplication with a vector -template CVector operator*(const CMatrix& aMatrix, const CVector& aVector); -// Multiplikation with a scalar -template CMatrix operator*(const CMatrix& aMatrix, const T aValue); -template inline CMatrix operator*(const T aValue, const CMatrix& aMatrix); -// Provides basic output functionality (only appropriate for small matrices) -template std::ostream& operator<<(std::ostream& aStream, const CMatrix& aMatrix); - -// Exceptions thrown by CMatrix------------------------------------------- - - -// Thrown when one tries to access an element of a matrix which is out of -// the matrix' bounds -struct EMatrixRangeOverflow { - EMatrixRangeOverflow(const int ax, const int ay) { - using namespace std; - cerr << "Exception EMatrixRangeOverflow: x = " << ax << ", y = " << ay << endl; - } -}; - -// Thrown when one tries to multiply two matrices where M1's column number -// is not equal to M2's row number or when one tries to add two matrices -// which have not the same size -struct EIncompatibleMatrices { - EIncompatibleMatrices(const int x1, const int y1, const int x2, const int y2) { - - using namespace std; - cerr << "Exception EIncompatibleMatrices: M1 = " << x1 << "x" << y1; - cerr << " M2 = " << x2 << "x" << y2 << endl; - } -}; - -// Thrown when a nonquadratic matrix is tried to be inversed -struct ENonquadraticMatrix { - ENonquadraticMatrix(const int x, const int y) { - using namespace std; - cerr << "Exception ENonquadarticMatrix: M = " << x << "x" << y << endl; - } -}; - -// Thrown when a matrix is not positive definite -struct ENonPositiveDefinite { - ENonPositiveDefinite() { - using namespace std; - cerr << "Exception ENonPositiveDefinite" << endl; - } -}; - -// Thrown when reading a file which does not keep to the PGM specification -struct EInvalidFileFormat { - EInvalidFileFormat(const char* s) { - using namespace std; - cerr << "Exception EInvalidFileFormat: File is not in " << s << " format" << endl; - } -}; - -// I M P L E M E N T A T I O N -------------------------------------------- -// -// You might wonder why there is implementation code in a header file. -// The reason is that not all C++ compilers yet manage separate compilation -// of templates. Inline functions cannot be compiled separately anyway. -// So in this case the whole implementation code is added to the header -// file. -// Users of CMatrix should ignore everything that's beyond this line :) -// ------------------------------------------------------------------------ - -// P U B L I C ------------------------------------------------------------ - -// standard constructor -template -inline CMatrix::CMatrix() { - mData = 0; mXSize = mYSize = 0; -} - -// constructor -template -inline CMatrix::CMatrix(const int aXSize, const int aYSize) - : mXSize(aXSize), mYSize(aYSize) { - mData = new T[aXSize*aYSize]; -} - -// copy constructor -template -CMatrix::CMatrix(const CMatrix& aCopyFrom) - : mXSize(aCopyFrom.mXSize), mYSize(aCopyFrom.mYSize) { - if (aCopyFrom.mData == 0) mData = 0; - else { - int wholeSize = mXSize*mYSize; - mData = new T[wholeSize]; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aCopyFrom.mData[i]; - } -} - -// constructor with implicit filling -template -CMatrix::CMatrix(const int aXSize, const int aYSize, const T aFillValue) - : mXSize(aXSize), mYSize(aYSize) { - mData = new T[aXSize*aYSize]; - fill(aFillValue); -} - -// destructor -template -CMatrix::~CMatrix() { - delete [] mData; -} - -// setSize -template -void CMatrix::setSize(int aXSize, int aYSize) { - if (mData != 0) delete[] mData; - mData = new T[aXSize*aYSize]; - mXSize = aXSize; - mYSize = aYSize; -} - -// downsampleBool -template -void CMatrix::downsampleBool(int aNewXSize, int aNewYSize, float aThreshold) { - CMatrix aTemp(mXSize,mYSize); - int aSize = size(); - for (int i = 0; i < aSize; i++) - aTemp.data()[i] = mData[i]; - aTemp.downsample(aNewXSize,aNewYSize); - setSize(aNewXSize,aNewYSize); - aSize = size(); - for (int i = 0; i < aSize; i++) - mData[i] = (aTemp.data()[i] >= aThreshold); -} - -// downsampleInt -template -void CMatrix::downsampleInt(int aNewXSize, int aNewYSize) { - T* newData = new int[aNewXSize*aNewYSize]; - float factorX = ((float)mXSize)/aNewXSize; - float factorY = ((float)mYSize)/aNewYSize; - float ay = 0.0; - for (int y = 0; y < aNewYSize; y++) { - float ax = 0.0; - for (int x = 0; x < aNewXSize; x++) { - CVector aHistogram(256,0.0); - for (float by = 0.0; by < factorY;) { - float restY = floor(by+1.0)-by; - if (restY+by >= factorY) restY = factorY-by; - for (float bx = 0.0; bx < factorX;) { - float restX = floor(bx+1.0)-bx; - if (restX+bx >= factorX) restX = factorX-bx; - aHistogram(operator()((int)(ax+bx),(int)(ay+by))) += restX*restY; - bx += restX; - } - by += restY; - } - float aMax = 0; int aMaxVal; - for (int i = 0; i < aHistogram.size(); i++) - if (aHistogram(i) > aMax) { - aMax = aHistogram(i); - aMaxVal = i; - } - newData[x+aNewXSize*y] = aMaxVal; - ax += factorX; - } - ay += factorY; - } - delete[] mData; - mData = newData; - mXSize = aNewXSize; mYSize = aNewYSize; -} - -template -void CMatrix::downsample(int aNewXSize, int aNewYSize) { - // Downsample in x-direction - int aIntermedSize = aNewXSize*mYSize; - T* aIntermedData = new T[aIntermedSize]; - if (aNewXSize < mXSize) { - for (int i = 0; i < aIntermedSize; i++) - aIntermedData[i] = 0.0; - T factor = ((float)mXSize)/aNewXSize; - for (int y = 0; y < mYSize; y++) { - int aFineOffset = y*mXSize; - int aCoarseOffset = y*aNewXSize; - int i = aFineOffset; - int j = aCoarseOffset; - int aLastI = aFineOffset+mXSize; - int aLastJ = aCoarseOffset+aNewXSize; - T rest = factor; - T part = 1.0; - do { - if (rest > 1.0) { - aIntermedData[j] += part*mData[i]; - rest -= part; - part = 1.0; - i++; - if (rest <= 0.0) { - rest = factor; - j++; - } - } - else { - aIntermedData[j] += rest*mData[i]; - part = 1.0-rest; - rest = factor; - j++; - } - } - while (i < aLastI && j < aLastJ); - } - } - else { - T* aTemp = aIntermedData; - aIntermedData = mData; - mData = aTemp; - } - // Downsample in y-direction - delete[] mData; - int aDataSize = aNewXSize*aNewYSize; - mData = new T[aDataSize]; - if (aNewYSize < mYSize) { - for (int i = 0; i < aDataSize; i++) - mData[i] = 0.0; - float factor = ((float)mYSize)/aNewYSize; - for (int x = 0; x < aNewXSize; x++) { - int i = x; - int j = x; - int aLastI = mYSize*aNewXSize+x; - int aLastJ = aNewYSize*aNewXSize+x; - float rest = factor; - float part = 1.0; - do { - if (rest > 1.0) { - mData[j] += part*aIntermedData[i]; - rest -= part; - part = 1.0; - i += aNewXSize; - if (rest <= 0.0) { - rest = factor; - j += aNewXSize; - } - } - else { - mData[j] += rest*aIntermedData[i]; - part = 1.0-rest; - rest = factor; - j += aNewXSize; - } - } - while (i < aLastI && j < aLastJ); - } - } - else { - T* aTemp = mData; - mData = aIntermedData; - aIntermedData = aTemp; - } - // Normalize - float aNormalization = ((float)aDataSize)/size(); - for (int i = 0; i < aDataSize; i++) - mData[i] *= aNormalization; - // Adapt size of matrix - mXSize = aNewXSize; - mYSize = aNewYSize; - delete[] aIntermedData; -} - -template -void CMatrix::downsample(int aNewXSize, int aNewYSize, CMatrix& aConfidence) { - int aNewSize = aNewXSize*aNewYSize; - T* newData = new T[aNewSize]; - float* aCounter = new float[aNewSize]; - for (int i = 0; i < aNewSize; i++) { - newData[i] = 0; - aCounter[i] = 0; - } - float factorX = ((float)aNewXSize)/mXSize; - float factorY = ((float)aNewYSize)/mYSize; - for (int y = 0; y < mYSize; y++) - for (int x = 0; x < mXSize; x++) - if (aConfidence(x,y) > 0) { - float ax = x*factorX; - float ay = y*factorY; - int x1 = (int)ax; - int y1 = (int)ay; - int x2 = x1+1; - int y2 = y1+1; - float alphax = ax-x1; - float betax = 1.0-alphax; - float alphay = ay-y1; - float betay = 1.0-alphay; - float conf = aConfidence(x,y); - T val = conf*operator()(x,y); - int i = x1+aNewXSize*y1; - newData[i] += betax*betay*val; - aCounter[i] += betax*betay*conf; - if (x2 < aNewXSize) { - i = x2+aNewXSize*y1; - newData[i] += alphax*betay*val; - aCounter[i] += alphax*betay*conf; - } - if (y2 < aNewYSize) { - i = x1+aNewXSize*y2; - newData[i] += betax*alphay*val; - aCounter[i] += betax*alphay*conf; - } - if (x2 < aNewXSize && y2 < aNewYSize) { - i = x2+aNewXSize*y2; - newData[i] += alphax*alphay*val; - aCounter[i] += alphax*alphay*conf; - } - } - for (int i = 0; i < aNewSize; i++) - if (aCounter[i] > 0) newData[i] /= aCounter[i]; - // Adapt size of matrix - mXSize = aNewXSize; - mYSize = aNewYSize; - delete[] mData; - delete[] aCounter; - mData = newData; -} - -// downsampleBilinear -template -void CMatrix::downsampleBilinear(int aNewXSize, int aNewYSize) { - int aNewSize = aNewXSize*aNewYSize; - T* aNewData = new T[aNewSize]; - float factorX = ((float)mXSize)/aNewXSize; - float factorY = ((float)mYSize)/aNewYSize; - for (int y = 0; y < aNewYSize; y++) - for (int x = 0; x < aNewXSize; x++) { - float ax = (x+0.5)*factorX-0.5; - float ay = (y+0.5)*factorY-0.5; - if (ax < 0) ax = 0.0; - if (ay < 0) ay = 0.0; - int x1 = (int)ax; - int y1 = (int)ay; - int x2 = x1+1; - int y2 = y1+1; - float alphaX = ax-x1; - float alphaY = ay-y1; - if (x1 < 0) x1 = 0; - if (y1 < 0) y1 = 0; - if (x2 >= mXSize) x2 = mXSize-1; - if (y2 >= mYSize) y2 = mYSize-1; - float a = (1.0-alphaX)*mData[x1+y1*mXSize]+alphaX*mData[x2+y1*mXSize]; - float b = (1.0-alphaX)*mData[x1+y2*mXSize]+alphaX*mData[x2+y2*mXSize]; - aNewData[x+y*aNewXSize] = (1.0-alphaY)*a+alphaY*b; - } - delete[] mData; - mData = aNewData; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -template -void CMatrix::upsample(int aNewXSize, int aNewYSize) { - // Upsample in x-direction - int aIntermedSize = aNewXSize*mYSize; - T* aIntermedData = new T[aIntermedSize]; - if (aNewXSize > mXSize) { - for (int i = 0; i < aIntermedSize; i++) - aIntermedData[i] = 0.0; - T factor = ((float)aNewXSize)/mXSize; - for (int y = 0; y < mYSize; y++) { - int aFineOffset = y*aNewXSize; - int aCoarseOffset = y*mXSize; - int i = aCoarseOffset; - int j = aFineOffset; - int aLastI = aCoarseOffset+mXSize; - int aLastJ = aFineOffset+aNewXSize; - T rest = factor; - T part = 1.0; - do { - if (rest > 1.0) { - aIntermedData[j] += part*mData[i]; - rest -= part; - part = 1.0; - j++; - if (rest <= 0.0) { - rest = factor; - i++; - } - } - else { - aIntermedData[j] += rest*mData[i]; - part = 1.0-rest; - rest = factor; - i++; - } - } - while (i < aLastI && j < aLastJ); - } - } - else { - T* aTemp = aIntermedData; - aIntermedData = mData; - mData = aTemp; - } - // Upsample in y-direction - delete[] mData; - int aDataSize = aNewXSize*aNewYSize; - mData = new T[aDataSize]; - if (aNewYSize > mYSize) { - for (int i = 0; i < aDataSize; i++) - mData[i] = 0.0; - float factor = ((float)aNewYSize)/mYSize; - for (int x = 0; x < aNewXSize; x++) { - int i = x; - int j = x; - int aLastI = mYSize*aNewXSize; - int aLastJ = aNewYSize*aNewXSize; - float rest = factor; - float part = 1.0; - do { - if (rest > 1.0) { - mData[j] += part*aIntermedData[i]; - rest -= part; - part = 1.0; - j += aNewXSize; - if (rest <= 0.0) { - rest = factor; - i += aNewXSize; - } - } - else { - mData[j] += rest*aIntermedData[i]; - part = 1.0-rest; - rest = factor; - i += aNewXSize; - } - } - while (i < aLastI && j < aLastJ); - } - } - else { - T* aTemp = mData; - mData = aIntermedData; - aIntermedData = aTemp; - } - // Adapt size of matrix - mXSize = aNewXSize; - mYSize = aNewYSize; - delete[] aIntermedData; -} - -// upsampleBilinear -template -void CMatrix::upsampleBilinear(int aNewXSize, int aNewYSize) { - int aNewSize = aNewXSize*aNewYSize; - T* aNewData = new T[aNewSize]; - float factorX = (float)(mXSize)/(aNewXSize); - float factorY = (float)(mYSize)/(aNewYSize); - for (int y = 0; y < aNewYSize; y++) - for (int x = 0; x < aNewXSize; x++) { - float ax = (x+0.5)*factorX-0.5; - float ay = (y+0.5)*factorY-0.5; - if (ax < 0) ax = 0.0; - if (ay < 0) ay = 0.0; - int x1 = (int)ax; - int y1 = (int)ay; - int x2 = x1+1; - int y2 = y1+1; - float alphaX = ax-x1; - float alphaY = ay-y1; - if (x1 < 0) x1 = 0; - if (y1 < 0) y1 = 0; - if (x2 >= mXSize) x2 = mXSize-1; - if (y2 >= mYSize) y2 = mYSize-1; - float a = (1.0-alphaX)*mData[x1+y1*mXSize]+alphaX*mData[x2+y1*mXSize]; - float b = (1.0-alphaX)*mData[x1+y2*mXSize]+alphaX*mData[x2+y2*mXSize]; - aNewData[x+y*aNewXSize] = (1.0-alphaY)*a+alphaY*b; - } - delete[] mData; - mData = aNewData; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -template -void CMatrix::rescale(int aNewXSize, int aNewYSize) { - if (mXSize >= aNewXSize) { - if (mYSize >= aNewYSize) downsample(aNewXSize,aNewYSize); - else { - downsample(aNewXSize,mYSize); - upsample(aNewXSize,aNewYSize); - } - } - else { - if (mYSize >= aNewYSize) { - downsample(mXSize,aNewYSize); - upsample(aNewXSize,aNewYSize); - } - else upsample(aNewXSize,aNewYSize); - } -} - -// identity -template -void CMatrix::identity(int aSize) { - if (aSize != mXSize || aSize != mYSize) { - delete[] mData; - mData = new T[aSize*aSize]; - mXSize = aSize; - mYSize = aSize; - } - fill(0); - for (int i = 0; i < aSize; i++) - operator()(i,i) = 1; -} - -// fill -template -void CMatrix::fill(const T aValue) { - int wholeSize = mXSize*mYSize; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aValue; -} - -// fillRect -template -void CMatrix::fillRect(const T aValue, int ax1, int ay1, int ax2, int ay2) { - for (int y = ay1; y <= ay2; y++) - for (register int x = ax1; x <= ax2; x++) - operator()(x,y) = aValue; -} - -// cut -template -void CMatrix::cut(CMatrix& aResult,const int x1, const int y1, const int x2, const int y2) { - aResult.mXSize = x2-x1+1; - aResult.mYSize = y2-y1+1; - delete[] aResult.mData; - aResult.mData = new T[aResult.mXSize*aResult.mYSize]; - for (int y = y1; y <= y2; y++) - for (int x = x1; x <= x2; x++) - aResult(x-x1,y-y1) = operator()(x,y); -} - -// paste -template -void CMatrix::paste(CMatrix& aCopyFrom, int ax, int ay) { - for (int y = 0; y < aCopyFrom.ySize(); y++) - for (int x = 0; x < aCopyFrom.xSize(); x++) - operator()(ax+x,ay+y) = aCopyFrom(x,y); -} - -// mirror -template -void CMatrix::mirror(int aFrom, int aTo) { - int aToXIndex = mXSize-aTo-1; - int aToYIndex = mYSize-aTo-1; - int aFromXIndex = mXSize-aFrom-1; - int aFromYIndex = mYSize-aFrom-1; - for (int y = aFrom; y <= aFromYIndex; y++) { - operator()(aTo,y) = operator()(aFrom,y); - operator()(aToXIndex,y) = operator()(aFromXIndex,y); - } - for (int x = aTo; x <= aToXIndex; x++) { - operator()(x,aTo) = operator()(x,aFrom); - operator()(x,aToYIndex) = operator()(x,aFromYIndex); - } -} - -// normalize -template -void CMatrix::normalize(T aMin, T aMax, T aInitialMin, T aInitialMax) { - int aSize = mXSize*mYSize; - T aCurrentMin = aInitialMax; - T aCurrentMax = aInitialMin; - for (int i = 0; i < aSize; i++) - if (mData[i] > aCurrentMax) aCurrentMax = mData[i]; - else if (mData[i] < aCurrentMin) aCurrentMin = mData[i]; - T aTemp = (aCurrentMax-aCurrentMin); - if (aTemp == 0) aTemp = 1; - else aTemp = (aMax-aMin)/aTemp; - for (int i = 0; i < aSize; i++) { - mData[i] -= aCurrentMin; - mData[i] *= aTemp; - mData[i] += aMin; - } -} - -// clip -template -void CMatrix::clip(T aMin, T aMax) { - int aSize = size(); - for (int i = 0; i < aSize; i++) - if (mData[i] < aMin) mData[i] = aMin; - else if (mData[i] > aMax) mData[i] = aMax; -} - -// applySimilarityTransform -template -void CMatrix::applySimilarityTransform(CMatrix& aWarped, CMatrix& aOutside, float tx, float ty, float cx, float cy, float phi, float scale) { - float cosphi = scale*cos(phi); - float sinphi = scale*sin(phi); - float ctx = cx+tx-cx*cosphi+cy*sinphi; - float cty = cy+ty-cy*cosphi-cx*sinphi; - aOutside = false; - int i = 0; - for (int y = 0; y < aWarped.ySize(); y++) - for (int x = 0; x < aWarped.xSize(); x++,i++) { - float xf = x; float yf = y; - float ax = xf*cosphi-yf*sinphi+ctx; - float ay = yf*cosphi+xf*sinphi+cty; - int x1 = (int)ax; int y1 = (int)ay; - float alphaX = ax-x1; float alphaY = ay-y1; - float betaX = 1.0-alphaX; float betaY = 1.0-alphaY; - if (x1 < 0 || y1 < 0 || x1+1 >= mXSize || y1+1 >= mYSize) aOutside.data()[i] = true; - else { - int j = y1*mXSize+x1; - float a = betaX*mData[j] +alphaX*mData[j+1]; - float b = betaX*mData[j+mXSize]+alphaX*mData[j+1+mXSize]; - aWarped.data()[i] = betaY*a+alphaY*b; - } - } -} - -// applyHomography -template -void CMatrix::applyHomography(CMatrix& aWarped, CMatrix& aOutside, const CMatrix& H) { - int aSize = size(); - aOutside = false; - int i = 0; - for (int y = 0; y < aWarped.ySize(); y++) - for (int x = 0; x < aWarped.xSize(); x++,i++) { - float xf = x; float yf = y; - float ax = H.data()[0]*xf+H.data()[1]*yf+H.data()[2]; - float ay = H.data()[3]*xf+H.data()[4]*yf+H.data()[5]; - float az = H.data()[6]*xf+H.data()[7]*yf+H.data()[8]; - float invaz = 1.0/az; - ax *= invaz; ay *= invaz; - int x1 = (int)ax; int y1 = (int)ay; - float alphaX = ax-x1; float alphaY = ay-y1; - float betaX = 1.0-alphaX; float betaY = 1.0-alphaY; - if (x1 < 0 || y1 < 0 || x1+1 >= mXSize || y1+1 >= mYSize) aOutside.data()[i] = true; - else { - int j = y1*mXSize+x1; - float a = betaX*mData[j] +alphaX*mData[j+1]; - float b = betaX*mData[j+mXSize]+alphaX*mData[j+1+mXSize]; - aWarped.data()[i] = betaY*a+alphaY*b; - } - } -} - -// drawLine -template -void CMatrix::drawLine(int dStartX, int dStartY, int dEndX, int dEndY, T aValue) { - // vertical line - if (dStartX == dEndX) { - if (dStartX < 0 || dStartX >= mXSize) return; - int x = dStartX; - if (dStartY < dEndY) { - for (int y = dStartY; y <= dEndY; y++) - if (y >= 0 && y < mYSize) mData[x+y*mXSize] = aValue; - } - else { - for (int y = dStartY; y >= dEndY; y--) - if (y >= 0 && y < mYSize) mData[x+y*mXSize] = aValue; - } - return; - } - // horizontal line - if (dStartY == dEndY) { - if (dStartY < 0 || dStartY >= mYSize) return; - int y = dStartY; - if (dStartX < dEndX) { - for (int x = dStartX; x <= dEndX; x++) - if (x >= 0 && x < mXSize) mData[x+y*mXSize] = aValue; - } - else { - for (int x = dStartX; x >= dEndX; x--) - if (x >= 0 && x < mXSize) mData[x+y*mXSize] = aValue; - } - return; - } - float m = float(dStartY - dEndY) / float(dStartX - dEndX); - float invm = 1.0/m; - if (fabs(m) > 1.0) { - if (dEndY > dStartY) { - for (int y = dStartY; y <= dEndY; y++) { - int x = (int)(0.5+dStartX+(y-dStartY)*invm); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) - mData[x+y*mXSize] = aValue; - } - } - else { - for (int y = dStartY; y >= dEndY; y--) { - int x = (int)(0.5+dStartX+(y-dStartY)*invm); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) - mData[x+y*mXSize] = aValue; - } - } - } - else { - if (dEndX > dStartX) { - for (int x = dStartX; x <= dEndX; x++) { - int y = (int)(0.5+dStartY+(x-dStartX)*m); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) - mData[x+y*mXSize] = aValue; - } - } - else { - for (int x = dStartX; x >= dEndX; x--) { - int y = (int)(0.5+dStartY+(x-dStartX)*m); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) - mData[x+y*mXSize] = aValue; - } - } - } -} - -// invertImage -template -void CMatrix::invertImage() { - int aSize = mXSize*mYSize; - for (int i = 0; i < aSize; i++) - mData[i] = 255-mData[i]; -} - -// connectedComponent -typedef struct {short y, xl, xr, dy;} CSegment; - -template -void CMatrix::connectedComponent (int x, int y) { - std::stack aStack; - #define PUSH(Y,XL,XR,DY) if (Y+(DY)>=0 && Y+(DY) aConnected(mXSize,mYSize,false); - int l,x1,x2,dy; - PUSH(y,x,x,1); - PUSH(y+1,x,x,-1); - while (!aStack.empty()) { - POP(y,x1,x2,dy); - for (x=x1; x >= 0 && operator()(x,y) == aCompValue && !aConnected(x,y);x--) - aConnected(x,y) = true; - if (x >= x1) goto skip2; - l = x+1; - if (l < x1) PUSH(y,l,x1-1,-dy); - x = x1+1; - do { - for (; x < mXSize && operator()(x,y) == aCompValue && !aConnected(x,y); x++) - aConnected(x,y) = true; - PUSH(y,l,x-1,dy); - if (x>x2+1) PUSH(y,x2+1,x-1,-dy); - skip2: for (x++;x <= x2 && (operator()(x,y) != aCompValue || aConnected(x,y)); x++); - l = x; - } - while (x <= x2); - } - int aSize = size(); - for (int i = 0; i < aSize; i++) - if (aConnected.data()[i]) mData[i] = 255; - else mData[i] = 0; - #undef PUSH - #undef POP -} - -// append -template -void CMatrix::append(CMatrix& aMatrix) { - #ifdef _DEBUG - if (aMatrix.xSize() != mXSize) throw EIncompatibleMatrices(mXSize,mYSize,aMatrix.xSize(),aMatrix.ySize()); - #endif - T* aNew = new T[mXSize*(mYSize+aMatrix.ySize())]; - int aSize = mXSize*mYSize; - for (int i = 0; i < aSize; i++) - aNew[i] = mData[i]; - int aSize2 = mXSize*aMatrix.ySize(); - for (int i = 0; i < aSize2; i++) - aNew[i+aSize] = aMatrix.data()[i]; - delete[] mData; - mData = aNew; - mYSize += aMatrix.ySize(); -} - -// inv -template -void CMatrix::inv() { - if (mXSize != mYSize) throw ENonquadraticMatrix(mXSize,mYSize); - int* p = new int[mXSize]; - T* hv = new T[mXSize]; - CMatrix& I(*this); - int n = mYSize; - for (int j = 0; j < n; j++) - p[j] = j; - for (int j = 0; j < n; j++) { - T max = fabs(I(j,j)); - int r = j; - for (int i = j+1; i < n; i++) - if (fabs(I(j,i)) > max) { - max = fabs(I(j,i)); - r = i; - } - // Matrix singular - if (max <= 0) return; - // Swap row j and r - if (r > j) { - for (int k = 0; k < n; k++) { - T hr = I(k,j); - I(k,j) = I(k,r); - I(k,r) = hr; - } - int hi = p[j]; - p[j] = p[r]; - p[r] = hi; - } - T hr = 1/I(j,j); - for (int i = 0; i < n; i++) - I(j,i) *= hr; - I(j,j) = hr; - hr *= -1; - for (int k = 0; k < n; k++) - if (k != j) { - for (int i = 0; i < n; i++) - if (i != j) I(k,i) -= I(j,i)*I(k,j); - I(k,j) *= hr; - } - } - for (int i = 0; i < n; i++) { - for (int k = 0; k < n; k++) - hv[p[k]] = I(k,i); - for (int k = 0; k < n; k++) - I(k,i) = hv[k]; - } - delete[] p; - delete[] hv; -} - -template -void CMatrix::trans() { - for (int y = 0; y < mYSize; y++) - for (int x = y; x < mXSize; x++) { - float temp = operator()(x,y); - operator()(x,y) = operator()(y,x); - operator()(y,x) = temp; - } -} - -template -float CMatrix::scalar(CVector& aLeft, CVector& aRight) { - #ifdef _DEBUG - if ((aLeft.size() != mYSize) || (aRight.size() != mXSize)) - throw EIncompatibleMatrices(mXSize,mYSize,aRight.size(),aLeft.size()); - #endif - T* vec = new T[mYSize]; - T* dat = mData; - for (int y = 0; y < mYSize; y++) { - vec[y] = 0; - for (int x = 0; x < mXSize; x++) - vec[y] += *(dat++)*aRight(x); - } - T aResult = 0.0; - for (int y = 0; y < mYSize; y++) - aResult += vec[y]*aLeft(y); - delete[] vec; - return aResult; -} - -// readFromPGM -template -void CMatrix::readFromPGM(const char* aFilename) { - FILE *aStream; - aStream = fopen(aFilename,"rb"); - if (aStream == 0) std::cerr << "File not found: " << aFilename << std::endl; - int dummy; - // Find beginning of file (P5) - while (getc(aStream) != 'P'); - if (getc(aStream) != '5') throw EInvalidFileFormat("PGM"); - do dummy = getc(aStream); while (dummy != '\n' && dummy != ' '); - // Remove comments and empty lines - dummy = getc(aStream); - while (dummy == '#') { - while (getc(aStream) != '\n'); - dummy = getc(aStream); - } - while (dummy == '\n') - dummy = getc(aStream); - // Read image size - mXSize = dummy-48; - while ((dummy = getc(aStream)) >= 48 && dummy < 58) - mXSize = 10*mXSize+dummy-48; - while ((dummy = getc(aStream)) < 48 || dummy >= 58); - mYSize = dummy-48; - while ((dummy = getc(aStream)) >= 48 && dummy < 58) - mYSize = 10*mYSize+dummy-48; - while (dummy != '\n' && dummy != ' ') - dummy = getc(aStream); - while ((dummy = getc(aStream)) >= 48 && dummy < 58); - if (dummy != '\n') while (getc(aStream) != '\n'); - // Adjust size of data structure - delete[] mData; - mData = new T[mXSize*mYSize]; - // Read image data - for (int i = 0; i < mXSize*mYSize; i++) - mData[i] = getc(aStream); - fclose(aStream); -} - -// writeToPGM -template -void CMatrix::writeToPGM(const char *aFilename) { - FILE *aStream; - aStream = fopen(aFilename,"wb"); - // write header - char line[60]; - sprintf(line,"P5\n%d %d\n255\n",mXSize,mYSize); - fwrite(line,strlen(line),1,aStream); - // write data - for (int i = 0; i < mXSize*mYSize; i++) { - char dummy = (char)mData[i]; - fwrite(&dummy,1,1,aStream); - } - fclose(aStream); -} - -// readFromTXT -template -void CMatrix::readFromTXT(const char* aFilename, bool aHeader, int aXSize, int aYSize) { - std::ifstream aStream(aFilename); - // read header - if (aHeader) aStream >> mXSize >> mYSize; - else { - mXSize = aXSize; - mYSize = aYSize; - } - // Adjust size of data structure - delete[] mData; - mData = new T[mXSize*mYSize]; - // read data - for (int i = 0; i < mXSize*mYSize; i++) - aStream >> mData[i]; -} - -// readFromMatlabTXT -template -void CMatrix::readFromMatlabTXT(const char* aFilename, bool aHeader, int aXSize, int aYSize) { - std::ifstream aStream(aFilename); - // read header - float nx,ny; - if (aHeader) { - aStream >> nx >> ny; - mXSize = (int)nx; mYSize = (int)ny; - } - else { - mXSize = aXSize; mYSize = aYSize; - } - // Adjust size of data structure - delete[] mData; - mData = new T[mXSize*mYSize]; - // read data - for (int i = 0; i < mXSize*mYSize; i++) - aStream >> mData[i]; -} - -//writeToTXT -template -void CMatrix::writeToTXT(const char* aFilename, bool aHeader) { - std::ofstream aStream(aFilename); - // write header - if (aHeader) aStream << mXSize << " " << mYSize << std::endl; - // write data - int i = 0; - for (int y = 0; y < mYSize; y++) { - for (int x = 0; x < mXSize; x++, i++) - aStream << mData[i] << " "; - aStream << std::endl; - } -} - -// readBodoProjectionMatrix -template -void CMatrix::readBodoProjectionMatrix(const char* aFilename) { - readFromTXT(aFilename,false,4,3); -} - -// operator () -template -inline T& CMatrix::operator()(const int ax, const int ay) const { - #ifdef _DEBUG - if (ax >= mXSize || ay >= mYSize || ax < 0 || ay < 0) - throw EMatrixRangeOverflow(ax,ay); - #endif - return mData[mXSize*ay+ax]; -} - -// operator = -template -inline CMatrix& CMatrix::operator=(const T aValue) { - fill(aValue); - return *this; -} - -template -CMatrix& CMatrix::operator=(const CMatrix& aCopyFrom) { - if (this != &aCopyFrom) { - if (mData != 0) delete[] mData; - mXSize = aCopyFrom.mXSize; - mYSize = aCopyFrom.mYSize; - if (aCopyFrom.mData == 0) mData = 0; - else { - int wholeSize = mXSize*mYSize; - mData = new T[wholeSize]; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aCopyFrom.mData[i]; - } - } - return *this; -} - -// operator += -template -CMatrix& CMatrix::operator+=(const CMatrix& aMatrix) { - if ((mXSize != aMatrix.mXSize) || (mYSize != aMatrix.mYSize)) - throw EIncompatibleMatrices(mXSize,mYSize,aMatrix.mXSize,aMatrix.mYSize); - int wholeSize = mXSize*mYSize; - for (int i = 0; i < wholeSize; i++) - mData[i] += aMatrix.mData[i]; - return *this; -} - -template -CMatrix& CMatrix::operator+=(const T aValue) { - int wholeSize = mXSize*mYSize; - for (int i = 0; i < wholeSize; i++) - mData[i] += aValue; - return *this; -} - -// operator -= -template -CMatrix& CMatrix::operator-=(const CMatrix& aMatrix) { - if ((mXSize != aMatrix.mXSize) || (mYSize != aMatrix.mYSize)) - throw EIncompatibleMatrices(mXSize,mYSize,aMatrix.mXSize,aMatrix.mYSize); - int wholeSize = mXSize*mYSize; - for (int i = 0; i < wholeSize; i++) - mData[i] -= aMatrix.mData[i]; - return *this; -} - -// operator *= -template -CMatrix& CMatrix::operator*=(const CMatrix& aMatrix) { - if (mXSize != aMatrix.mYSize) - throw EIncompatibleMatrices(mXSize,mYSize,aMatrix.mXSize,aMatrix.mYSize); - T* oldData = mData; - mData = new T[mYSize*aMatrix.mXSize]; - for (int y = 0; y < mYSize; y++) - for (int x = 0; x < aMatrix.mXSize; x++) { - mData[aMatrix.mXSize*y+x] = 0; - for (int i = 0; i < mXSize; i++) - mData[aMatrix.mXSize*y+x] += oldData[mXSize*y+i]*aMatrix(x,i); - } - delete[] oldData; - mXSize = aMatrix.mXSize; - return *this; -} - -template -CMatrix& CMatrix::operator*=(const T aValue) { - int wholeSize = mXSize*mYSize; - for (int i = 0; i < wholeSize; i++) - mData[i] *= aValue; - return *this; -} - -// min -template -T CMatrix::min() const { - T aMin = mData[0]; - int aSize = mXSize*mYSize; - for (int i = 1; i < aSize; i++) - if (mData[i] < aMin) aMin = mData[i]; - return aMin; -} - -// max -template -T CMatrix::max() const { - T aMax = mData[0]; - int aSize = mXSize*mYSize; - for (int i = 1; i < aSize; i++) - if (mData[i] > aMax) aMax = mData[i]; - return aMax; -} - -// avg -template -T CMatrix::avg() const { - T aAvg = 0; - int aSize = mXSize*mYSize; - for (int i = 0; i < aSize; i++) - aAvg += mData[i]; - return aAvg/aSize; -} - -// xSize -template -inline int CMatrix::xSize() const { - return mXSize; -} - -// ySize -template -inline int CMatrix::ySize() const { - return mYSize; -} - -// size -template -inline int CMatrix::size() const { - return mXSize*mYSize; -} - -// getVector -template -void CMatrix::getVector(CVector& aVector, int ay) { - int aOffset = mXSize*ay; - for (int x = 0; x < mXSize; x++) - aVector(x) = mData[x+aOffset]; -} - -// data() -template -inline T* CMatrix::data() const { - return mData; -} - -// N O N - M E M B E R F U N C T I O N S -------------------------------------- - -// abs -template -CMatrix abs(const CMatrix& aMatrix) { - CMatrix result(aMatrix.xSize(),aMatrix.ySize()); - int wholeSize = aMatrix.size(); - for (register int i = 0; i < wholeSize; i++) { - if (aMatrix.data()[i] < 0) result.data()[i] = -aMatrix.data()[i]; - else result.data()[i] = aMatrix.data()[i]; - } - return result; -} - -// trans -template -CMatrix trans(const CMatrix& aMatrix) { - CMatrix result(aMatrix.ySize(),aMatrix.xSize()); - for (int y = 0; y < aMatrix.ySize(); y++) - for (int x = 0; x < aMatrix.xSize(); x++) - result(y,x) = aMatrix(x,y); - return result; -} - -// operator + -template -CMatrix operator+(const CMatrix& aM1, const CMatrix& aM2) { - if ((aM1.xSize() != aM2.xSize()) || (aM1.ySize() != aM2.ySize())) - throw EIncompatibleMatrices(aM1.xSize(),aM1.ySize(),aM2.xSize(),aM2.ySize()); - CMatrix result(aM1.xSize(),aM1.ySize()); - int wholeSize = aM1.xSize()*aM1.ySize(); - for (int i = 0; i < wholeSize; i++) - result.data()[i] = aM1.data()[i] + aM2.data()[i]; - return result; -} - -// operator - -template -CMatrix operator-(const CMatrix& aM1, const CMatrix& aM2) { - if ((aM1.xSize() != aM2.xSize()) || (aM1.ySize() != aM2.ySize())) - throw EIncompatibleMatrices(aM1.xSize(),aM1.ySize(),aM2.xSize(),aM2.ySize()); - CMatrix result(aM1.xSize(),aM1.ySize()); - int wholeSize = aM1.xSize()*aM1.ySize(); - for (int i = 0; i < wholeSize; i++) - result.data()[i] = aM1.data()[i] - aM2.data()[i]; - return result; -} - -// operator * -template -CMatrix operator*(const CMatrix& aM1, const CMatrix& aM2) { - if (aM1.xSize() != aM2.ySize()) - throw EIncompatibleMatrices(aM1.xSize(),aM1.ySize(),aM2.xSize(),aM2.ySize()); - CMatrix result(aM2.xSize(),aM1.ySize(),0); - for (int y = 0; y < result.ySize(); y++) - for (int x = 0; x < result.xSize(); x++) - for (int i = 0; i < aM1.xSize(); i++) - result(x,y) += aM1(i,y)*aM2(x,i); - return result; -} - -template -CVector operator*(const CMatrix& aMatrix, const CVector& aVector) { - if (aMatrix.xSize() != aVector.size()) - throw EIncompatibleMatrices(aMatrix.xSize(),aMatrix.ySize(),1,aVector.size()); - CVector result(aMatrix.ySize(),0); - for (int y = 0; y < aMatrix.ySize(); y++) - for (int x = 0; x < aMatrix.xSize(); x++) - result(y) += aMatrix(x,y)*aVector(x); - return result; -} - -template -CMatrix operator*(const CMatrix& aMatrix, const T aValue) { - CMatrix result(aMatrix.xSize(),aMatrix.ySize()); - int wholeSize = aMatrix.xSize()*aMatrix.ySize(); - for (int i = 0; i < wholeSize; i++) - result.data()[i] = aMatrix.data()[i]*aValue; - return result; -} - -template -inline CMatrix operator*(const T aValue, const CMatrix& aMatrix) { - return aMatrix*aValue; -} - -// operator << -template -std::ostream& operator<<(std::ostream& aStream, const CMatrix& aMatrix) { - for (int y = 0; y < aMatrix.ySize(); y++) { - for (int x = 0; x < aMatrix.xSize(); x++) - aStream << aMatrix(x,y) << ' '; - aStream << std::endl; - } - return aStream; -} - - -// Comparison of two matrices -template bool CMatrix::operator==(const CMatrix& aMatrix) -{ - if((*this).size()!=aMatrix.size()) - return false; - - for(int i=0; i -#include -#include -#include -#include -#include - -inline int int_min(int x, int& y) { return (x -class CTensor { -public: - // standard constructor - inline CTensor(); - // constructor - inline CTensor(const int aXSize, const int aYSize, const int aZSize); - // copy constructor - CTensor(const CTensor& aCopyFrom); - // constructor with implicit filling - CTensor(const int aXSize, const int aYSize, const int aZSize, const T aFillValue); - // destructor - virtual ~CTensor(); - - // Changes the size of the tensor, data will be lost - void setSize(int aXSize, int aYSize, int aZSize); - // Downsamples the tensor - void downsample(int aNewXSize, int aNewYSize); - void downsample(int aNewXSize, int aNewYSize, CMatrix& aConfidence); - void downsample(int aNewXSize, int aNewYSize, CTensor& aConfidence); - // Upsamples the tensor - void upsample(int aNewXSize, int aNewYSize); - void upsampleBilinear(int aNewXSize, int aNewYSize); - // Fills the tensor with the value aValue (see also operator =) - void fill(const T aValue); - // Fills a rectangular area with the value aValue - void fillRect(const CVector& aValue, int ax1, int ay1, int ax2, int ay2); - // Copies a box from the tensor into aResult, the size of aResult will be adjusted - void cut(CTensor& aResult, int x1, int y1, int z1, int x2, int y2, int z2); - // Copies aCopyFrom at a certain position of the tensor - void paste(CTensor& aCopyFrom, int ax, int ay, int az); - // Mirrors the boundaries, aFrom is the distance from the boundaries where the pixels are copied from, - // aTo is the distance from the boundaries they are copied to - void mirrorLayers(int aFrom, int aTo); - // Transforms the values so that they are all between aMin and aMax - // aInitialMin/Max are initializations for seeking the minimum and maximum, change if your - // data is not in this range or the data type T cannot hold these values - void normalizeEach(T aMin, T aMax, T aInitialMin = -30000, T aInitialMax = 30000); - void normalize(T aMin, T aMax, int aChannel, T aInitialMin = -30000, T aInitialMax = 30000); - void normalize(T aMin, T aMax, T aInitialMin = -30000, T aInitialMax = 30000); - // Converts from RGB to CIELab color space and vice-versa - void rgbToCielab(); - void cielabToRGB(); - // Draws a line into the image (only for mZSize = 3) - void drawLine(int dStartX, int dStartY, int dEndX, int dEndY, T aValue1 = 255, T aValue2 = 255, T aValue3 = 255); - void drawRect(int dStartX, int dStartY, int dEndX, int dEndY, T aValue1 = 255, T aValue2 = 255, T aValue3 = 255); - - // Applies a similarity transform (translation, rotation, scaling) to the image - void applySimilarityTransform(CTensor& aWarped, CMatrix& aOutside, float tx, float ty, float cx, float cy, float phi, float scale); - // Applies a homography (linear projective transformation) to the image - void applyHomography(CTensor& aWarped, CMatrix& aOutside, const CMatrix& H); - - // Reads the tensor from a file in Mathematica format - void readFromMathematicaFile(const char* aFilename); - // Writes the tensor to a file in Mathematica format - void writeToMathematicaFile(const char* aFilename); - // Reads the tensor from a movie file in IM format - void readFromIMFile(const char* aFilename); - // Writes the tensor to a movie file in IM format - void writeToIMFile(const char* aFilename); - // Reads an image from a PGM file - void readFromPGM(const char* aFilename); - // Writes the tensor in PGM-Format - void writeToPGM(const char* aFilename); - // Extends a XxYx1 tensor to a XxYx3 tensor with three identical layers - void makeColorTensor(); - // Reads a color image from a PPM file - void readFromPPM(const char* aFilename); - // Writes the tensor in PPM-Format - void writeToPPM(const char* aFilename); - // Reads the tensor from a PDM file - void readFromPDM(const char* aFilename); - // Writes the tensor in PDM-Format - void writeToPDM(const char* aFilename, char aFeatureType); - - // Gives full access to tensor's values - inline T& operator()(const int ax, const int ay, const int az) const; - // Read access with bilinear interpolation - CVector operator()(const float ax, const float ay) const; - // Fills the tensor with the value aValue (equivalent to fill()) - inline CTensor& operator=(const T aValue); - // Copies the tensor aCopyFrom to this tensor (size of tensor might change) - CTensor& operator=(const CTensor& aCopyFrom); - // Adds a tensor of same size - CTensor& operator+=(const CTensor& aMatrix); - // Adds a constant to the tensor - CTensor& operator+=(const T aValue); - // Multiplication with a scalar - CTensor& operator*=(const T aValue); - - // Returns the minimum value - T min() const; - // Returns the maximum value - T max() const; - // Returns the average value - T avg() const; - // Returns the average value of a specific layer - T avg(int az) const; - // Gives access to the tensor's size - inline int xSize() const; - inline int ySize() const; - inline int zSize() const; - inline int size() const; - // Returns the az layer of the tensor as matrix (slow and fast version) - CMatrix getMatrix(const int az) const; - void getMatrix(CMatrix& aMatrix, const int az) const; - // Copies the matrix components of aMatrix into the az layer of the tensor - void putMatrix(CMatrix& aMatrix, const int az); - // Gives access to the internal data representation (use sparingly) - inline T* data() const; - - // Possible interpretations of the third tensor dimension for PDM format - static const char cSpacial = 'S'; - static const char cVector = 'V'; - static const char cColor = 'C'; - static const char cSymmetricMatrix = 'Y'; -protected: - int mXSize,mYSize,mZSize; - T *mData; -}; - -// Provides basic output functionality (only appropriate for very small tensors) -template std::ostream& operator<<(std::ostream& aStream, const CTensor& aTensor); - -// Exceptions thrown by CTensor------------------------------------------------- - -// Thrown when one tries to access an element of a tensor which is out of -// the tensor's bounds -struct ETensorRangeOverflow { - ETensorRangeOverflow(const int ax, const int ay, const int az) { - using namespace std; - cerr << "Exception ETensorRangeOverflow: x = " << ax << ", y = " << ay << ", z = " << az << endl; - } -}; - -// Thrown when the size of a tensor does not match the needed size for a certain operation -struct ETensorIncompatibleSize { - ETensorIncompatibleSize(int ax, int ay, int ax2, int ay2) { - using namespace std; - cerr << "Exception ETensorIncompatibleSize: x = " << ax << ":" << ax2; - cerr << ", y = " << ay << ":" << ay2 << endl; - } - ETensorIncompatibleSize(int ax, int ay, int az) { - std::cerr << "Exception ETensorIncompatibleTensorSize: x = " << ax << ", y = " << ay << ", z= " << az << std::endl; - } -}; - -// I M P L E M E N T A T I O N -------------------------------------------- -// -// You might wonder why there is implementation code in a header file. -// The reason is that not all C++ compilers yet manage separate compilation -// of templates. Inline functions cannot be compiled separately anyway. -// So in this case the whole implementation code is added to the header -// file. -// Users of CTensor should ignore everything that's beyond this line :) -// ------------------------------------------------------------------------ - -// P U B L I C ------------------------------------------------------------ - -// standard constructor -template -inline CTensor::CTensor() { - mData = 0; - mXSize = mYSize = mZSize = 0; -} - -// constructor -template -inline CTensor::CTensor(const int aXSize, const int aYSize, const int aZSize) - : mXSize(aXSize), mYSize(aYSize), mZSize(aZSize) { - mData = new T[aXSize*aYSize*aZSize]; -} - -// copy constructor -template -CTensor::CTensor(const CTensor& aCopyFrom) - : mXSize(aCopyFrom.mXSize), mYSize(aCopyFrom.mYSize), mZSize(aCopyFrom.mZSize) { - int wholeSize = mXSize*mYSize*mZSize; - mData = new T[wholeSize]; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aCopyFrom.mData[i]; -} - -// constructor with implicit filling -template -CTensor::CTensor(const int aXSize, const int aYSize, const int aZSize, const T aFillValue) - : mXSize(aXSize), mYSize(aYSize), mZSize(aZSize) { - mData = new T[aXSize*aYSize*aZSize]; - fill(aFillValue); -} - -// destructor -template -CTensor::~CTensor() { - delete[] mData; -} - -// setSize -template -void CTensor::setSize(int aXSize, int aYSize, int aZSize) { - if (mData != 0) delete[] mData; - mData = new T[aXSize*aYSize*aZSize]; - mXSize = aXSize; - mYSize = aYSize; - mZSize = aZSize; -} - -//downsample -template -void CTensor::downsample(int aNewXSize, int aNewYSize) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize]; - int aSize = aNewXSize*aNewYSize; - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z); - aTemp.downsample(aNewXSize,aNewYSize); - for (int i = 0; i < aSize; i++) - mData2[i+z*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -template -void CTensor::downsample(int aNewXSize, int aNewYSize, CMatrix& aConfidence) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize]; - int aSize = aNewXSize*aNewYSize; - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z); - aTemp.downsample(aNewXSize,aNewYSize,aConfidence); - for (int i = 0; i < aSize; i++) - mData2[i+z*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -template -void CTensor::downsample(int aNewXSize, int aNewYSize, CTensor& aConfidence) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize]; - int aSize = aNewXSize*aNewYSize; - CMatrix aConf(mXSize,mYSize); - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z); - aConfidence.getMatrix(aConf,z); - aTemp.downsample(aNewXSize,aNewYSize,aConf); - for (int i = 0; i < aSize; i++) - mData2[i+z*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -// upsample -template -void CTensor::upsample(int aNewXSize, int aNewYSize) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize]; - int aSize = aNewXSize*aNewYSize; - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z); - aTemp.upsample(aNewXSize,aNewYSize); - for (int i = 0; i < aSize; i++) - mData2[i+z*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -// upsampleBilinear -template -void CTensor::upsampleBilinear(int aNewXSize, int aNewYSize) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize]; - int aSize = aNewXSize*aNewYSize; - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z); - aTemp.upsampleBilinear(aNewXSize,aNewYSize); - for (int i = 0; i < aSize; i++) - mData2[i+z*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -// fill -template -void CTensor::fill(const T aValue) { - int wholeSize = mXSize*mYSize*mZSize; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aValue; -} - -// fillRect -template -void CTensor::fillRect(const CVector& aValue, int ax1, int ay1, int ax2, int ay2) { - for (int z = 0; z < mZSize; z++) { - T val = aValue(z); - for (int y = int_max(0,ay1); y <= int_min(ySize()-1,ay2); y++) - for (register int x = int_max(0,ax1); x <= int_min(xSize()-1,ax2); x++) - operator()(x,y,z) = val; - } -} - -// cut -template -void CTensor::cut(CTensor& aResult, int x1, int y1, int z1, int x2, int y2, int z2) { - aResult.mXSize = x2-x1+1; - aResult.mYSize = y2-y1+1; - aResult.mZSize = z2-z1+1; - delete[] aResult.mData; - aResult.mData = new T[aResult.mXSize*aResult.mYSize*aResult.mZSize]; - for (int z = z1; z <= z2; z++) - for (int y = y1; y <= y2; y++) - for (int x = x1; x <= x2; x++) - aResult(x-x1,y-y1,z-z1) = operator()(x,y,z); -} - -// paste -template -void CTensor::paste(CTensor& aCopyFrom, int ax, int ay, int az) { - for (int z = 0; z < aCopyFrom.zSize(); z++) - for (int y = 0; y < aCopyFrom.ySize(); y++) - for (int x = 0; x < aCopyFrom.xSize(); x++) - operator()(ax+x,ay+y,az+z) = aCopyFrom(x,y,z); -} - -// mirrorLayers -template -void CTensor::mirrorLayers(int aFrom, int aTo) { - for (int z = 0; z < mZSize; z++) { - int aToXIndex = mXSize-aTo-1; - int aToYIndex = mYSize-aTo-1; - int aFromXIndex = mXSize-aFrom-1; - int aFromYIndex = mYSize-aFrom-1; - for (int y = aFrom; y <= aFromYIndex; y++) { - operator()(aTo,y,z) = operator()(aFrom,y,z); - operator()(aToXIndex,y,z) = operator()(aFromXIndex,y,z); - } - for (int x = aTo; x <= aToXIndex; x++) { - operator()(x,aTo,z) = operator()(x,aFrom,z); - operator()(x,aToYIndex,z) = operator()(x,aFromYIndex,z); - } - } -} - -// normalize -template -void CTensor::normalizeEach(T aMin, T aMax, T aInitialMin, T aInitialMax) { - for (int k = 0; k < mZSize; k++) - normalize(aMin,aMax,k,aInitialMin,aInitialMax); -} - -template -void CTensor::normalize(T aMin, T aMax, int aChannel, T aInitialMin, T aInitialMax) { - int aChannelSize = mXSize*mYSize; - T aCurrentMin = aInitialMax; - T aCurrentMax = aInitialMin; - int aIndex = aChannelSize*aChannel; - for (int i = 0; i < aChannelSize; i++) { - if (mData[aIndex] > aCurrentMax) aCurrentMax = mData[aIndex]; - else if (mData[aIndex] < aCurrentMin) aCurrentMin = mData[aIndex]; - aIndex++; - } - T aTemp1 = aCurrentMin - aMin; - T aTemp2 = (aCurrentMax-aCurrentMin); - if (aTemp2 == 0) aTemp2 = 1; - else aTemp2 = (aMax-aMin)/aTemp2; - aIndex = aChannelSize*aChannel; - for (int i = 0; i < aChannelSize; i++) { - mData[aIndex] -= aTemp1; - mData[aIndex] *= aTemp2; - aIndex++; - } -} - -// drawLine -template -void CTensor::drawLine(int dStartX, int dStartY, int dEndX, int dEndY, T aValue1, T aValue2, T aValue3) { - int aOffset1 = mXSize*mYSize; - int aOffset2 = 2*aOffset1; - // vertical line - if (dStartX == dEndX) { - if (dStartX < 0 || dStartX >= mXSize) return; - int x = dStartX; - if (dStartY < dEndY) { - for (int y = dStartY; y <= dEndY; y++) - if (y >= 0 && y < mYSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - else { - for (int y = dStartY; y >= dEndY; y--) - if (y >= 0 && y < mYSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - return; - } - // horizontal line - if (dStartY == dEndY) { - if (dStartY < 0 || dStartY >= mYSize) return; - int y = dStartY; - if (dStartX < dEndX) { - for (int x = dStartX; x <= dEndX; x++) - if (x >= 0 && x < mXSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - else { - for (int x = dStartX; x >= dEndX; x--) - if (x >= 0 && x < mXSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - return; - } - float m = float(dStartY - dEndY) / float(dStartX - dEndX); - float invm = 1.0/m; - if (fabs(m) > 1.0) { - if (dEndY > dStartY) { - for (int y = dStartY; y <= dEndY; y++) { - int x = (int)(0.5+dStartX+(y-dStartY)*invm); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - } - else { - for (int y = dStartY; y >= dEndY; y--) { - int x = (int)(0.5+dStartX+(y-dStartY)*invm); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - } - } - else { - if (dEndX > dStartX) { - for (int x = dStartX; x <= dEndX; x++) { - int y = (int)(0.5+dStartY+(x-dStartX)*m); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - } - else { - for (int x = dStartX; x >= dEndX; x--) { - int y = (int)(0.5+dStartY+(x-dStartX)*m); - if (x >= 0 && x < mXSize && y >= 0 && y < mYSize) { - mData[x+y*mXSize] = aValue1; - mData[x+y*mXSize+aOffset1] = aValue2; - mData[x+y*mXSize+aOffset2] = aValue3; - } - } - } - } -} - -// drawRect -template -void CTensor::drawRect(int dStartX, int dStartY, int dEndX, int dEndY, T aValue1, T aValue2, T aValue3) { - drawLine(dStartX,dStartY,dEndX,dStartY,aValue1,aValue2,aValue3); - drawLine(dStartX,dEndY,dEndX,dEndY,aValue1,aValue2,aValue3); - drawLine(dStartX,dStartY,dStartX,dEndY,aValue1,aValue2,aValue3); - drawLine(dEndX,dStartY,dEndX,dEndY,aValue1,aValue2,aValue3); -} - -template -void CTensor::normalize(T aMin, T aMax, T aInitialMin, T aInitialMax) { - int aSize = mXSize*mYSize*mZSize; - T aCurrentMin = aInitialMax; - T aCurrentMax = aInitialMin; - for (int i = 0; i < aSize; i++) { - if (mData[i] > aCurrentMax) aCurrentMax = mData[i]; - else if (mData[i] < aCurrentMin) aCurrentMin = mData[i]; - } - T aTemp1 = aCurrentMin - aMin; - T aTemp2 = (aCurrentMax-aCurrentMin); - if (aTemp2 == 0) aTemp2 = 1; - else aTemp2 = (aMax-aMin)/aTemp2; - for (int i = 0; i < aSize; i++) { - mData[i] -= aTemp1; - mData[i] *= aTemp2; - } -} - -template -void CTensor::rgbToCielab() { - for (int y = 0; y < mYSize; y++) - for (int x = 0; x < mXSize; x++) { - float R = operator()(x,y,0)*0.003921569; - float G = operator()(x,y,1)*0.003921569; - float B = operator()(x,y,2)*0.003921569; - if (R>0.0031308) R = pow((R + 0.055)*0.9478673, 2.4); else R *= 0.077399381; - if (G>0.0031308) G = pow((G + 0.055)*0.9478673, 2.4); else G *= 0.077399381; - if (B>0.0031308) B = pow((B + 0.055)*0.9478673, 2.4); else B *= 0.077399381; - //Observer. = 2?, Illuminant = D65 - float X = R * 0.4124 + G * 0.3576 + B * 0.1805; - float Y = R * 0.2126 + G * 0.7152 + B * 0.0722; - float Z = R * 0.0193 + G * 0.1192 + B * 0.9505; - X *= 1.052111; - Z *= 0.918417; - if (X > 0.008856) X = pow(X,0.33333333333); else X = 7.787*X + 0.137931034; - if (Y > 0.008856) Y = pow(Y,0.33333333333); else Y = 7.787*Y + 0.137931034; - if (Z > 0.008856) Z = pow(Z,0.33333333333); else Z = 7.787*Z + 0.137931034; - operator()(x,y,0) = 1000.0*((295.8*Y) - 40.8)/255.0; - operator()(x,y,1) = 128.0+637.5*(X-Y); - operator()(x,y,2) = 128.0+255.0*(Y-Z); - } -} - -template -void CTensor::cielabToRGB() { - for (int y = 0; y < mYSize; y++) - for (int x = 0; x < mXSize; x++) { - float L = operator()(x,y,0)*0.255; - float A = operator()(x,y,1); - float B = operator()(x,y,2); - float Y = (L+40.8)*0.00338066; - float X = (A-128.0+637.5*Y)*0.0015686; - float Z = (128.0+255.0*Y-B)*0.00392157; - float temp = Y*Y*Y; - if (temp > 0.008856) Y = temp; - else Y = (Y-0.137931034)*0.12842; - temp = X*X*X; - if (temp > 0.008856) X = temp; - else X = (X-0.137931034)*0.12842; - temp = Z*Z*Z; - if (temp > 0.008856) Z = temp; - else Z = (Z-0.137931034)*0.12842; - X *= 0.95047; - Y *= 1.0; - Z *= 1.08883; - float r = 3.2406*X-1.5372*Y-0.4986*Z; - float g = -0.9689*X+1.8758*Y+0.0415*Z; - float b = 0.0557*X-0.204*Y+1.057*Z; - if (r < 0) r = 0; - temp = 1.055*pow(r,0.41667)-0.055; - if (temp > 0.0031308) r = temp; - else r *= 12.92; - if (g < 0) g = 0; - temp = 1.055*pow(g,0.41667)-0.055; - if (temp > 0.0031308) g = temp; - else g *= 12.92; - if (b < 0) b = 0; - temp = 1.055*pow(b,0.41667)-0.055; - if (temp > 0.0031308) b = temp; - else b *= 12.92; - operator()(x,y,0) = 255.0*r; - operator()(x,y,1) = 255.0*g; - operator()(x,y,2) = 255.0*b; - } -} - -// applySimilarityTransform -template -void CTensor::applySimilarityTransform(CTensor& aWarped, CMatrix& aOutside, float tx, float ty, float cx, float cy, float phi, float scale) { - float cosphi = scale*cos(phi); - float sinphi = scale*sin(phi); - int aSize = mXSize*mYSize; - int aWarpedSize = aWarped.xSize()*aWarped.ySize(); - float ctx = cx+tx-cx*cosphi+cy*sinphi; - float cty = cy+ty-cy*cosphi-cx*sinphi; - aOutside = false; - int i = 0; - for (int y = 0; y < aWarped.ySize(); y++) - for (int x = 0; x < aWarped.xSize(); x++,i++) { - float xf = x; float yf = y; - float ax = xf*cosphi-yf*sinphi+ctx; - float ay = yf*cosphi+xf*sinphi+cty; - int x1 = (int)ax; int y1 = (int)ay; - float alphaX = ax-x1; float alphaY = ay-y1; - float betaX = 1.0-alphaX; float betaY = 1.0-alphaY; - if (x1 < 0 || y1 < 0 || x1+1 >= mXSize || y1+1 >= mYSize) aOutside.data()[i] = true; - else { - int j = y1*mXSize+x1; - for (int k = 0; k < mZSize; k++) { - float a = betaX*mData[j] +alphaX*mData[j+1]; - float b = betaX*mData[j+mXSize]+alphaX*mData[j+1+mXSize]; - aWarped.data()[i+k*aWarpedSize] = betaY*a+alphaY*b; - j += aSize; - } - } - } -} - -// applyHomography -template -void CTensor::applyHomography(CTensor& aWarped, CMatrix& aOutside, const CMatrix& H) { - int aSize = mXSize*mYSize; - int aWarpedSize = aWarped.xSize()*aWarped.ySize(); - aOutside = false; - int i = 0; - for (int y = 0; y < aWarped.ySize(); y++) - for (int x = 0; x < aWarped.xSize(); x++,i++) { - float xf = x; float yf = y; - float ax = H.data()[0]*xf+H.data()[1]*yf+H.data()[2]; - float ay = H.data()[3]*xf+H.data()[4]*yf+H.data()[5]; - float az = H.data()[6]*xf+H.data()[7]*yf+H.data()[8]; - float invaz = 1.0/az; - ax *= invaz; ay *= invaz; - int x1 = (int)ax; int y1 = (int)ay; - float alphaX = ax-x1; float alphaY = ay-y1; - float betaX = 1.0-alphaX; float betaY = 1.0-alphaY; - if (x1 < 0 || y1 < 0 || x1+1 >= mXSize || y1+1 >= mYSize) aOutside.data()[i] = true; - else { - int j = y1*mXSize+x1; - for (int k = 0; k < mZSize; k++) { - float a = betaX*mData[j] +alphaX*mData[j+1]; - float b = betaX*mData[j+mXSize]+alphaX*mData[j+1+mXSize]; - aWarped.data()[i+k*aWarpedSize] = betaY*a+alphaY*b; - j += aSize; - } - } - } -} - -// ----------------------------------------------------------------------------- -// File I/O -// ----------------------------------------------------------------------------- - -// readFromMathematicaFile -template -void CTensor::readFromMathematicaFile(const char* aFilename) { - using namespace std; - // Read the whole file and store data in aData - // Ignore blanks, tabs and lines - // Also ignore Mathematica comments (* ... *) - ifstream aStream(aFilename); - string aData; - char aChar; - bool aBracketFound = false; - bool aStarFound = false; - bool aCommentFound = false; - while (aStream.get(aChar)) - if (aChar != ' ' && aChar != '\t' && aChar != '\n') { - if (aCommentFound) { - if (!aStarFound && aChar == '*') aStarFound = true; - else { - if (aStarFound && aChar == ')') aCommentFound = false; - aStarFound = false; - } - } - else { - if (!aBracketFound && aChar == '(') aBracketFound = true; - else { - if (aBracketFound && aChar == '*') aCommentFound = true; - else aData += aChar; - aBracketFound = false; - } - } - } - // Count the number of braces and double braces to figure out z- and y-Size of tensor - int aDoubleBraceCount = 0; - int aBraceCount = 0; - int aPos = 0; - while ((aPos = aData.find_first_of('{',aPos)+1) > 0) { - aBraceCount++; - if (aData[aPos] == '{' && aData[aPos+1] != '{') aDoubleBraceCount++; - } - // Count the number of commas in the first section to figure out xSize of tensor - int aCommaCount = 0; - aPos = 0; - while (aData[aPos] != '}') { - if (aData[aPos] == ',') aCommaCount++; - aPos++; - } - // Adapt size of tensor - if (mData != 0) delete[] mData; - mXSize = aCommaCount+1; - mYSize = (aBraceCount-1-aDoubleBraceCount) / aDoubleBraceCount; - mZSize = aDoubleBraceCount; - mData = new T[mXSize*mYSize*mZSize]; - // Analyse file --------------- - aPos = 0; - if (aData[aPos] != '{') throw EInvalidFileFormat("Mathematica"); - aPos++; - for (int z = 0; z < mZSize; z++) { - if (aData[aPos] != '{') throw EInvalidFileFormat("Mathematica"); - aPos++; - for (int y = 0; y < mYSize; y++) { - if (aData[aPos] != '{') throw EInvalidFileFormat("Mathematica"); - aPos++; - for (int x = 0; x < mXSize; x++) { - int oldPos = aPos; - if (x+1 < mXSize) aPos = aData.find_first_of(',',aPos); - else aPos = aData.find_first_of('}',aPos); - #ifdef GNU_COMPILER - string s = aData.substr(oldPos,aPos-oldPos); - istrstream is(s.c_str()); - #else - string s = aData.substr(oldPos,aPos-oldPos); - istringstream is(s); - #endif - T aItem; - is >> aItem; - operator()(x,y,z) = aItem; - aPos++; - } - if (y+1 < mYSize) { - if (aData[aPos] != ',') throw EInvalidFileFormat("Mathematica"); - aPos++; - while (aData[aPos] != '{') - aPos++; - } - } - aPos++; - if (z+1 < mZSize) { - if (aData[aPos] != ',') throw EInvalidFileFormat("Mathematica"); - aPos++; - while (aData[aPos] != '{') - aPos++; - } - } -} - -// writeToMathematicaFile -template -void CTensor::writeToMathematicaFile(const char* aFilename) { - using namespace std; - ofstream aStream(aFilename); - aStream << '{'; - for (int z = 0; z < mZSize; z++) { - aStream << '{'; - for (int y = 0; y < mYSize; y++) { - aStream << '{'; - for (int x = 0; x < mXSize; x++) { - aStream << operator()(x,y,z); - if (x+1 < mXSize) aStream << ','; - } - aStream << '}'; - if (y+1 < mYSize) aStream << ",\n"; - } - aStream << '}'; - if (z+1 < mZSize) aStream << ",\n"; - } - aStream << '}'; -} - -// readFromIMFile -template -void CTensor::readFromIMFile(const char* aFilename) { - FILE *aStream; - aStream = fopen(aFilename,"rb"); - // Read image data - for (int i = 0; i < mXSize*mYSize*mZSize; i++) - mData[i] = getc(aStream); - fclose(aStream); -} - -// writeToIMFile -template -void CTensor::writeToIMFile(const char *aFilename) { - FILE *aStream; - aStream = fopen(aFilename,"wb"); - // write data - for (int i = 0; i < mXSize*mYSize*mZSize; i++) { - char dummy = (char)mData[i]; - fwrite(&dummy,1,1,aStream); - } - fclose(aStream); -} - -// readFromPGM -template -void CTensor::readFromPGM(const char* aFilename) { - FILE *aStream; - aStream = fopen(aFilename,"rb"); - if (aStream == 0) std::cerr << "File not found: " << aFilename << std::endl; - int dummy; - // Find beginning of file (P5) - while (getc(aStream) != 'P'); - if (getc(aStream) != '5') throw EInvalidFileFormat("PGM"); - do - dummy = getc(aStream); - while (dummy != '\n' && dummy != ' '); - // Remove comments and empty lines - dummy = getc(aStream); - while (dummy == '#') { - while (getc(aStream) != '\n'); - dummy = getc(aStream); - } - while (dummy == '\n') - dummy = getc(aStream); - // Read image size - mXSize = dummy-48; - while ((dummy = getc(aStream)) >= 48 && dummy < 58) - mXSize = 10*mXSize+dummy-48; - while ((dummy = getc(aStream)) < 48 || dummy >= 58); - mYSize = dummy-48; - while ((dummy = getc(aStream)) >= 48 && dummy < 58) - mYSize = 10*mYSize+dummy-48; - mZSize = 1; - while (dummy != '\n' && dummy != ' ') - dummy = getc(aStream); - while (dummy != '\n' && dummy != ' ') - dummy = getc(aStream); - // Adjust size of data structure - delete[] mData; - mData = new T[mXSize*mYSize]; - // Read image data - for (int i = 0; i < mXSize*mYSize; i++) - mData[i] = getc(aStream); - fclose(aStream); -} - -// writeToPGM -template -void CTensor::writeToPGM(const char* aFilename) { - int rows = (int)floor(sqrt(mZSize)); - int cols = (int)ceil(mZSize*1.0/rows); - FILE* outimage = fopen(aFilename, "wb"); - fprintf(outimage, "P5 \n"); - fprintf(outimage, "%ld %ld \n255\n", cols*mXSize,rows*mYSize); - for (int r = 0; r < rows; r++) - for (int y = 0; y < mYSize; y++) - for (int c = 0; c < cols; c++) - for (int x = 0; x < mXSize; x++) { - unsigned char aHelp; - if (r*cols+c >= mZSize) aHelp = 0; - else aHelp = (unsigned char)operator()(x,y,r*cols+c); - fwrite (&aHelp, sizeof(unsigned char), 1, outimage); - } - fclose(outimage); -} - -// makeColorTensor -template -void CTensor::makeColorTensor() { - if (mZSize != 1) return; - int aSize = mXSize*mYSize; - int a2Size = 2*aSize; - T* aNewData = new T[aSize*3]; - for (int i = 0; i < aSize; i++) - aNewData[i] = aNewData[i+aSize] = aNewData[i+a2Size] = mData[i]; - mZSize = 3; - delete[] mData; - mData = aNewData; -} - -// readFromPPM -template -void CTensor::readFromPPM(const char* aFilename) { - FILE *aStream; - aStream = fopen(aFilename,"rb"); - if (aStream == 0) - std::cerr << "File not found: " << aFilename << std::endl; - int dummy; - // Find beginning of file (P6) - while (getc(aStream) != 'P'); - dummy = getc(aStream); - if (dummy == '5') mZSize = 1; - else if (dummy == '6') mZSize = 3; - else throw EInvalidFileFormat("PPM"); - do dummy = getc(aStream); while (dummy != '\n' && dummy != ' '); - // Remove comments and empty lines - dummy = getc(aStream); - while (dummy == '#') { - while (getc(aStream) != '\n'); - dummy = getc(aStream); - } - while (dummy == '\n') - dummy = getc(aStream); - // Read image size - mXSize = dummy-48; - while ((dummy = getc(aStream)) >= 48 && dummy < 58) - mXSize = 10*mXSize+dummy-48; - while ((dummy = getc(aStream)) < 48 || dummy >= 58); - mYSize = dummy-48; - while ((dummy = getc(aStream)) >= 48 && dummy < 58) - mYSize = 10*mYSize+dummy-48; - while (dummy != '\n' && dummy != ' ') - dummy = getc(aStream); - while (dummy < 48 || dummy >= 58) dummy = getc(aStream); - while ((dummy = getc(aStream)) >= 48 && dummy < 58); - if (dummy != '\n') while (getc(aStream) != '\n'); - // Adjust size of data structure - delete[] mData; - mData = new T[mXSize*mYSize*mZSize]; - // Read image data - int aSize = mXSize*mYSize; - if (mZSize == 1) - for (int i = 0; i < aSize; i++) - mData[i] = getc(aStream); - else { - int aSizeTwice = aSize+aSize; - for (int i = 0; i < aSize; i++) { - mData[i] = getc(aStream); - mData[i+aSize] = getc(aStream); - mData[i+aSizeTwice] = getc(aStream); - } - } - fclose(aStream); -} - -// writeToPPM -template -void CTensor::writeToPPM(const char* aFilename) { - FILE* outimage = fopen(aFilename, "wb"); - fprintf(outimage, "P6 \n"); - fprintf(outimage, "%d %d \n255\n", mXSize,mYSize); - for (int y = 0; y < mYSize; y++) - for (int x = 0; x < mXSize; x++) { - unsigned char aHelp = (unsigned char)operator()(x,y,0); - fwrite (&aHelp, sizeof(unsigned char), 1, outimage); - aHelp = (unsigned char)operator()(x,y,1); - fwrite (&aHelp, sizeof(unsigned char), 1, outimage); - aHelp = (unsigned char)operator()(x,y,2); - fwrite (&aHelp, sizeof(unsigned char), 1, outimage); - } - fclose(outimage); -} - -// readFromPDM -template -void CTensor::readFromPDM(const char* aFilename) { - std::ifstream aStream(aFilename); - std::string s; - // Read header - aStream >> s; - if (s != "P9") throw EInvalidFileFormat("PDM"); - char aFeatureType; - aStream >> aFeatureType; - aStream >> s; - aStream >> mXSize; - aStream >> mYSize; - aStream >> mZSize; - aStream >> s; - // Adjust size of data structure - delete[] mData; - mData = new T[mXSize*mYSize*mZSize]; - // Read data - for (int i = 0; i < mXSize*mYSize*mZSize; i++) - aStream >> mData[i]; -} - -// writeToPDM -template -void CTensor::writeToPDM(const char* aFilename, char aFeatureType) { - std::ofstream aStream(aFilename); - // write header - aStream << "P9" << std::endl; - aStream << aFeatureType << "SS" << std::endl; - aStream << mZSize << ' ' << mYSize << ' ' << mXSize << std::endl; - aStream << "F" << std::endl; - // write data - for (int i = 0; i < mXSize*mYSize*mZSize; i++) { - aStream << mData[i]; - if (i % 8 == 0) aStream << std::endl; - else aStream << ' '; - } -} - -// operator () -template -inline T& CTensor::operator()(const int ax, const int ay, const int az) const { - #ifdef _DEBUG - if (ax >= mXSize || ay >= mYSize || az >= mZSize || ax < 0 || ay < 0 || az < 0) - throw ETensorRangeOverflow(ax,ay,az); - #endif - return mData[mXSize*(mYSize*az+ay)+ax]; -} - -template -CVector CTensor::operator()(const float ax, const float ay) const { - CVector aResult(mZSize); - int x1 = (int)ax; - int y1 = (int)ay; - int x2 = x1+1; - int y2 = y1+1; - #ifdef _DEBUG - if (x2 >= mXSize || y2 >= mYSize || x1 < 0 || y1 < 0) throw ETensorRangeOverflow(ax,ay,0); - #endif - float alphaX = ax-x1; float alphaXTrans = 1.0-alphaX; - float alphaY = ay-y1; float alphaYTrans = 1.0-alphaY; - for (int k = 0; k < mZSize; k++) { - float a = alphaXTrans*operator()(x1,y1,k)+alphaX*operator()(x2,y1,k); - float b = alphaXTrans*operator()(x1,y2,k)+alphaX*operator()(x2,y2,k); - aResult(k) = alphaYTrans*a+alphaY*b; - } - return aResult; -} - -// operator = -template -inline CTensor& CTensor::operator=(const T aValue) { - fill(aValue); - return *this; -} - -template -CTensor& CTensor::operator=(const CTensor& aCopyFrom) { - if (this != &aCopyFrom) { - delete[] mData; - if (aCopyFrom.mData == 0) { - mData = 0; mXSize = 0; mYSize = 0; mZSize = 0; - } - else { - mXSize = aCopyFrom.mXSize; - mYSize = aCopyFrom.mYSize; - mZSize = aCopyFrom.mZSize; - int wholeSize = mXSize*mYSize*mZSize; - mData = new T[wholeSize]; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aCopyFrom.mData[i]; - } - } - return *this; -} - -// operator += -template -CTensor& CTensor::operator+=(const CTensor& aTensor) { - #ifdef _DEBUG - if (mXSize != aTensor.mXSize || mYSize != aTensor.mYSize || mZSize != aTensor.mZSize) - throw ETensorIncompatibleSize(mXSize,mYSize,mZSize); - #endif - int wholeSize = size(); - for (int i = 0; i < wholeSize; i++) - mData[i] += aTensor.mData[i]; - return *this; -} - -// operator += -template -CTensor& CTensor::operator+=(const T aValue) { - int wholeSize = mXSize*mYSize*mZSize; - for (int i = 0; i < wholeSize; i++) - mData[i] += aValue; - return *this; -} - -// operator *= -template -CTensor& CTensor::operator*=(const T aValue) { - int wholeSize = mXSize*mYSize*mZSize; - for (int i = 0; i < wholeSize; i++) - mData[i] *= aValue; - return *this; -} - -// min -template -T CTensor::min() const { - T aMin = mData[0]; - int aSize = mXSize*mYSize*mZSize; - for (int i = 1; i < aSize; i++) - if (mData[i] < aMin) aMin = mData[i]; - return aMin; -} - -// max -template -T CTensor::max() const { - T aMax = mData[0]; - int aSize = mXSize*mYSize*mZSize; - for (int i = 1; i < aSize; i++) - if (mData[i] > aMax) aMax = mData[i]; - return aMax; -} - -// avg -template -T CTensor::avg() const { - T aAvg = 0; - for (int z = 0; z < mZSize; z++) - aAvg += avg(z); - return aAvg/mZSize; -} - -template -T CTensor::avg(int az) const { - T aAvg = 0; - int aSize = mXSize*mYSize; - int aTemp = (az+1)*aSize; - for (int i = az*aSize; i < aTemp; i++) - aAvg += mData[i]; - return aAvg/aSize; -} - -// xSize -template -inline int CTensor::xSize() const { - return mXSize; -} - -// ySize -template -inline int CTensor::ySize() const { - return mYSize; -} - -// zSize -template -inline int CTensor::zSize() const { - return mZSize; -} - -// size -template -inline int CTensor::size() const { - return mXSize*mYSize*mZSize; -} - -// getMatrix -template -CMatrix CTensor::getMatrix(const int az) const { - CMatrix aTemp(mXSize,mYSize); - int aMatrixSize = mXSize*mYSize; - int aOffset = az*aMatrixSize; - for (int i = 0; i < aMatrixSize; i++) - aTemp.data()[i] = mData[i+aOffset]; - return aTemp; -} - -// getMatrix -template -void CTensor::getMatrix(CMatrix& aMatrix, const int az) const { - if (aMatrix.xSize() != mXSize || aMatrix.ySize() != mYSize) - throw ETensorIncompatibleSize(aMatrix.xSize(),aMatrix.ySize(),mXSize,mYSize); - int aMatrixSize = mXSize*mYSize; - int aOffset = az*aMatrixSize; - for (int i = 0; i < aMatrixSize; i++) - aMatrix.data()[i] = mData[i+aOffset]; -} - -// putMatrix -template -void CTensor::putMatrix(CMatrix& aMatrix, const int az) { - if (aMatrix.xSize() != mXSize || aMatrix.ySize() != mYSize) - throw ETensorIncompatibleSize(aMatrix.xSize(),aMatrix.ySize(),mXSize,mYSize); - int aMatrixSize = mXSize*mYSize; - int aOffset = az*aMatrixSize; - for (int i = 0; i < aMatrixSize; i++) - mData[i+aOffset] = aMatrix.data()[i]; -} - -// data() -template -inline T* CTensor::data() const { - return mData; -} - -// N O N - M E M B E R F U N C T I O N S -------------------------------------- - -// operator << -template -std::ostream& operator<<(std::ostream& aStream, const CTensor& aTensor) { - for (int z = 0; z < aTensor.zSize(); z++) { - for (int y = 0; y < aTensor.ySize(); y++) { - for (int x = 0; x < aTensor.xSize(); x++) - aStream << aTensor(x,y,z) << ' '; - aStream << std::endl; - } - aStream << std::endl; - } - return aStream; -} - -#endif diff --git a/video_input/consistencyChecker/CTensor4D.h b/video_input/consistencyChecker/CTensor4D.h deleted file mode 100644 index 6feeb5d..0000000 --- a/video_input/consistencyChecker/CTensor4D.h +++ /dev/null @@ -1,656 +0,0 @@ -// CTensor4D -// A four-dimensional array -// -// Author: Thomas Brox -// Last change: 05.11.2001 -//------------------------------------------------------------------------- -// Note: -// There is a difference between the GNU Compiler's STL and the standard -// concerning the definition and usage of string streams as well as substrings. -// Thus if using a GNU Compiler you should write #define GNU_COMPILER at the -// beginning of your program. -// -// Another Note: -// Linker problems occured in connection with from the STL. -// In this case you should include this file in a namespace. -// Example: -// namespace NTensor4D { -// #include -// } -// After including other packages you can then write: -// using namespace NTensor4D; - -#ifndef CTENSOR4D_H -#define CTENSOR4D_H - -#include -#include -#include -#ifdef GNU_COMPILER - #include -#else - #include -#endif -#include "CTensor.h" - -template -class CTensor4D { -public: - // constructor - inline CTensor4D(); - inline CTensor4D(const int aXSize, const int aYSize, const int aZSize, const int aASize); - // copy constructor - CTensor4D(const CTensor4D& aCopyFrom); - // constructor with implicit filling - CTensor4D(const int aXSize, const int aYSize, const int aZSize, const int aASize, const T aFillValue); - // destructor - virtual ~CTensor4D(); - - // Changes the size of the tensor, data will be lost - void setSize(int aXSize, int aYSize, int aZSize, int aASize); - // Downsamples the tensor - void downsample(int aNewXSize, int aNewYSize); - void downsample(int aNewXSize, int aNewYSize, int aNewZSize); - // Upsamples the tensor - void upsample(int aNewXSize, int aNewYSize); - void upsampleBilinear(int aNewXSize, int aNewYSize); - void upsampleTrilinear(int aNewXSize, int aNewYSize, int aNewZSize); - // Fills the tensor with the value aValue (see also operator =) - void fill(const T aValue); - // Copies a box from the tensor into aResult, the size of aResult will be adjusted - void cut(CTensor4D& aResult, int x1, int y1, int z1, int a1, int x2, int y2, int z2, int a2); - // Reads data from a list of PPM or PGM files given in a text file - void readFromFile(char* aFilename); - // Writes a set of colour images to a large PPM image - void writeToPPM(const char* aFilename, int aCols = 0, int aRows = 0); - - // Gives full access to tensor's values - inline T& operator()(const int ax, const int ay, const int az, const int aa) const; - // Read access with bilinear interpolation - CVector operator()(const float ax, const float ay, const int aa) const; - // Fills the tensor with the value aValue (equivalent to fill()) - inline CTensor4D& operator=(const T aValue); - // Copies the tensor aCopyFrom to this tensor (size of tensor might change) - CTensor4D& operator=(const CTensor4D& aCopyFrom); - // Multiplication with a scalar - CTensor4D& operator*=(const T aValue); - // Component-wise addition - CTensor4D& operator+=(const CTensor4D& aTensor); - - // Gives access to the tensor's size - inline int xSize() const; - inline int ySize() const; - inline int zSize() const; - inline int aSize() const; - inline int size() const; - // Returns the aath layer of the 4D-tensor as 3D-tensor - CTensor getTensor3D(const int aa) const; - // Removes one dimension and returns the resulting 3D-tensor - void getTensor3D(CTensor& aTensor, int aIndex, int aDim = 3) const; - // Copies the components of a 3D-tensor in the aDimth layer of the 4D-tensor - void putTensor3D(CTensor& aTensor, int aIndex, int aDim = 3); - // Removes two dimensions and returns the resulting matrix - void getMatrix(CMatrix& aMatrix, int aZIndex, int aAIndex) const; - // Copies the components of a 3D-tensor in the aDimth layer of the 4D-tensor - void putMatrix(CMatrix& aMatrix, int aZIndex, int aAIndex); - // Gives access to the internal data representation (use sparingly) - inline T* data() const; -protected: - int mXSize,mYSize,mZSize,mASize; - T *mData; -}; - -// Provides basic output functionality (only appropriate for very small tensors) -template std::ostream& operator<<(std::ostream& aStream, const CTensor4D& aTensor); - -// Exceptions thrown by CTensor------------------------------------------------- - -// Thrown when one tries to access an element of a tensor which is out of -// the tensor's bounds -struct ETensor4DRangeOverflow { - ETensor4DRangeOverflow(const int ax, const int ay, const int az, const int aa) { - using namespace std; - cerr << "Exception ETensor4DRangeOverflow: x = " << ax << ", y = " << ay << ", z = " << az << ", a = " << aa << endl; - } -}; - -// Thrown from getTensor3D if the parameter's size does not match with the size -// of this tensor -struct ETensor4DIncompatibleSize { - ETensor4DIncompatibleSize(int ax, int ay, int az, int ax2, int ay2, int az2) { - using namespace std; - cerr << "Exception ETensor4DIncompatibleSize: x = " << ax << ":" << ax2; - cerr << ", y = " << ay << ":" << ay2; - cerr << ", z = " << az << ":" << az2 << endl; - } -}; - -// Thrown from readFromFile if the file format is unknown -struct ETensor4DInvalidFileFormat { - ETensor4DInvalidFileFormat() { - using namespace std; - cerr << "Exception ETensor4DInvalidFileFormat" << endl; - } -}; - -// I M P L E M E N T A T I O N -------------------------------------------- -// -// You might wonder why there is implementation code in a header file. -// The reason is that not all C++ compilers yet manage separate compilation -// of templates. Inline functions cannot be compiled separately anyway. -// So in this case the whole implementation code is added to the header -// file. -// Users of CTensor4D should ignore everything that's beyond this line :) -// ------------------------------------------------------------------------ - -// P U B L I C ------------------------------------------------------------ - -// constructor -template -inline CTensor4D::CTensor4D() { - mData = 0; mXSize = 0; mYSize = 0; mZSize = 0; mASize = 0; -} - -// constructor -template -inline CTensor4D::CTensor4D(const int aXSize, const int aYSize, const int aZSize, const int aASize) - : mXSize(aXSize), mYSize(aYSize), mZSize(aZSize), mASize(aASize) { - mData = new T[aXSize*aYSize*aZSize*aASize]; -} - -// copy constructor -template -CTensor4D::CTensor4D(const CTensor4D& aCopyFrom) - : mXSize(aCopyFrom.mXSize), mYSize(aCopyFrom.mYSize), mZSize(aCopyFrom.mZSize), mASize(aCopyFrom.mASize) { - int wholeSize = mXSize*mYSize*mZSize*mASize; - mData = new T[wholeSize]; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aCopyFrom.mData[i]; -} - -// constructor with implicit filling -template -CTensor4D::CTensor4D(const int aXSize, const int aYSize, const int aZSize, const int aASize, const T aFillValue) - : mXSize(aXSize), mYSize(aYSize), mZSize(aZSize), mASize(aASize) { - mData = new T[aXSize*aYSize*aZSize*aASize]; - fill(aFillValue); -} - -// destructor -template -CTensor4D::~CTensor4D() { - delete[] mData; -} - -// setSize -template -void CTensor4D::setSize(int aXSize, int aYSize, int aZSize, int aASize) { - if (mData != 0) delete[] mData; - mData = new T[aXSize*aYSize*aZSize*aASize]; - mXSize = aXSize; - mYSize = aYSize; - mZSize = aZSize; - mASize = aASize; -} - -//downsample -template -void CTensor4D::downsample(int aNewXSize, int aNewYSize) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize*mASize]; - int aSize = aNewXSize*aNewYSize; - for (int a = 0; a < mASize; a++) - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z,a); - aTemp.downsample(aNewXSize,aNewYSize); - for (int i = 0; i < aSize; i++) - mData2[i+(a*mZSize+z)*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -template -void CTensor4D::downsample(int aNewXSize, int aNewYSize, int aNewZSize) { - T* mData2 = new T[aNewXSize*aNewYSize*aNewZSize*mASize]; - int aSize = aNewXSize*aNewYSize*aNewZSize; - for (int a = 0; a < mASize; a++) { - CTensor aTemp(mXSize,mYSize,mZSize); - getTensor3D(aTemp,a); - aTemp.downsample(aNewXSize,aNewYSize,aNewZSize); - for (int i = 0; i < aSize; i++) - mData2[i+a*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; - mZSize = aNewZSize; -} - -// upsample -template -void CTensor4D::upsample(int aNewXSize, int aNewYSize) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize*mASize]; - int aSize = aNewXSize*aNewYSize; - for (int a = 0; a < mASize; a++) - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z,a); - aTemp.upsample(aNewXSize,aNewYSize); - for (int i = 0; i < aSize; i++) - mData2[i+(a*mZSize+z)*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -// upsampleBilinear -template -void CTensor4D::upsampleBilinear(int aNewXSize, int aNewYSize) { - T* mData2 = new T[aNewXSize*aNewYSize*mZSize*mASize]; - int aSize = aNewXSize*aNewYSize; - for (int a = 0; a < mASize; a++) - for (int z = 0; z < mZSize; z++) { - CMatrix aTemp(mXSize,mYSize); - getMatrix(aTemp,z,a); - aTemp.upsampleBilinear(aNewXSize,aNewYSize); - for (int i = 0; i < aSize; i++) - mData2[i+(a*mZSize+z)*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; -} - -// upsampleTrilinear -template -void CTensor4D::upsampleTrilinear(int aNewXSize, int aNewYSize, int aNewZSize) { - T* mData2 = new T[aNewXSize*aNewYSize*aNewZSize*mASize]; - int aSize = aNewXSize*aNewYSize*aNewZSize; - for (int a = 0; a < mASize; a++) { - CTensor aTemp(mXSize,mYSize,mZSize); - getTensor3D(aTemp,a); - aTemp.upsampleTrilinear(aNewXSize,aNewYSize,aNewZSize); - for (int i = 0; i < aSize; i++) - mData2[i+a*aSize] = aTemp.data()[i]; - } - delete[] mData; - mData = mData2; - mXSize = aNewXSize; - mYSize = aNewYSize; - mZSize = aNewZSize; -} - -// fill -template -void CTensor4D::fill(const T aValue) { - int wholeSize = mXSize*mYSize*mZSize*mASize; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aValue; -} - -// cut -template -void CTensor4D::cut(CTensor4D& aResult, int x1, int y1, int z1, int a1, int x2, int y2, int z2, int a2) { - aResult.mXSize = x2-x1+1; - aResult.mYSize = y2-y1+1; - aResult.mZSize = z2-z1+1; - aResult.mASize = a2-a1+1; - delete[] aResult.mData; - aResult.mData = new T[aResult.mXSize*aResult.mYSize*aResult.mZSize*aResult.mASize]; - for (int a = a1; a <= a2; a++) - for (int z = z1; z <= z2; z++) - for (int y = y1; y <= y2; y++) - for (int x = x1; x <= x2; x++) - aResult(x-x1,y-y1,z-z1,a-a1) = operator()(x,y,z,a); -} - -// readFromFile -template -void CTensor4D::readFromFile(char* aFilename) { - if (mData != 0) delete[] mData; - std::string s; - std::string aPath = aFilename; - aPath.erase(aPath.find_last_of('\\')+1,100); - mASize = 0; - { - std::ifstream aStream(aFilename); - while (!aStream.eof()) { - aStream >> s; - if (s != "") { - mASize++; - if (mASize == 1) { - s.erase(0,s.find_last_of('.')); - if (s == ".ppm" || s == ".PPM") mZSize = 3; - else if (s == ".pgm" || s == ".PGM") mZSize = 1; - else throw ETensor4DInvalidFileFormat(); - } - } - } - } - std::ifstream aStream(aFilename); - aStream >> s; - s = aPath+s; - // PGM - if (mZSize == 1) { - CMatrix aTemp; - aTemp.readFromPGM(s.c_str()); - mXSize = aTemp.xSize(); - mYSize = aTemp.ySize(); - int aSize = mXSize*mYSize; - mData = new T[aSize*mASize]; - for (int i = 0; i < aSize; i++) - mData[i] = aTemp.data()[i]; - for (int a = 1; a < mASize; a++) { - aStream >> s; - s = aPath+s; - aTemp.readFromPGM(s.c_str()); - for (int i = 0; i < aSize; i++) - mData[i+a*aSize] = aTemp.data()[i]; - } - } - // PPM - else { - CTensor aTemp; - aTemp.readFromPPM(s.c_str()); - mXSize = aTemp.xSize(); - mYSize = aTemp.ySize(); - int aSize = 3*mXSize*mYSize; - mData = new T[aSize*mASize]; - for (int i = 0; i < aSize; i++) - mData[i] = aTemp.data()[i]; - for (int a = 1; a < mASize; a++) { - aStream >> s; - s = aPath+s; - aTemp.readFromPPM(s.c_str()); - for (int i = 0; i < aSize; i++) - mData[i+a*aSize] = aTemp.data()[i]; - } - } -} - -// writeToPPM -template -void CTensor4D::writeToPPM(const char* aFilename, int aCols, int aRows) { - int rows = (int)floor(sqrt(mASize)); - if (aRows != 0) rows = aRows; - int cols = (int)ceil(mASize*1.0/rows); - if (aCols != 0) cols = aCols; - FILE* outimage = fopen(aFilename, "wb"); - fprintf(outimage, "P6 \n"); - fprintf(outimage, "%ld %ld \n255\n", cols*mXSize,rows*mYSize); - for (int r = 0; r < rows; r++) - for (int y = 0; y < mYSize; y++) - for (int c = 0; c < cols; c++) - for (int x = 0; x < mXSize; x++) { - unsigned char aHelp; - if (r*cols+c >= mASize) aHelp = 0; - else aHelp = (unsigned char)operator()(x,y,0,r*cols+c); - fwrite (&aHelp, sizeof(unsigned char), 1, outimage); - if (r*cols+c >= mASize) aHelp = 0; - else aHelp = (unsigned char)operator()(x,y,1,r*cols+c); - fwrite (&aHelp, sizeof(unsigned char), 1, outimage); - if (r*cols+c >= mASize) aHelp = 0; - else aHelp = (unsigned char)operator()(x,y,2,r*cols+c); - fwrite (&aHelp, sizeof(unsigned char), 1, outimage); - } - fclose(outimage); -} - -// operator () -template -inline T& CTensor4D::operator()(const int ax, const int ay, const int az, const int aa) const { - #ifdef DEBUG - if (ax >= mXSize || ay >= mYSize || az >= mZSize || aa >= mASize || ax < 0 || ay < 0 || az < 0 || aa < 0) - throw ETensorRangeOverflow(ax,ay,az,aa); - #endif - return mData[mXSize*(mYSize*(mZSize*aa+az)+ay)+ax]; -} - -template -CVector CTensor4D::operator()(const float ax, const float ay, const int aa) const { - CVector aResult(mZSize); - int x1 = (int)ax; - int y1 = (int)ay; - int x2 = x1+1; - int y2 = y1+1; - #ifdef _DEBUG - if (x2 >= mXSize || y2 >= mYSize || x1 < 0 || y1 < 0) throw ETensorRangeOverflow(ax,ay,0); - #endif - float alphaX = ax-x1; float alphaXTrans = 1.0-alphaX; - float alphaY = ay-y1; float alphaYTrans = 1.0-alphaY; - for (int k = 0; k < mZSize; k++) { - float a = alphaXTrans*operator()(x1,y1,k,aa)+alphaX*operator()(x2,y1,k,aa); - float b = alphaXTrans*operator()(x1,y2,k,aa)+alphaX*operator()(x2,y2,k,aa); - aResult(k) = alphaYTrans*a+alphaY*b; - } - return aResult; -} - -// operator = -template -inline CTensor4D& CTensor4D::operator=(const T aValue) { - fill(aValue); - return *this; -} - -template -CTensor4D& CTensor4D::operator=(const CTensor4D& aCopyFrom) { - if (this != &aCopyFrom) { - if (mData != 0) delete[] mData; - mXSize = aCopyFrom.mXSize; - mYSize = aCopyFrom.mYSize; - mZSize = aCopyFrom.mZSize; - mASize = aCopyFrom.mASize; - int wholeSize = mXSize*mYSize*mZSize*mASize; - mData = new T[wholeSize]; - for (register int i = 0; i < wholeSize; i++) - mData[i] = aCopyFrom.mData[i]; - } - return *this; -} - -// operator *= -template -CTensor4D& CTensor4D::operator*=(const T aValue) { - int wholeSize = mXSize*mYSize*mZSize*mASize; - for (int i = 0; i < wholeSize; i++) - mData[i] *= aValue; - return *this; -} - -// operator += -template -CTensor4D& CTensor4D::operator+=(const CTensor4D& aTensor) { - #ifdef _DEBUG - if (mXSize != aTensor.mXSize || mYSize != aTensor.mYSize || mZSize != aTensor.mZSize || mASize != aTensor.mASize) - throw ETensorIncompatibleSize(mXSize,mYSize,mZSize); - #endif - int wholeSize = size(); - for (int i = 0; i < wholeSize; i++) - mData[i] += aTensor.mData[i]; - return *this; -} - -// xSize -template -inline int CTensor4D::xSize() const { - - return mXSize; -} - -// ySize -template -inline int CTensor4D::ySize() const { - return mYSize; -} - -// zSize -template -inline int CTensor4D::zSize() const { - return mZSize; -} - -// aSize -template -inline int CTensor4D::aSize() const { - return mASize; -} - -// size -template -inline int CTensor4D::size() const { - return mXSize*mYSize*mZSize*mASize; -} - -// getTensor3D -template -CTensor CTensor4D::getTensor3D(const int aa) const { - CTensor aTemp(mXSize,mYSize,mZSize); - int aTensorSize = mXSize*mYSize*mZSize; - int aOffset = aa*aTensorSize; - for (int i = 0; i < aTensorSize; i++) - aTemp.data()[i] = mData[i+aOffset]; - return aTemp; -} - -// getTensor3D -template -void CTensor4D::getTensor3D(CTensor& aTensor, int aIndex, int aDim) const { - int aSize; - int aOffset; - switch (aDim) { - case 3: - if (aTensor.xSize() != mXSize || aTensor.ySize() != mYSize || aTensor.zSize() != mZSize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mXSize,mYSize,mZSize); - aSize = mXSize*mYSize*mZSize; - aOffset = aIndex*aSize; - for (int i = 0; i < aSize; i++) - aTensor.data()[i] = mData[i+aOffset]; - break; - case 2: - if (aTensor.xSize() != mXSize || aTensor.ySize() != mYSize || aTensor.zSize() != mASize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mXSize,mYSize,mASize); - aSize = mXSize*mYSize; - aOffset = aIndex*aSize; - for (int a = 0; a < mASize; a++) - for (int i = 0; i < aSize; i++) - aTensor.data()[i+a*aSize] = mData[i+aOffset+a*aSize*mZSize]; - break; - case 1: - if (aTensor.xSize() != mXSize || aTensor.ySize() != mZSize || aTensor.zSize() != mASize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mXSize,mZSize,mASize); - for (int a = 0; a < mASize; a++) - for (int z = 0; z < mZSize; z++) - for (int x = 0; x < mXSize; x++) - aTensor(x,z,a) = operator()(x,aIndex,z,a); - break; - case 0: - if (aTensor.xSize() != mYSize || aTensor.ySize() != mZSize || aTensor.zSize() != mASize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mYSize,mZSize,mASize); - for (int a = 0; a < mASize; a++) - for (int z = 0; z < mZSize; z++) - for (int y = 0; y < mYSize; y++) - aTensor(y,z,a) = operator()(aIndex,y,z,a); - break; - default: getTensor3D(aTensor,aIndex); - } -} - -// putTensor3D -template -void CTensor4D::putTensor3D(CTensor& aTensor, int aIndex, int aDim) { - int aSize; - int aOffset; - switch (aDim) { - case 3: - if (aTensor.xSize() != mXSize || aTensor.ySize() != mYSize || aTensor.zSize() != mZSize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mXSize,mYSize,mZSize); - aSize = mXSize*mYSize*mZSize; - aOffset = aIndex*aSize; - for (int i = 0; i < aSize; i++) - mData[i+aOffset] = aTensor.data()[i]; - break; - case 2: - if (aTensor.xSize() != mXSize || aTensor.ySize() != mYSize || aTensor.zSize() != mASize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mXSize,mYSize,mASize); - aSize = mXSize*mYSize; - aOffset = aIndex*aSize; - for (int a = 0; a < mASize; a++) - for (int i = 0; i < aSize; i++) - mData[i+aOffset+a*aSize*mZSize] = aTensor.data()[i+a*aSize]; - break; - case 1: - if (aTensor.xSize() != mXSize || aTensor.ySize() != mZSize || aTensor.zSize() != mASize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mXSize,mZSize,mASize); - for (int a = 0; a < mASize; a++) - for (int z = 0; z < mZSize; z++) - for (int x = 0; x < mXSize; x++) - operator()(x,aIndex,z,a) = aTensor(x,z,a); - break; - case 0: - if (aTensor.xSize() != mYSize || aTensor.ySize() != mZSize || aTensor.zSize() != mASize) - throw ETensor4DIncompatibleSize(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),mYSize,mZSize,mASize); - for (int a = 0; a < mASize; a++) - for (int z = 0; z < mZSize; z++) - for (int y = 0; y < mYSize; y++) - operator()(aIndex,y,z,a) = aTensor(y,z,a); - break; - default: putTensor3D(aTensor,aIndex); - } -} - -// getMatrix -template -void CTensor4D::getMatrix(CMatrix& aMatrix, int aZIndex, int aAIndex) const { - if (aMatrix.xSize() != mXSize || aMatrix.ySize() != mYSize) - throw ETensor4DIncompatibleSize(aMatrix.xSize(),aMatrix.ySize(),1,mXSize,mYSize,1); - int aSize = mXSize*mYSize; - int aOffset = aSize*(aAIndex*mZSize+aZIndex); - for (int i = 0; i < aSize; i++) - aMatrix.data()[i] = mData[i+aOffset]; -} - -// putMatrix -template -void CTensor4D::putMatrix(CMatrix& aMatrix, int aZIndex, int aAIndex) { - if (aMatrix.xSize() != mXSize || aMatrix.ySize() != mYSize) - throw ETensor4DIncompatibleSize(aMatrix.xSize(),aMatrix.ySize(),1,mXSize,mYSize,1); - int aSize = mXSize*mYSize; - int aOffset = aSize*(aAIndex*mZSize+aZIndex); - for (int i = 0; i < aSize; i++) - mData[i+aOffset] = aMatrix.data()[i]; -} - -// data() -template -inline T* CTensor4D::data() const { - return mData; -} - -// N O N - M E M B E R F U N C T I O N S -------------------------------------- - -// operator << -template -std::ostream& operator<<(std::ostream& aStream, const CTensor4D& aTensor) { - for (int a = 0; a < aTensor.aSize(); a++) { - for (int z = 0; z < aTensor.zSize(); z++) { - for (int y = 0; y < aTensor.ySize(); y++) { - for (int x = 0; x < aTensor.xSize(); x++) - aStream << aTensor(x,y,z) << ' '; - aStream << std::endl; - } - aStream << std::endl; - } - aStream << std::endl; - } - return aStream; -} - -#endif diff --git a/video_input/consistencyChecker/CVector.h b/video_input/consistencyChecker/CVector.h deleted file mode 100644 index aacb0fa..0000000 --- a/video_input/consistencyChecker/CVector.h +++ /dev/null @@ -1,519 +0,0 @@ -// CVector -// A one-dimensional array including basic vector operations -// -// Author: Thomas Brox -// Last change: 23.05.2005 -//------------------------------------------------------------------------- -#ifndef CVECTOR_H -#define CVECTOR_H - -#include -#include - -template class CMatrix; -template class CTensor; - -template -class CVector { -public: - // constructor - inline CVector(); - // constructor - inline CVector(const int aSize); - // copy constructor - CVector(const CVector& aCopyFrom); - // constructor (from array) - CVector(const T* aPointer, const int aSize); - // constructor with implicit filling - CVector(const int aSize, const T aFillValue); - // destructor - virtual ~CVector(); - - // Changes the size of the vector (data is lost) - void setSize(int aSize); - // Fills the vector with the specified value (see also operator=) - void fill(const T aValue); - // Appends the values of another vector - void append(CVector& aVector); - // Normalizes the length of the vector to 1 - void normalize(); - // Normalizes the component sum to 1 - void normalizeSum(); - // Reads values from a text file - void readFromTXT(const char* aFilename); - // Writes values to a text file - void writeToTXT(char* aFilename); - // Returns the sum of all values - T sum(); - // Returns the minimum value - T min(); - // Returns the maximum value - T max(); - // Returns the Euclidean norm - T norm(); - - // Converts vector to homogeneous coordinates, i.e., all components are divided by last component - CVector& homogen(); - // Remove the last component - inline void homogen_nD(); - // Computes the cross product between this vector and aVector - void cross(CVector& aVector); - - // Gives full access to the vector's values - inline T& operator()(const int aIndex) const; - inline T& operator[](const int aIndex) const; - // Fills the vector with the specified value (equivalent to fill) - inline CVector& operator=(const T aValue); - // Copies a vector into this vector (size might change) - CVector& operator=(const CVector& aCopyFrom); - // Copies values from a matrix to the vector (size might change) - CVector& operator=(const CMatrix& aCopyFrom); - // Copies values from a tensor to the vector (size might change) - CVector& operator=(const CTensor& aCopyFrom); - // Adds another vector - CVector& operator+=(const CVector& aVector); - // Substracts another vector - CVector& operator-=(const CVector& aVector); - // Multiplies the vector with a scalar - CVector& operator*=(const T aValue); - // Scalar product - T operator*=(const CVector& aVector); - // Checks (non-)equivalence to another vector - bool operator==(const CVector& aVector); - inline bool operator!=(const CVector& aVector); - - // Gives access to the vector's size - inline int size() const; - // Gives access to the internal data representation - inline T* data() const {return mData;} -protected: - int mSize; - T* mData; -}; - -// Adds two vectors -template CVector operator+(const CVector& vec1, const CVector& vec2); -// Substracts two vectors -template CVector operator-(const CVector& vec1, const CVector& vec2); -// Multiplies vector with a scalar -template CVector operator*(const CVector& aVector, const T aValue); -template CVector operator*(const T aValue, const CVector& aVector); -// Computes the scalar product of two vectors -template T operator*(const CVector& vec1, const CVector& vec2); -// Computes cross product of two vectors -template CVector operator/(const CVector& vec1, const CVector& vec2); -// Sends the vector to an output stream -template std::ostream& operator<<(std::ostream& aStream, const CVector& aVector); - -// Exceptions thrown by CVector-------------------------------------------- - -// Thrown if one tries to access an element of a vector which is out of -// the vector's bounds -struct EVectorRangeOverflow { - EVectorRangeOverflow(const int aIndex) { - using namespace std; - cerr << "Exception EVectorRangeOverflow: Index = " << aIndex << endl; - } -}; - -struct EVectorIncompatibleSize { - EVectorIncompatibleSize(int aSize1, int aSize2) { - using namespace std; - cerr << "Exception EVectorIncompatibleSize: " << aSize1 << " <> " << aSize2 << endl; - } -}; - - -// I M P L E M E N T A T I O N -------------------------------------------- -// -// You might wonder why there is implementation code in a header file. -// The reason is that not all C++ compilers yet manage separate compilation -// of templates. Inline functions cannot be compiled separately anyway. -// So in this case the whole implementation code is added to the header -// file. -// Users of CVector should ignore everything that's beyond this line. -// ------------------------------------------------------------------------ - -// P U B L I C ------------------------------------------------------------ -// constructor -template -inline CVector::CVector() : mSize(0) { - mData = new T[0]; -} - -// constructor -template -inline CVector::CVector(const int aSize) - : mSize(aSize) { - mData = new T[aSize]; -} - -// copy constructor -template -CVector::CVector(const CVector& aCopyFrom) - : mSize(aCopyFrom.mSize) { - mData = new T[mSize]; - for (int i = 0; i < mSize; i++) - mData[i] = aCopyFrom.mData[i]; -} - -// constructor (from array) -template -CVector::CVector(const T* aPointer, const int aSize) - : mSize(aSize) { - mData = new T[mSize]; - for (int i = 0; i < mSize; i++) - mData[i] = aPointer[i]; -} - -// constructor with implicit filling -template -CVector::CVector(const int aSize, const T aFillValue) - : mSize(aSize) { - mData = new T[aSize]; - fill(aFillValue); -} - -// destructor -template -CVector::~CVector() { - delete[] mData; -} - -// setSize -template -void CVector::setSize(int aSize) { - if (mData != 0) delete[] mData; - mData = new T[aSize]; - mSize = aSize; -} - -// fill -template -void CVector::fill(const T aValue) { - for (register int i = 0; i < mSize; i++) - mData[i] = aValue; -} - -// append -template -void CVector::append(CVector& aVector) { - T* aNewData = new T[mSize+aVector.size()]; - for (int i = 0; i < mSize; i++) - aNewData[i] = mData[i]; - for (int i = 0; i < aVector.size(); i++) - aNewData[i+mSize] = aVector(i); - mSize += aVector.size(); - delete[] mData; - mData = aNewData; -} - -// normalize -template -void CVector::normalize() { - T aSum = 0; - for (register int i = 0; i < mSize; i++) - aSum += mData[i]*mData[i]; - if (aSum == 0) return; - aSum = 1.0/sqrt(aSum); - for (register int i = 0; i < mSize; i++) - mData[i] *= aSum; -} - -// normalizeSum -template -void CVector::normalizeSum() { - T aSum = 0; - for (register int i = 0; i < mSize; i++) - aSum += mData[i]; - if (aSum == 0) return; - aSum = 1.0/aSum; - for (register int i = 0; i < mSize; i++) - mData[i] *= aSum; -} - -// readFromTXT -template -void CVector::readFromTXT(const char* aFilename) { - std::ifstream aStream(aFilename); - mSize = 0; - float aDummy; - while (!aStream.eof()) { - aStream >> aDummy; - mSize++; - } - aStream.close(); - std::ifstream aStream2(aFilename); - delete mData; - mData = new T[mSize]; - for (int i = 0; i < mSize; i++) - aStream2 >> mData[i]; -} - -// writeToTXT -template -void CVector::writeToTXT(char* aFilename) { - std::ofstream aStream(aFilename); - for (int i = 0; i < mSize; i++) - aStream << mData[i] << std::endl; -} - -// sum -template -T CVector::sum() { - T val = mData[0]; - for (int i = 1; i < mSize; i++) - val += mData[i]; - return val; -} - -// min -template -T CVector::min() { - T bestValue = mData[0]; - for (int i = 1; i < mSize; i++) - if (mData[i] < bestValue) bestValue = mData[i]; - return bestValue; -} - -// max -template -T CVector::max() { - T bestValue = mData[0]; - for (int i = 1; i < mSize; i++) - if (mData[i] > bestValue) bestValue = mData[i]; - return bestValue; -} - -// norm -template -T CVector::norm() { - T aSum = 0.0; - for (int i = 0; i < mSize; i++) - aSum += mData[i]*mData[i]; - return sqrt(aSum); -} - -// homogen -template -CVector& CVector::homogen() { - if (mSize > 1 && mData[mSize-1] != 0) { - T invVal = 1.0/mData[mSize-1]; - for (int i = 0; i < mSize; i++) - mData[i] *= invVal; - } - return (*this); -} - -// homogen_nD -template -inline void CVector::homogen_nD() { - mSize--; -} - -// cross -template -void CVector::cross(CVector& aVector) { - T aHelp0 = aVector(2)*mData[1] - aVector(1)*mData[2]; - T aHelp1 = aVector(0)*mData[2] - aVector(2)*mData[0]; - T aHelp2 = aVector(1)*mData[0] - aVector(0)*mData[1]; - mData[0] = aHelp0; - mData[1] = aHelp1; - mData[2] = aHelp2; -} - -// operator() -template -inline T& CVector::operator()(const int aIndex) const { - #ifdef _DEBUG - if (aIndex >= mSize || aIndex < 0) - throw EVectorRangeOverflow(aIndex); - #endif - return mData[aIndex]; -} - -// operator[] -template -inline T& CVector::operator[](const int aIndex) const { - return operator()(aIndex); -} - -// operator= -template -inline CVector& CVector::operator=(const T aValue) { - fill(aValue); - return *this; -} - -template -CVector& CVector::operator=(const CVector& aCopyFrom) { - if (this != &aCopyFrom) { - if (mSize != aCopyFrom.size()) { - delete[] mData; - mSize = aCopyFrom.size(); - mData = new T[mSize]; - } - for (register int i = 0; i < mSize; i++) - mData[i] = aCopyFrom.mData[i]; - } - return *this; -} - -template -CVector& CVector::operator=(const CMatrix& aCopyFrom) { - if (mSize != aCopyFrom.size()) { - delete[] mData; - mSize = aCopyFrom.size(); - mData = new T[mSize]; - } - for (register int i = 0; i < mSize; i++) - mData[i] = aCopyFrom.data()[i]; - return *this; -} - -template -CVector& CVector::operator=(const CTensor& aCopyFrom) { - if (mSize != aCopyFrom.size()) { - delete[] mData; - mSize = aCopyFrom.size(); - mData = new T[mSize]; - } - for (register int i = 0; i < mSize; i++) - mData[i] = aCopyFrom.data()[i]; - return *this; -} - -// operator += -template -CVector& CVector::operator+=(const CVector& aVector) { - #ifdef _DEBUG - if (mSize != aVector.size()) throw EVectorIncompatibleSize(mSize,aVector.size()); - #endif - for (int i = 0; i < mSize; i++) - mData[i] += aVector(i); - return *this; -} - -// operator -= -template -CVector& CVector::operator-=(const CVector& aVector) { - #ifdef _DEBUG - if (mSize != aVector.size()) throw EVectorIncompatibleSize(mSize,aVector.size()); - #endif - for (int i = 0; i < mSize; i++) - mData[i] -= aVector(i); - return *this; -} - -// operator *= -template -CVector& CVector::operator*=(const T aValue) { - for (int i = 0; i < mSize; i++) - mData[i] *= aValue; - return *this; -} - -template -T CVector::operator*=(const CVector& aVector) { - #ifdef _DEBUG - if (mSize != aVector.size()) throw EVectorIncompatibleSize(mSize,aVector.size()); - #endif - T aSum = 0.0; - for (int i = 0; i < mSize; i++) - aSum += mData[i]*aVector(i); - return aSum; -} - -// operator == -template -bool CVector::operator==(const CVector& aVector) { - if (mSize != aVector.size()) return false; - int i = 0; - while (i < mSize && aVector(i) == mData[i]) - i++; - return (i == mSize); -} - -// operator != -template -inline bool CVector::operator!=(const CVector& aVector) { - return !((*this)==aVector); -} - -// size -template -inline int CVector::size() const { - return mSize; -} - -// N O N - M E M B E R F U N C T I O N S ------------------------------------- - -// operator + -template -CVector operator+(const CVector& vec1, const CVector& vec2) { - #ifdef _DEBUG - if (vec1.size() != vec2.size()) throw EVectorIncompatibleSize(vec1.size(),vec2.size()); - #endif - CVector result(vec1.size()); - for (int i = 0; i < vec1.size(); i++) - result(i) = vec1[i]+vec2[i]; - return result; -} - -// operator - -template -CVector operator-(const CVector& vec1, const CVector& vec2) { - #ifdef _DEBUG - if (vec1.size() != vec2.size()) throw EVectorIncompatibleSize(vec1.size(),vec2.size()); - #endif - CVector result(vec1.size()); - for (int i = 0; i < vec1.size(); i++) - result(i) = vec1(i)-vec2(i); - return result; -} - -// operator * -template -CVector operator*(const T aValue, const CVector& aVector) { - CVector result(aVector.size()); - for (int i = 0; i < aVector.size(); i++) - result(i) = aValue*aVector(i); - return result; -} - -template -CVector operator*(const CVector& aVector, const T aValue) { - return operator*(aValue,aVector); -} - -template -T operator*(const CVector& vec1, const CVector& vec2) { - #ifdef _DEBUG - if (vec1.size() != vec2.size()) throw EVectorIncompatibleSize(vec1.size(),vec2.size()); - #endif - T aSum = 0.0; - for (int i = 0; i < vec1.size(); i++) - aSum += vec1(i)*vec2(i); - return aSum; -} - -// operator / -template -CVector operator/(const CVector& vec1, const CVector& vec2) { - CVector result(3); - result[0]=vec1[1]*vec2[2] - vec1[2]*vec2[1]; - result[1]=vec1[2]*vec2[0] - vec1[0]*vec2[2]; - result[2]=vec1[0]*vec2[1] - vec1[1]*vec2[0]; - return result; -} - -// operator << -template -std::ostream& operator<<(std::ostream& aStream, const CVector& aVector) { - for (int i = 0; i < aVector.size(); i++) - aStream << aVector(i) << '|'; - aStream << std::endl; - return aStream; -} - -#endif diff --git a/video_input/consistencyChecker/Makefile b/video_input/consistencyChecker/Makefile deleted file mode 100644 index 94725e2..0000000 --- a/video_input/consistencyChecker/Makefile +++ /dev/null @@ -1,3 +0,0 @@ -default: - g++ -O3 -fPIC consistencyChecker.cpp NMath.cpp -I. -o consistencyChecker -L. - diff --git a/video_input/consistencyChecker/NMath.cpp b/video_input/consistencyChecker/NMath.cpp deleted file mode 100644 index 3a58b16..0000000 --- a/video_input/consistencyChecker/NMath.cpp +++ /dev/null @@ -1,664 +0,0 @@ -// Copyright: Thomas Brox - -#include -#include -#include - -namespace NMath { - - const float Pi = 3.1415926536; - - // faculty - int faculty(int n) { - int aResult = 1; - for (int i = 2; i <= n; i++) - aResult *= i; - return aResult; - } - - // binCoeff - int binCoeff(const int n, const int k) { - if (k > (n >> 1)) return binCoeff(n,n-k); - int aResult = 1; - for (int i = n; i > (n-k); i--) - aResult *= i; - for (int j = 2; j <= k; j++) - aResult /= j; - return aResult; - } - - // tangent - float tangent(const float x1, const float y1, const float x2, const float y2) { - float alpha; - float xDiff = x2-x1; - float yDiff = y2-y1; - if (yDiff > 0) { - if (xDiff == 0) alpha = 0.5*Pi; - else if (xDiff > 0) alpha = atan(yDiff/xDiff); - else alpha = Pi+atan(yDiff/xDiff); - } - else { - if (xDiff == 0) alpha = -0.5*Pi; - else if (xDiff > 0) alpha = atan(yDiff/xDiff); - else alpha = -Pi+atan(yDiff/xDiff); - } - return alpha; - } - - // absAngleDifference - float absAngleDifference(const float aFirstAngle, const float aSecondAngle) { - float aAlphaDiff = abs(aFirstAngle - aSecondAngle); - if (aAlphaDiff > Pi) aAlphaDiff = 2*Pi-aAlphaDiff; - return aAlphaDiff; - } - - // angleDifference - float angleDifference(const float aFirstAngle, const float aSecondAngle) { - float aAlphaDiff = aFirstAngle - aSecondAngle; - if (aAlphaDiff > Pi) aAlphaDiff = -2*Pi+aAlphaDiff; - else if (aAlphaDiff < -Pi) aAlphaDiff = 2*Pi+aAlphaDiff; - return aAlphaDiff; - } - - // angleSum - float angleSum(const float aFirstAngle, const float aSecondAngle) { - float aSum = aFirstAngle + aSecondAngle; - if (aSum > Pi) aSum = -2*Pi+aSum; - else if (aSum < -Pi) aSum = 2*Pi+aSum; - return aSum; - } - - // round - int round(const float aValue) { - float temp1 = floor(aValue); - float temp2 = ceil(aValue); - if (aValue-temp1 < 0.5) return (int)temp1; - else return (int)temp2; - } - - // PATransformation - // Cyclic Jacobi method for determining the eigenvalues and eigenvectors - // of a symmetric matrix. - // Ref.: H.R. Schwarz: Numerische Mathematik. Teubner, Stuttgart, 1988. - // pp. 243-246. - void PATransformation(const CMatrix& aMatrix, CVector& aEigenvalues, CMatrix& aEigenvectors, bool aOrdering) { - static const float eps = 0.0001; - static const float delta = 0.000001; - static const float eps2 = eps*eps; - float sum,theta,t,c,r,s,g,h; - // Initialization - CMatrix aCopy(aMatrix); - int n = aEigenvalues.size(); - aEigenvectors = 0; - for (int i = 0; i < n; i++) - aEigenvectors(i,i) = 1; - // Loop - do { - // check whether accuracy is reached - sum = 0.0; - for (int i = 1; i < n; i++) - for (int j = 0; j <= i-1; j++) - sum += aCopy(i,j)*aCopy(i,j); - if (sum+sum > eps2) { - for (int p = 0; p < n-1; p++) - for (int q = p+1; q < n; q++) - if (fabs(aCopy(q,p)) >= eps2) { - theta = (aCopy(q,q) - aCopy(p,p)) / (2.0 * aCopy(q,p)); - t = 1.0; - if (fabs(theta) > delta) t = 1.0 / (theta + theta/fabs(theta) * sqrt (theta*theta + 1.0)); - c = 1.0 / sqrt (1.0 + t*t); - s = c*t; - r = s / (1.0 + c); - aCopy(p,p) -= t * aCopy(q,p); - aCopy(q,q) += t * aCopy(q,p); - aCopy(q,p) = 0; - for (int j = 0; j <= p-1; j++) { - g = aCopy(q,j) + r * aCopy(p,j); - h = aCopy(p,j) - r * aCopy(q,j); - aCopy(p,j) -= s*g; - aCopy(q,j) += s*h; - } - for (int i = p+1; i <= q-1; i++) { - g = aCopy(q,i) + r * aCopy(i,p); - h = aCopy(i,p) - r * aCopy(q,i); - aCopy(i,p) -= s * g; - aCopy(q,i) += s * h; - } - for (int i = q+1; i < n; i++) { - g = aCopy(i,q) + r * aCopy(i,p); - h = aCopy(i,p) - r * aCopy(i,q); - aCopy(i,p) -= s * g; - aCopy(i,q) += s * h; - } - for (int i = 0; i < n; i++) { - g = aEigenvectors(i,q) + r * aEigenvectors(i,p); - h = aEigenvectors(i,p) - r * aEigenvectors(i,q); - aEigenvectors(i,p) -= s * g; - aEigenvectors(i,q) += s * h; - } - } - } - } - // Return eigenvalues - while (sum+sum > eps2); - for (int i = 0; i < n; i++) - aEigenvalues(i) = aCopy(i,i); - if (aOrdering) { - // Order eigenvalues and eigenvectors - for (int i = 0; i < n-1; i++) { - int k = i; - for (int j = i+1; j < n; j++) - if (fabs(aEigenvalues(j)) > fabs(aEigenvalues(k))) k = j; - if (k != i) { - // Switch eigenvalue i and k - float help = aEigenvalues(k); - aEigenvalues(k) = aEigenvalues(i); - aEigenvalues(i) = help; - // Switch eigenvector i and k - for (int j = 0; j < n; j++) { - help = aEigenvectors(j,k); - aEigenvectors(j,k) = aEigenvectors(j,i); - aEigenvectors(j,i) = help; - } - } - } - } - } - - // PABackTransformation - void PABacktransformation(const CMatrix& aEigenvectors, const CVector& aEigenvalues, CMatrix& aMatrix) { - for (int i = 0; i < aEigenvalues.size(); i++) - for (int j = 0; j <= i; j++) { - float sum = aEigenvalues(0) * aEigenvectors(i,0) * aEigenvectors(j,0); - for (int k = 1; k < aEigenvalues.size(); k++) - sum += aEigenvalues(k) * aEigenvectors(i,k) * aEigenvectors(j,k); - aMatrix(i,j) = sum; - } - for (int i = 0; i < aEigenvalues.size(); i++) - for (int j = i+1; j < aEigenvalues.size(); j++) - aMatrix(i,j) = aMatrix(j,i); - } - - // svd (nach Numerical Recipes in C basierend auf Forsythe et al.: Computer Methods for - // Mathematical Computations (Englewood Cliffs, NJ: Prentice-Hall), Chapter 9, 1977, - // Code übernommen von Bodo Rosenhahn) - void svd(CMatrix& U, CMatrix& S, CMatrix& V, bool aOrdering, int aIterations) { - static float at,bt,ct; - static float maxarg1,maxarg2; - #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0)) - #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2)) - #define MIN(a,b) ((a) >(b) ? (b) : (a)) - #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) - int flag,i,its,j,jj,k,l,nm; - float c,f,h,s,x,y,z; - float anorm=0.0,g=0.0,scale=0.0; - int aXSize = U.xSize(); - int aYSize = U.ySize(); - CVector aBuffer(aXSize); - for (i = 0; i < aXSize; i++) { - l=i+1; - aBuffer(i)=scale*g; - g=s=scale=0.0; - if (i < aYSize) { - for (k = i; k < aYSize; k++) - scale += fabs(U(i,k)); - if (scale) { - for (k = i; k < aYSize; k++) { - U(i,k) /= scale; - s += U(i,k)*U(i,k); - } - f = U(i,i); - g = -SIGN(sqrt(s),f); - h = f*g-s; - U(i,i) = f-g; - for (j = l; j < aXSize; j++) { - for (s = 0.0, k = i; k < aYSize; k++) - s += U(i,k)*U(j,k); - f = s/h; - for (k = i; k < aYSize; k++) - U(j,k) += f*U(i,k); - } - for ( k = i; k < aYSize; k++) - U(i,k) *= scale; - } - } - S(i,i) = scale*g; - g=s=scale=0.0; - if (i < aYSize && i != aXSize-1) { - for (k = l; k < aXSize; k++) - scale += fabs(U(k,i)); - if (scale != 0) { - for (k = l; k < aXSize; k++) { - U(k,i) /= scale; - s += U(k,i)*U(k,i); - } - f = U(l,i); - g = -SIGN(sqrt(s),f); - h = f*g-s; - U(l,i) = f-g; - for (k = l; k < aXSize; k++) - aBuffer(k) = U(k,i)/h; - for (j = l; j < aYSize; j++) { - for (s = 0.0, k = l; k < aXSize; k++) - s += U(k,j)*U(k,i); - for (k = l; k < aXSize; k++) - U(k,j) += s*aBuffer(k); - } - for (k = l; k < aXSize; k++) - U(k,i) *= scale; - } - } - anorm = MAX(anorm,(fabs(S(i,i))+fabs(aBuffer(i)))); - } - for (i = aXSize-1; i >= 0; i--) { - if (i < aXSize-1) { - if (g != 0) { - for (j = l; j < aXSize; j++) - V(i,j) = U(j,i)/(U(l,i)*g); - for (j = l; j < aXSize; j++) { - for (s = 0.0, k = l; k < aXSize; k++) - s += U(k,i)*V(j,k); - for (k = l; k < aXSize; k++) - V(j,k) += s*V(i,k); - } - } - for (j = l; j < aXSize; j++) - V(j,i) = V(i,j) = 0.0; - } - V(i,i) = 1.0; - g = aBuffer(i); - l = i; - } - for (i = MIN(aYSize-1,aXSize-1); i >= 0; i--) { - l = i+1; - g = S(i,i); - for (j = l; j < aXSize; j++) - U(j,i) = 0.0; - if (g != 0) { - g = 1.0/g; - for (j = l; j < aXSize; j++) { - for (s = 0.0, k = l; k < aYSize; k++) - s += U(i,k)*U(j,k); - f = (s/U(i,i))*g; - for (k = i; k < aYSize; k++) - U(j,k) += f*U(i,k); - } - for (j = i; j < aYSize; j++) - U(i,j) *= g; - } - else { - for (j = i; j < aYSize; j++) - U(i,j) = 0.0; - } - ++U(i,i); - } - for (k = aXSize-1; k >= 0; k--) { - for (its = 1; its <= aIterations; its++) { - flag = 1; - for (l = k; l > 0; l--) { - nm = l - 1; - if (fabs(aBuffer(l))+anorm == anorm) { - flag = 0; break; - } - if (fabs(S(nm,nm))+anorm == anorm) break; - } - if (flag) { - c = 0.0; - s = 1.0; - for (i = l; i <= k; i++) { - f = s*aBuffer(i); - aBuffer(i) = c*aBuffer(i); - if (fabs(f)+anorm == anorm) break; - g = S(i,i); - h = PYTHAG(f,g); - S(i,i) = h; - h = 1.0/h; - c = g*h; - s=-f*h; - for (j = 0; j < aYSize; j++) { - y = U(nm,j); - z = U(i,j); - U(nm,j) = y*c + z*s; - U(i,j) = z*c - y*s; - } - } - } - z = S(k,k); - if (l == k) { - if (z < 0.0) { - S(k,k) = -z; - for (j = 0; j < aXSize; j++) - V(k,j) = -V(k,j); - } - break; - } - if (its == aIterations) std::cerr << "svd: No convergence in " << aIterations << " iterations" << std::endl; - x = S(l,l); - nm = k-1; - y = S(nm,nm); - g = aBuffer(nm); - h = aBuffer(k); - f = ((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); - g = PYTHAG(f,1.0); - f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x; - c = s = 1.0; - for (j = l; j <= nm; j++) { - i = j+1; - g = aBuffer(i); - y = S(i,i); - h = s*g; - g = c*g; - z = PYTHAG(f,h); - aBuffer(j) = z; - float invZ = 1.0/z; - c = f*invZ; - s = h*invZ; - f = x*c+g*s; - g = g*c-x*s; - h = y*s; - y *= c; - for (jj = 0; jj < aXSize; jj++) { - x = V(j,jj); - z = V(i,jj); - V(j,jj) = x*c + z*s; - V(i,jj) = z*c - x*s; - } - z = PYTHAG(f,h); - S(j,j) = z; - if (z != 0) { - z = 1.0/z; - c = f*z; - s = h*z; - } - f = (c*g)+(s*y); - x = (c*y)-(s*g); - for (jj = 0; jj < aYSize; jj++) { - y = U(j,jj); - z = U(i,jj); - U(j,jj) = y*c + z*s; - U(i,jj) = z*c - y*s; - } - } - aBuffer(l) = 0.0; - aBuffer(k) = f; - S(k,k) = x; - } - } - // Order singular values - if (aOrdering) { - for (int i = 0; i < aXSize-1; i++) { - int k = i; - for (int j = i+1; j < aXSize; j++) - if (fabs(S(j,j)) > fabs(S(k,k))) k = j; - if (k != i) { - // Switch singular value i and k - float help = S(k,k); - S(k,k) = S(i,i); - S(i,i) = help; - // Switch columns i and k in U and V - for (int j = 0; j < aYSize; j++) { - help = U(k,j); - U(k,j) = U(i,j); - U(i,j) = help; - } - for (int j = 0; j < aXSize; j++) { - help = V(k,j); - V(k,j) = V(i,j); - V(i,j) = help; - } - } - } - } - } - - #undef PYTHAG - #undef MAX - #undef MIN - #undef SIGN - - // svdBack - void svdBack(CMatrix& U, const CMatrix& S, const CMatrix& V) { - for (int y = 0; y < U.ySize(); y++) - for (int x = 0; x < U.xSize(); x++) - U(x,y) = S(x,x)*U(x,y); - U *= trans(V); - } - - // Householder-Verfahren (nach Stoer), uebernommen von Bodo Rosenhahn - // Bei dem Verfahren wird die Matrix A (hier:*this und die rechte Seite (b) - // mit unitaeren Matrizen P multipliziert, so dass A in eine - // obere Dreiecksmatrix umgewandelt wird. - // Dabei ist P = I + beta * u * uH - // Die Vektoren u werden bei jeder Transformation in den nicht - // benoetigten unteren Spalten von A gesichert. - - void householder(CMatrix& A, CVector& b) { - int i,j,k; - float sigma,s,beta,sum; - CVector d(A.xSize()); - for (j = 0; j < A.xSize(); ++j) { - sigma = 0; - for (i = j; i < A.ySize(); ++i) - sigma += A(j,i)*A(j,i); - if (sigma == 0) { - std::cerr << "NMath::householder(): matrix is singular!" << std::endl; - break; - } - // Choose sign to avoid elimination - s = d(j) = A(j,j)<0 ? sqrt(sigma) : -sqrt(sigma); - beta = 1.0/(s*A(j,j)-sigma); - A(j,j) -= s; - // Transform submatrix of A with P - for (k = j+1; k < A.xSize(); ++k) { - sum = 0.0; - for (i = j; i < A.ySize(); ++i) - sum += (A(j,i)*A(k,i)); - sum *= beta; - for (i = j; i < A.ySize(); ++i) - A(k,i) += A(j,i)*sum; - } - // Transform right hand side of linear system with P - sum = 0.0; - for (i = j; i < A.ySize(); ++i) - sum += A(j,i)*b(i); - sum *= beta; - for (i = j; i < A.ySize(); ++i) - b(i) += A(j,i)*sum; - } - for (i = 0; i < A.xSize(); ++i) - A(i,i) = d(i); - } - - // leastSquares - CVector leastSquares(CMatrix& A, CVector& b) { - CVector aResult(A.xSize()); - householder(A,b); - for (int i = A.xSize()-1; i >= 0; i--) { - float s = 0; - for (int k = i+1; k < A.xSize(); k++) - s += A(k,i)*aResult(k); - aResult(i) = (b(i)-s)/A(i,i); - } - return aResult; - } - - // invRegularized - void invRegularized(CMatrix& A, int aReg) { - if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize()); - CVector eVals(A.xSize()); - CMatrix eVecs(A.xSize(),A.ySize()); - PATransformation(A,eVals,eVecs); - for (int i = 0 ; i < A.xSize(); i++) - if (eVals(i) < aReg) eVals(i) = 1.0/aReg; - else eVals(i) = 1.0/eVals(i); - PABacktransformation(eVecs,eVals,A); - } - - // cholesky - void cholesky(CMatrix& A) { - if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize()); - CVector d(A.xSize()); - for (int i = 0; i < A.xSize(); i++) - for (int j = i; j < A.ySize(); j++) { - float sum = A(j,i); - for (int k = i-1; k >= 0; k--) - sum -= A(k,i)*A(k,j); - if (i == j) { - if (sum <= 0.0) return;//throw ENonPositiveDefinite(); - d(i) = sqrt(sum); - } - else A(i,j) = sum/d(i); - } - for (int i = 0; i < A.xSize(); i++) - A(i,i) = d(i); - } - - // triangularSolve - void triangularSolve(CMatrix& L, CVector& aIn, CVector& aOut) { - for (int i = 0; i < aIn.size(); i++) { - float sum = aIn(i); - for (int j = 0; j < i; j++) - sum -= L(j,i)*aOut(j); - aOut(i) = sum/L(i,i); - } - } - - void triangularSolve(CMatrix& L, CMatrix& aIn, CMatrix& aOut) { - CVector invLii(aIn.xSize()); - for (int i = 0; i < aIn.xSize(); i++) - invLii(i) = 1.0/L(i,i); - for (int k = 0; k < aIn.ySize(); k++) - for (int i = 0; i < aIn.xSize(); i++) { - float sum = aIn(i,k); - for (int j = 0; j < i; j++) - sum -= L(j,i)*aOut(j,k); - aOut(i,k) = sum*invLii(i); - } - } - - // triangularSolveTransposed - void triangularSolveTransposed(CMatrix& L, CVector& aIn, CVector& aOut) { - for (int i = aIn.size()-1; i >= 0; i--) { - float sum = aIn(i); - for (int j = aIn.size()-1; j > i; j--) - sum -= L(i,j)*aOut(j); - aOut(i) = sum/L(i,i); - } - } - - void triangularSolveTransposed(CMatrix& L, CMatrix& aIn, CMatrix& aOut) { - CVector invLii(aIn.xSize()); - for (int i = 0; i < aIn.xSize(); i++) - invLii(i) = 1.0/L(i,i); - for (int k = 0; k < aIn.ySize(); k++) - for (int i = aIn.xSize()-1; i >= 0; i--) { - float sum = aIn(i,k); - for (int j = aIn.xSize()-1; j > i; j--) - sum -= L(i,j)*aOut(j,k); - aOut(i,k) = sum*invLii(i); - } - } - - // choleskyInv - void choleskyInv(const CMatrix& L, CMatrix& aInv) { - aInv = 0; - // Compute the inverse of L - CMatrix invL(L.xSize(),L.ySize()); - for (int i = 0; i < L.xSize(); i++) - invL(i,i) = 1.0/L(i,i); - for (int i = 0; i < L.xSize(); i++) - for (int j = i+1; j < L.ySize(); j++) { - float sum = 0.0; - for (int k = i; k < j; k++) - sum -= L(k,j)*invL(i,k); - invL(i,j) = sum*invL(j,j); - } - // Compute lower triangle of aInv = invL^T * invL - for (int i = 0; i < aInv.xSize(); i++) - for (int j = i; j < aInv.ySize(); j++) { - float sum = 0.0; - for (int k = j; k < aInv.ySize(); k++) - sum += invL(j,k)*invL(i,k); - aInv(i,j) = sum; - } - // Complete aInv - for (int i = 0; i < aInv.xSize(); i++) - for (int j = i+1; j < aInv.ySize(); j++) - aInv(j,i) = aInv(i,j); - } - - // eulerAngles - void eulerAngles(float rx, float ry, float rz, CMatrix& A) { - CMatrix Rx(4,4,0); - CMatrix Ry(4,4,0); - CMatrix Rz(4,4,0); - Rx(0,0)=1.0;Rx(1,1)=cos(rx);Rx(2,2)=cos(rx);Rx(3,3)=1.0; - Rx(2,1)=-sin(rx);Rx(1,2)=sin(rx); - Ry(1,1)=1.0;Ry(0,0)=cos(ry);Ry(2,2)=cos(ry);Ry(3,3)=1.0; - Ry(0,2)=-sin(ry);Ry(2,0)=sin(ry); - Rz(2,2)=1.0;Rz(0,0)=cos(rz);Rz(1,1)=cos(rz);Rz(3,3)=1.0; - Rz(1,0)=-sin(rz);Rz(0,1)=sin(rz); - A=Rz*Ry*Rx; - } - - // RBM2Twist - void RBM2Twist(CVector& T, CMatrix& fRBM) { - T.setSize(6); - CMatrix dRBM(4,4); - for (int i = 0; i < 16; i++) - dRBM.data()[i] = fRBM.data()[i]; - CVector omega; - double theta; - CVector v; - CMatrix R(3,3); - double sum = 0.0; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (i != j) sum += dRBM(i,j)*dRBM(i,j); - else sum += (dRBM(i,i)-1.0)*(dRBM(i,i)-1.0); - if (sum < 0.0000001) { - T(0)=fRBM(3,0); T(1)=fRBM(3,1); T(2)=fRBM(3,2); - T(3)=0.0; T(4)=0.0; T(5)=0.0; - } - else { - double diag = (dRBM(0,0)+dRBM(1,1)+dRBM(2,2)-1.0)*0.5; - if (diag < -1.0) diag = -1.0; - else if (diag > 1.0) diag = 1.0; - theta = acos(diag); - if (sin(theta)==0) theta += 0.0000001; - omega.setSize(3); - omega(0)=(dRBM(1,2)-dRBM(2,1)); - omega(1)=(dRBM(2,0)-dRBM(0,2)); - omega(2)=(dRBM(0,1)-dRBM(1,0)); - omega*=(1.0/(2.0*sin(theta))); - CMatrix omegaHat(3,3); - omegaHat.data()[0] = 0.0; omegaHat.data()[1] = -omega(2); omegaHat.data()[2] = omega(1); - omegaHat.data()[3] = omega(2); omegaHat.data()[4] = 0.0; omegaHat.data()[5] = -omega(0); - omegaHat.data()[6] = -omega(1); omegaHat.data()[7] = omega(0); omegaHat.data()[8] = 0.0; - CMatrix omegaT(3,3); - for (int j = 0; j < 3; j++) - for (int i = 0; i < 3; i++) - omegaT(i,j) = omega(i)*omega(j); - R = (omegaHat*(double)sin(theta))+((omegaHat*omegaHat)*(double)(1.0-cos(theta))); - R(0,0) += 1.0; R(1,1) += 1.0; R(2,2) += 1.0; - CMatrix A(3,3); - A.fill(0.0); - A(0,0)=1.0; A(1,1)=1.0; A(2,2)=1.0; - A -= R; A*=omegaHat; A+=omegaT*theta; - CVector p(3); - p(0)=dRBM(3,0); - p(1)=dRBM(3,1); - p(2)=dRBM(3,2); - A.inv(); - v=A*p; - T(0) = (float)(v(0)*theta); - T(1) = (float)(v(1)*theta); - T(2) = (float)(v(2)*theta); - T(3) = (float)(theta*omega(0)); - T(4) = (float)(theta*omega(1)); - T(5) = (float)(theta*omega(2)); - } - } - -} - diff --git a/video_input/consistencyChecker/NMath.h b/video_input/consistencyChecker/NMath.h deleted file mode 100644 index 5f31c3a..0000000 --- a/video_input/consistencyChecker/NMath.h +++ /dev/null @@ -1,170 +0,0 @@ -// NMath -// A collection of mathematical functions and numerical algorithms -// -// Author: Thomas Brox - -#ifndef NMathH -#define NMathH - -#include -#include -#include -#include - -namespace NMath { - // Returns the faculty of a number - int faculty(int n); - // Computes the binomial coefficient of two numbers - int binCoeff(const int n, const int k); - // Returns the angle of the line connecting (x1,y1) with (y1,y2) - float tangent(const float x1, const float y1, const float x2, const float y2); - // Absolute for floating points - inline float abs(const float aValue); - // Computes min or max value of two numbers - inline float min(float aVal1, float aVal2); - inline float max(float aVal1, float aVal2); - inline int min(int aVal1, int aVal2); - inline int max(int aVal1, int aVal2); - // Computes the sign of a value - inline float sign(float aVal); - // minmod function (see description in implementation) - inline float minmod(float a, float b, float c); - // Computes the difference between two angles respecting the cyclic property of an angle - // The result is always between 0 and Pi - float absAngleDifference(const float aFirstAngle, const float aSecondAngle); - // Computes the difference between two angles aFirstAngle - aSecondAngle - // respecting the cyclic property of an angle - // The result ist between -Pi and Pi - float angleDifference(const float aFirstAngle, const float aSecondAngle); - // Computes the sum of two angles respecting the cyclic property of an angle - // The result is between -Pi and Pi - float angleSum(const float aFirstAngle, const float aSecondAngle); - // Rounds to the nearest integer - int round(const float aValue); - // Computes the arctan with results between 0 and 2*Pi - inline float arctan(float x, float y); - - // Computes [0,1] uniformly distributed random number - inline float random(); - // Computes N(0,1) distributed random number - inline float randomGauss(); - - extern const float Pi; - - // Computes a principal axis transformation - // Eigenvectors are in the rows of aEigenvectors - void PATransformation(const CMatrix& aMatrix, CVector& aEigenvalues, CMatrix& aEigenvectors, bool aOrdering = true); - // Computes the principal axis backtransformation - void PABacktransformation(const CMatrix& aEigenVectors, const CVector& aEigenValues, CMatrix& aMatrix); - // Computes a singular value decomposition A=USV^T - // Input: U MxN matrix - // Output: U MxN matrix, S NxN diagonal matrix, V NxN diagonal matrix - void svd(CMatrix& U, CMatrix& S, CMatrix& V, bool aOrdering = true, int aIterations = 20); - // Reassembles A = USV^T, Result in U - void svdBack(CMatrix& U, const CMatrix& S, const CMatrix& V); - // Applies the Householder method to A and b, i.e., A is transformed into an upper triangular matrix - void householder(CMatrix& A, CVector& b); - // Computes least squares solution of an overdetermined linear system Ax=b using the Householder method - CVector leastSquares(CMatrix& A, CVector& b); - // Inverts a square matrix by eigenvalue decomposition, - // eigenvalues smaller than aReg are replaced by aReg - void invRegularized(CMatrix& A, int aReg); - // Given a positive-definite symmetric matrix A, this routine constructs A = LL^T. - // Only the upper triangle of A need be given. L is returned in the lower triangle. - void cholesky(CMatrix& A); - // Solves L*aOut = aIn when L is a lower triangular matrix (e.g. result from cholesky) - void triangularSolve(CMatrix& L, CVector& aIn, CVector& aOut); - void triangularSolve(CMatrix& L, CMatrix& aIn, CMatrix& aOut); - // Solves L^T*aOut = aIn when L is a lower triangular matrix (e.g. result from cholesky) - void triangularSolveTransposed(CMatrix& L, CVector& aIn, CVector& aOut); - void triangularSolveTransposed(CMatrix& L, CMatrix& aIn, CMatrix& aOut); - // Computes the inverse of a matrix, given its cholesky decomposition L (lower triangle) - void choleskyInv(const CMatrix& L, CMatrix& aInv); - // Creates the rotation matrix RzRyRx and extends it to a 4x4 RBM matrix with translation 0 - void eulerAngles(float rx, float ry, float rz, CMatrix& A); - // Transforms a rigid body motion in matrix representation to a twist representation - void RBM2Twist(CVector &T, CMatrix& RBM); -} - -// I M P L E M E N T A T I O N ------------------------------------------------- -// Inline functions have to be implemented directly in the header file - -namespace NMath { - - // abs - inline float abs(const float aValue) { - if (aValue >= 0) return aValue; - else return -aValue; - } - - // min - inline float min(float aVal1, float aVal2) { - if (aVal1 < aVal2) return aVal1; - else return aVal2; - } - - // max - inline float max(float aVal1, float aVal2) { - if (aVal1 > aVal2) return aVal1; - else return aVal2; - } - - // min - inline int min(int aVal1, int aVal2) { - if (aVal1 < aVal2) return aVal1; - else return aVal2; - } - - // max - inline int max(int aVal1, int aVal2) { - if (aVal1 > aVal2) return aVal1; - else return aVal2; - } - - // sign - inline float sign(float aVal) { - if (aVal > 0) return 1.0; - else return -1.0; - } - - // minmod function: - // 0, if any of the a, b, c are 0 or of opposite sign - // sign(a) min(|a|,|b|,|c|) else - inline float minmod(float a, float b, float c) { - if ((sign(a) == sign(b)) && (sign(b) == sign(c)) && (a != 0.0)) { - float aMin = fabs(a); - if (fabs(b) < aMin) aMin = fabs(b); - if (fabs(c) < aMin) aMin = fabs(c); - return sign(a)*aMin; - } - else return 0.0; - } - - // arctan - inline float arctan(float x, float y) { - if (x == 0.0) - if (y >= 0.0) return 0.5 * 3.1415926536; - else return 1.5 * 3.1415926536; - else if (x > 0.0) - if (y >= 0.0) return atan (y/x); - else return 2.0 * 3.1415926536 + atan (y/x); - else return 3.1415926536 + atan (y/x); - } - - // random - inline float random() { - return (float)rand()/RAND_MAX; - } - - // randomGauss - inline float randomGauss() { - // Draw two [0,1]-uniformly distributed numbers a and b - float a = random(); - float b = random(); - // assemble a N(0,1) number c according to Box-Muller */ - if (a > 0.0) return sqrt(-2.0*log(a)) * cos(2.0*3.1415926536*b); - else return 0; - } - -} -#endif -- cgit v1.2.3-70-g09d2