From 200496c4216ab08824851c15632b1a1db6e76b31 Mon Sep 17 00:00:00 2001 From: Ryan Baumann Date: Fri, 29 Jul 2016 15:34:38 -0400 Subject: [PATCH] Add files/code from https://github.com/manuelruder/artistic-videos --- LICENSE | 90 + consistencyChecker/CFilter.h | 2210 +++++++++++++++++++++ consistencyChecker/CMatrix.h | 1395 +++++++++++++ consistencyChecker/CTensor.h | 1205 +++++++++++ consistencyChecker/CTensor4D.h | 656 ++++++ consistencyChecker/CVector.h | 519 +++++ consistencyChecker/Makefile | 3 + consistencyChecker/NMath.cpp | 664 +++++++ consistencyChecker/NMath.h | 170 ++ consistencyChecker/consistencyChecker.cpp | 113 ++ makeOptFlow.sh | 56 + run-deepflow.sh | 10 + 12 files changed, 7091 insertions(+) create mode 100644 LICENSE create mode 100644 consistencyChecker/CFilter.h create mode 100644 consistencyChecker/CMatrix.h create mode 100644 consistencyChecker/CTensor.h create mode 100644 consistencyChecker/CTensor4D.h create mode 100644 consistencyChecker/CVector.h create mode 100644 consistencyChecker/Makefile create mode 100644 consistencyChecker/NMath.cpp create mode 100644 consistencyChecker/NMath.h create mode 100644 consistencyChecker/consistencyChecker.cpp create mode 100755 makeOptFlow.sh create mode 100644 run-deepflow.sh diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..c81d9c1 --- /dev/null +++ b/LICENSE @@ -0,0 +1,90 @@ +The code in the following files/directories is adapted from https://github.com/manuelruder/artistic-videos: + +* makeOptFlow.sh +* run-deepflow.sh +* flowFileLoader.lua +* consistencyChecker + +These files are under the following license: + +This code is for non-profit use only. Any commercial use is +prohibited. + +(c) Manuel Ruder, Alexey Dosovitskiy, Thomas Brox 2016 + +If you use this program, you should cite the following paper: + +M. Ruder, A. Dosovitskiy, T. Brox (2016). "Artistic style transfer for videos". arXiv:1604.08610 + + + +This code is partially based on the neural-style code by Justin Johnson, +which is covered by the following copyright and permission notice: + +****************************************************************************** +The MIT License (MIT) + +Copyright (c) 2015 Justin Johnson + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +****************************************************************************** + + + +The present "lbfgs.lua" is a modified version of "lbfgs.lua" included in the +Torch "Optimization package", which is covered by the following copyright and +permission notice: + +****************************************************************************** +Copyright (c) 2011-2014 Idiap Research Institute (Ronan Collobert) +Copyright (c) 2011-2012 NEC Laboratories America (Koray Kavukcuoglu) +Copyright (c) 2011-2013 NYU (Clement Farabet) +Copyright (c) 2006-2010 NEC Laboratories America (Ronan Collobert, Leon Bottou, Iain Melvin, Jason Weston) +Copyright (c) 2006 Idiap Research Institute (Samy Bengio) +Copyright (c) 2001-2004 Idiap Research Institute (Ronan Collobert, Samy Bengio, Johnny Mariethoz) + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +3. Neither the names of NEC Laboratories American and IDIAP Research + Institute nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior + written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +****************************************************************************** diff --git a/consistencyChecker/CFilter.h b/consistencyChecker/CFilter.h new file mode 100644 index 0000000..29bef9e --- /dev/null +++ b/consistencyChecker/CFilter.h @@ -0,0 +1,2210 @@ +// - 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/consistencyChecker/CMatrix.h b/consistencyChecker/CMatrix.h new file mode 100644 index 0000000..9342a7f --- /dev/null +++ b/consistencyChecker/CMatrix.h @@ -0,0 +1,1395 @@ +// 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/consistencyChecker/CTensor4D.h b/consistencyChecker/CTensor4D.h new file mode 100644 index 0000000..6feeb5d --- /dev/null +++ b/consistencyChecker/CTensor4D.h @@ -0,0 +1,656 @@ +// 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/consistencyChecker/CVector.h b/consistencyChecker/CVector.h new file mode 100644 index 0000000..aacb0fa --- /dev/null +++ b/consistencyChecker/CVector.h @@ -0,0 +1,519 @@ +// 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/consistencyChecker/Makefile b/consistencyChecker/Makefile new file mode 100644 index 0000000..94725e2 --- /dev/null +++ b/consistencyChecker/Makefile @@ -0,0 +1,3 @@ +default: + g++ -O3 -fPIC consistencyChecker.cpp NMath.cpp -I. -o consistencyChecker -L. + diff --git a/consistencyChecker/NMath.cpp b/consistencyChecker/NMath.cpp new file mode 100644 index 0000000..3a58b16 --- /dev/null +++ b/consistencyChecker/NMath.cpp @@ -0,0 +1,664 @@ +// 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/consistencyChecker/NMath.h b/consistencyChecker/NMath.h new file mode 100644 index 0000000..5f31c3a --- /dev/null +++ b/consistencyChecker/NMath.h @@ -0,0 +1,170 @@ +// 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 diff --git a/consistencyChecker/consistencyChecker.cpp b/consistencyChecker/consistencyChecker.cpp new file mode 100644 index 0000000..0a8ea11 --- /dev/null +++ b/consistencyChecker/consistencyChecker.cpp @@ -0,0 +1,113 @@ +// consistencyChecker +// Check consistency of forward flow via backward flow. +// +// (c) Manuel Ruder, Alexey Dosovitskiy, Thomas Brox 2016 + +#include +#include +#include "CTensor.h" +#include "CFilter.h" + +// Which certainty value motion boundaries should get. Value between 0 (uncertain) and 255 (certain). +#define MOTION_BOUNDARIE_VALUE 0 + +// The amount of gaussian smoothing that sould be applied. Set 0 to disable smoothing. +#define SMOOTH_STRENGH 0.8 + +// readMiddlebury +bool readMiddlebury(const char* filename, CTensor& flow) { + FILE *stream = fopen(filename, "rb"); + if (stream == 0) { + std::cout << "Could not open " << filename << std::endl; + return false; + } + float help; + int dummy; + dummy = fread(&help,sizeof(float),1,stream); + int aXSize,aYSize; + dummy = fread(&aXSize,sizeof(int),1,stream); + dummy = fread(&aYSize,sizeof(int),1,stream); + flow.setSize(aXSize,aYSize,2); + for (int y = 0; y < flow.ySize(); y++) + for (int x = 0; x < flow.xSize(); x++) { + dummy = fread(&flow(x,y,0),sizeof(float),1,stream); + dummy = fread(&flow(x,y,1),sizeof(float),1,stream); + } + fclose(stream); + return true; +} + +void checkConsistency(const CTensor& flow1, const CTensor& flow2, CMatrix& reliable, int argc, char** args) { + int xSize = flow1.xSize(), ySize = flow1.ySize(); + int size = xSize * ySize; + CTensor dx(xSize,ySize,2); + CTensor dy(xSize,ySize,2); + CDerivative derivative(3); + NFilter::filter(flow1,dx,derivative,1,1); + NFilter::filter(flow1,dy,1,derivative,1); + CMatrix motionEdge(xSize,ySize,0); + for (int i = 0; i < size; i++) { + motionEdge.data()[i] += dx.data()[i]*dx.data()[i]; + motionEdge.data()[i] += dx.data()[size+i]*dx.data()[size+i]; + motionEdge.data()[i] += dy.data()[i]*dy.data()[i]; + motionEdge.data()[i] += dy.data()[size+i]*dy.data()[size+i]; + } + + for (int ay = 0; ay < flow1.ySize(); ay++) + for (int ax = 0; ax < flow1.xSize(); ax++) { + float bx = ax+flow1(ax, ay, 0); + float by = ay+flow1(ax, ay, 1); + int x1 = floor(bx); + int y1 = floor(by); + int x2 = x1 + 1; + int y2 = y1 + 1; + if (x1 < 0 || x2 >= xSize || y1 < 0 || y2 >= ySize) + { reliable(ax, ay) = 0.0f; continue; } + float alphaX = bx-x1; float alphaY = by-y1; + float a = (1.0-alphaX) * flow2(x1, y1, 0) + alphaX * flow2(x2, y1, 0); + float b = (1.0-alphaX) * flow2(x1, y2, 0) + alphaX * flow2(x2, y2, 0); + float u = (1.0-alphaY)*a+alphaY*b; + a = (1.0-alphaX) * flow2(x1, y1, 1) + alphaX * flow2(x2, y1, 1); + b = (1.0-alphaX) * flow2(x1, y2, 1) + alphaX * flow2(x2, y2, 1); + float v = (1.0-alphaY)*a+alphaY*b; + float cx = bx+u; + float cy = by+v; + float u2 = flow1(ax,ay,0); + float v2 = flow1(ax,ay,1); + if (((cx-ax) * (cx-ax) + (cy-ay) * (cy-ay)) >= 0.01*(u2*u2 + v2*v2 + u*u + v*v) + 0.5f) { + // Set to a negative value so that when smoothing is applied the smoothing goes "to the outside". + // Afterwards, we clip values below 0. + reliable(ax, ay) = -255.0f; + continue; + } + if (motionEdge(ax, ay) > 0.01 * (u2*u2+v2*v2) + 0.002f) { + reliable(ax, ay) = MOTION_BOUNDARIE_VALUE; + continue; + } + } +} + +int main(int argc, char** args) { + assert(argc >= 4); + + CTensor flow1,flow2; + readMiddlebury(args[1], flow1); + readMiddlebury(args[2], flow2); + + assert(flow1.xSize() == flow2.xSize()); + assert(flow1.ySize() == flow2.ySize()); + + int xSize = flow1.xSize(), ySize = flow1.ySize(); + + // Check consistency of forward flow via backward flow and exlucde motion boundaries + CMatrix reliable(xSize, ySize, 255.0f); + checkConsistency(flow1, flow2, reliable, argc, args); + + if (SMOOTH_STRENGH > 0) { + CSmooth smooth(SMOOTH_STRENGH, 2.0f); + NFilter::filter(reliable, smooth, smooth); + } + reliable.clip(0.0f, 255.0f); + + reliable.writeToPGM(args[3]); +} \ No newline at end of file diff --git a/makeOptFlow.sh b/makeOptFlow.sh new file mode 100755 index 0000000..0edbb77 --- /dev/null +++ b/makeOptFlow.sh @@ -0,0 +1,56 @@ +# Specify the path to the optical flow utility here. +# Also check line 44 and 47 whether the arguments are in the correct order. +flowCommandLine="bash run-deepflow.sh" + +if [ -z "$flowCommandLine" ]; then + echo "Please open makeOptFlow.sh and specify the command line for computing the optical flow." + exit 1 +fi + +if [ ! -f ./consistencyChecker/consistencyChecker ]; then + if [ ! -f ./consistencyChecker/Makefile ]; then + echo "Consistency checker makefile not found." + exit 1 + fi + cd consistencyChecker/ + make + cd .. +fi + +filePattern=$1 +folderName=$2 +startFrame=${3:-1} +stepSize=${4:-1} + +if [ "$#" -le 1 ]; then + echo "Usage: ./makeOptFlow [ []]" + echo -e "\tfilePattern:\tFilename pattern of the frames of the videos." + echo -e "\toutputFolder:\tOutput folder." + echo -e "\tstartNumber:\tThe index of the first frame. Default: 1" + echo -e "\tstepSize:\tThe step size to create long-term flow. Default: 1" + exit 1 +fi + +i=$[$startFrame] +j=$[$startFrame + $stepSize] + +mkdir -p "${folderName}" + +while true; do + file1=$(printf "$filePattern" "$i") + file2=$(printf "$filePattern" "$j") + if [ -a $file2 ]; then + if [ ! -f ${folderName}/forward_${i}_${j}.flo ]; then + eval $flowCommandLine "$file1" "$file2" "${folderName}/forward_${i}_${j}.flo" + fi + if [ ! -f ${folderName}/backward_${j}_${i}.flo ]; then + eval $flowCommandLine "$file2" "$file1" "${folderName}/backward_${j}_${i}.flo" + fi + ./consistencyChecker/consistencyChecker "${folderName}/backward_${j}_${i}.flo" "${folderName}/forward_${i}_${j}.flo" "${folderName}/reliable_${j}_${i}.pgm" + ./consistencyChecker/consistencyChecker "${folderName}/forward_${i}_${j}.flo" "${folderName}/backward_${j}_${i}.flo" "${folderName}/reliable_${i}_${j}.pgm" + else + break + fi + i=$[$i +1] + j=$[$j +1] +done diff --git a/run-deepflow.sh b/run-deepflow.sh new file mode 100644 index 0000000..68fd593 --- /dev/null +++ b/run-deepflow.sh @@ -0,0 +1,10 @@ +if [ "$#" -ne 3 ]; then + echo "This is an auxiliary script for makeOptFlow.sh. No need to call this script directly." + exit 1 +fi +if [ ! -f deepmatching-static ] && [ ! -f deepflow2-static ]; then + echo "Place deepflow2-static and deepmatching-static in this directory." + exit 1 +fi + +./deepmatching-static $1 $2 -nt 0 | ./deepflow2-static $1 $2 $3 -match \ No newline at end of file