diff options
| -rw-r--r-- | LICENSE | 90 | ||||
| -rw-r--r-- | consistencyChecker/CFilter.h | 2210 | ||||
| -rw-r--r-- | consistencyChecker/CMatrix.h | 1395 | ||||
| -rw-r--r-- | consistencyChecker/CTensor.h | 1205 | ||||
| -rw-r--r-- | consistencyChecker/CTensor4D.h | 656 | ||||
| -rw-r--r-- | consistencyChecker/CVector.h | 519 | ||||
| -rw-r--r-- | consistencyChecker/Makefile | 3 | ||||
| -rw-r--r-- | consistencyChecker/NMath.cpp | 664 | ||||
| -rw-r--r-- | consistencyChecker/NMath.h | 170 | ||||
| -rw-r--r-- | consistencyChecker/consistencyChecker.cpp | 113 | ||||
| -rwxr-xr-x | makeOptFlow.sh | 56 | ||||
| -rw-r--r-- | run-deepflow.sh | 10 |
12 files changed, 7091 insertions, 0 deletions
@@ -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 <math.h>
+#include <NMath.h>
+#include <CVector.h>
+#include <CMatrix.h>
+#include <CTensor.h>
+#include <CTensor4D.h>
+
+// 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<double> 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 T>
+class CFilter : public CVector<T> {
+public:
+ // constructor
+ inline CFilter(const int aSize, const int aDelta = 0);
+ // copy constructor
+ CFilter(const CFilter<T>& aCopyFrom);
+ // constructor initialized by a vector
+ CFilter(const CVector<T>& 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<T>& operator=(const CFilter<T>& aCopyFrom);
+
+ // Access to the filter's delta
+ inline int delta() const;
+ // Access to the filter's range A<=i<B
+ inline int A() const;
+ inline int B() const;
+ // Returns the sum of all filter co-efficients (absolutes)
+ T sum() const;
+ // Shifts the filter
+ inline void shift(int aDelta);
+protected:
+ int mDelta;
+};
+
+template <class T>
+class CFilter2D : public CMatrix<T> {
+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<T>& aCopyFrom);
+ // constructor initialized by a matrix
+ CFilter2D(const CMatrix<T>& 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<T>& operator=(const CFilter2D<T>& aCopyFrom);
+
+ // Access to the filter's delta
+ inline int deltaX() const;
+ inline int deltaY() const;
+ // Access to the filter's range A<=i<B
+ inline int AX() const;
+ inline int BX() const;
+ inline int AY() const;
+ inline int BY() const;
+ // Returns the sum of all filter co-efficients (absolutes)
+ T sum() const;
+protected:
+ int mDeltaX;
+ int mDeltaY;
+};
+
+namespace NFilter {
+
+ // Linear 1D filtering
+
+ // Convolution of the vector aVector with aFilter
+ // The result will be written into aVector, so its initial values will get lost
+ template <class T> inline void filter(CVector<T>& aVector, const CFilter<T>& aFilter);
+ // Convolution of the vector aVector with aFilter, the initial values of aVector will persist.
+ template <class T> void filter(const CVector<T>& aVector, CVector<T>& aResult, const CFilter<T>& aFilter);
+
+ // Convolution with a rectangle -> approximation of Gaussian
+ template <class T> inline void boxFilter(CVector<T>& aVector, int aWidth);
+ template <class T> void boxFilter(const CVector<T>& aVector, CVector<T>& 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 <class T> inline void filter(CMatrix<T>& aMatrix, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY);
+ template <class T> inline void filterMin(CMatrix<T>& aMatrix, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY);
+ // Convolution of the matrix aMatrix with aFilter, aFilter must be separable
+ // The initial values of aMatrix will persist.
+ template <class T> inline void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY);
+ template <class T> inline void filterMin(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter<T>& aFilterX, const CFilter<T>& 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 <class T> inline void filter(CMatrix<T>& aMatrix, const CFilter<T>& aFilter, const int aDummy);
+ template <class T> inline void filterMin(CMatrix<T>& aMatrix, const CFilter<T>& 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 <class T> void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter<T>& aFilter, const int aDummy);
+ template <class T> void filterMin(const CMatrix<T>& aMatrix, const CMatrix<T>& aOrig, CMatrix<T>& aResult, const CFilter<T>& 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 <class T> inline void filter(CMatrix<T>& aMatrix, const int aDummy, const CFilter<T>& aFilter);
+ template <class T> inline void filterMin(CMatrix<T>& aMatrix, const int aDummy, const CFilter<T>& 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 <class T> void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const int aDummy, const CFilter<T>& aFilter);
+ template <class T> void filterMin(const CMatrix<T>& aMatrix, const CMatrix<T>& aOrig, CMatrix<T>& aResult, const int aDummy, const CFilter<T>& aFilter);
+
+ // Convolution of the matrix aMatrix with aFilter
+ // The result will be written to aMatrix, so its initial values will get lost
+ template <class T> inline void filter(CMatrix<T>& aMatrix, const CFilter2D<T>& aFilter);
+ // Convolution of the matrix aMatrix with aFilter, the initial values of aMatrix will persist
+ template <class T> void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter2D<T>& aFilter);
+
+ // Convolution with a rectangle -> approximation of Gaussian
+ template <class T> inline void boxFilterX(CMatrix<T>& aMatrix, int aWidth);
+ template <class T> void boxFilterX(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, int aWidth);
+ template <class T> inline void boxFilterY(CMatrix<T>& aMatrix, int aWidth);
+ template <class T> void boxFilterY(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, int aWidth);
+
+ // Recursive filter -> approximation of Gaussian
+ template <class T> void recursiveSmoothX(CMatrix<T>& aMatrix, float aSigma);
+ template <class T> void recursiveSmoothY(CMatrix<T>& aMatrix, float aSigma);
+ template <class T> inline void recursiveSmooth(CMatrix<T>& 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 <class T> inline void filter(CTensor<T>& aTensor, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY, const CFilter<T>& aFilterZ);
+ // Convolution of the 3D Tensor aTensor with aFilter, aFilter must be separable
+ // The initial values of aTensor will persist
+ template <class T> inline void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY, const CFilter<T>& aFilterZ);
+
+ // Convolution of the 3D Tensor aTensor with aFilter only in x-Direction
+ template <class T> inline void filter(CTensor<T>& aTensor, const CFilter<T>& aFilter, const int aDummy1, const int aDummy2);
+ template <class T> void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const CFilter<T>& aFilter, const int aDummy1, const int aDummy2);
+ // Convolution of the 3D Tensor aTensor with aFilter only in y-Direction
+ template <class T> inline void filter(CTensor<T>& aTensor, const int aDummy1, const CFilter<T>& aFilter, const int aDummy2);
+ template <class T> void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const int aDummy1, const CFilter<T>& aFilter, const int aDummy2);
+ // Convolution of the 3D Tensor aTensor with aFilter only in z-Direction
+ template <class T> inline void filter(CTensor<T>& aTensor, const int aDummy1, const int aDummy2, const CFilter<T>& aFilter);
+ template <class T> void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const int aDummy1, const int aDummy2, const CFilter<T>& aFilter);
+
+ // Convolution with a rectangle -> approximation of Gaussian
+ template <class T> inline void boxFilterX(CTensor<T>& aTensor, int aWidth);
+ template <class T> void boxFilterX(const CTensor<T>& aTensor, CTensor<T>& aResult, int aWidth);
+ template <class T> inline void boxFilterY(CTensor<T>& aTensor, int aWidth);
+ template <class T> void boxFilterY(const CTensor<T>& aTensor, CTensor<T>& aResult, int aWidth);
+ template <class T> inline void boxFilterZ(CTensor<T>& aTensor, int aWidth);
+ template <class T> void boxFilterZ(const CTensor<T>& aTensor, CTensor<T>& aResult, int aWidth);
+
+ // Recursive filter -> approximation of Gaussian
+ template <class T> void recursiveSmoothX(CTensor<T>& aTensor, float aSigma);
+ template <class T> void recursiveSmoothY(CTensor<T>& aTensor, float aSigma);
+ template <class T> void recursiveSmoothZ(CTensor<T>& 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 <class T> inline void filter(CTensor4D<T>& aTensor, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY, const CFilter<T>& aFilterZ, const CFilter<T>& aFilterA);
+
+ // Convolution of the 4D Tensor aTensor with aFilter only in x-Direction
+ template <class T> inline void filter(CTensor4D<T>& aTensor, const CFilter<T>& aFilter, const int aDummy1, const int aDummy2, const int aDummy3);
+ template <class T> void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const CFilter<T>& aFilter, const int aDummy1, const int aDummy2, const int aDummy3);
+ // Convolution of the 4D Tensor aTensor with aFilter only in y-Direction
+ template <class T> inline void filter(CTensor4D<T>& aTensor, const int aDummy1, const CFilter<T>& aFilter, const int aDummy2, const int aDummy3);
+ template <class T> void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const int aDummy1, const CFilter<T>& aFilter, const int aDummy2, const int aDummy3);
+ // Convolution of the 4D Tensor aTensor with aFilter only in z-Direction
+ template <class T> inline void filter(CTensor4D<T>& aTensor, const int aDummy1, const int aDummy2, const CFilter<T>& aFilter, const int aDummy3);
+ template <class T> void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const int aDummy1, const int aDummy2, const CFilter<T>& aFilter, const int aDummy3);
+ // Convolution of the 4D Tensor aTensor with aFilter only in a-Direction
+ template <class T> inline void filter(CTensor4D<T>& aTensor, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter<T>& aFilter);
+ template <class T> void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter<T>& aFilter);
+
+ // Recursive filter -> approximation of Gaussian
+ template <class T> void recursiveSmoothX(CTensor4D<T>& aTensor, float aSigma);
+ template <class T> void recursiveSmoothY(CTensor4D<T>& aTensor, float aSigma);
+ template <class T> void recursiveSmoothZ(CTensor4D<T>& aTensor, float aSigma);
+ template <class T> void recursiveSmoothA(CTensor4D<T>& aTensor, float aSigma);
+
+ // Nonlinear filtering: Osher shock filter
+ template <class T> void osher(CMatrix<T>& aData, int aIterations = 20);
+ template <class T> inline void osher(const CMatrix<T>& aData, CMatrix<T>& aResult, int aIterations = 20);
+}
+
+// Common filters
+
+template <class T>
+class CGauss : public CFilter<T> {
+public:
+ CGauss(const int aSize, const int aDegreeOfDerivative);
+};
+
+template <class T>
+class CSmooth : public CFilter<T> {
+public:
+ CSmooth(float aSigma, float aPrecision);
+};
+
+template <class T>
+class CGaussianFirstDerivative : public CFilter<T> {
+public:
+ CGaussianFirstDerivative(float aSigma, float aPrecision);
+};
+
+template <class T>
+class CGaussianSecondDerivative : public CFilter<T> {
+public:
+ CGaussianSecondDerivative(float aSigma, float aPrecision);
+};
+
+template <class T>
+class CDerivative : public CFilter<T> {
+public:
+ CDerivative(const int aSize);
+};
+
+template <class T>
+class CHighOrderDerivative : public CFilter<T> {
+public:
+ CHighOrderDerivative(int aOrder, int aSize);
+};
+
+template <class T>
+class CGaborReal : public CFilter2D<T> {
+public:
+ CGaborReal(float aFrequency, float aAngle, float aSigma1 = 3.0, float aSigma2 = 3.0);
+};
+
+template <class T>
+class CGaborImaginary : public CFilter2D<T> {
+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 <class T>
+inline CFilter<T>::CFilter(const int aSize, const int aDelta)
+ : CVector<T>(aSize),mDelta(aDelta) {
+}
+
+// copy constructor
+template <class T>
+CFilter<T>::CFilter(const CFilter<T>& aCopyFrom)
+ : CVector<T>(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 <class T>
+CFilter<T>::CFilter(const CVector<T>& aCopyFrom, const int aDelta)
+ : CVector<T>(aCopyFrom.size()),mDelta(aDelta) {
+ for (register int i = 0; i < this->mSize; i++)
+ this->mData[i] = aCopyFrom(i);
+}
+
+// operator()
+template <class T>
+inline T& CFilter<T>::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 <class T>
+inline T& CFilter<T>::operator[](const int aIndex) const {
+ return operator()(aIndex);
+}
+
+// operator=
+template <class T>
+CFilter<T>& CFilter<T>::operator=(const CFilter<T>& 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 <class T>
+inline int CFilter<T>::delta() const {
+ return mDelta;
+}
+
+// A
+template <class T>
+inline int CFilter<T>::A() const {
+ return -mDelta;
+}
+
+// B
+template <class T>
+inline int CFilter<T>::B() const {
+ return this->mSize-mDelta;
+}
+
+// sum
+template <class T>
+T CFilter<T>::sum() const {
+ T aResult = 0;
+ for (int i = 0; i < this->mSize; i++)
+ aResult += fabs(this->mData[i]);
+ return aResult;
+}
+
+// shift
+template <class T>
+inline void CFilter<T>::shift(int aDelta) {
+ mDelta += aDelta;
+}
+
+// C F I L T E R 2 D -----------------------------------------------------------
+// P U B L I C ----------------------------------------------------------------
+// constructor
+template <class T>
+inline CFilter2D<T>::CFilter2D()
+ : CMatrix<T>(),mDeltaX(0),mDeltaY(0) {
+}
+
+template <class T>
+inline CFilter2D<T>::CFilter2D(const int aXSize, const int aYSize, const int aDeltaX, const int aDeltaY)
+ : CMatrix<T>(aXSize,aYSize),mDeltaX(aDeltaX),mDeltaY(aDeltaY) {
+}
+
+// copy constructor
+template <class T>
+CFilter2D<T>::CFilter2D(const CFilter2D<T>& aCopyFrom)
+ : CMatrix<T>(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 <class T>
+CFilter2D<T>::CFilter2D(const CMatrix<T>& aCopyFrom, const int aDeltaX, const int aDeltaY)
+ : CMatrix<T>(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 <class T>
+void CFilter2D<T>::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 <class T>
+void CFilter2D<T>::shift(int aXDelta, int aYDelta) {
+ mDeltaX = aXDelta;
+ mDeltaY = aYDelta;
+}
+
+// operator()
+template <class T>
+inline T& CFilter2D<T>::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 <class T>
+CFilter2D<T>& CFilter2D<T>::operator=(const CFilter2D<T>& 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 <class T>
+inline int CFilter2D<T>::deltaX() const {
+ return mDeltaX;
+}
+
+// deltaY
+template <class T>
+inline int CFilter2D<T>::deltaY() const {
+ return mDeltaY;
+}
+
+// AX
+template <class T>
+inline int CFilter2D<T>::AX() const {
+ return -mDeltaX;
+}
+
+// AY
+template <class T>
+inline int CFilter2D<T>::AY() const {
+ return -mDeltaY;
+}
+
+// BX
+template <class T>
+inline int CFilter2D<T>::BX() const {
+ return this->mXSize-mDeltaX;
+}
+
+// BY
+template <class T>
+inline int CFilter2D<T>::BY() const {
+ return this->mYSize-mDeltaY;
+}
+
+// sum
+template <class T>
+T CFilter2D<T>::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 <class T>
+CGauss<T>::CGauss(const int aSize, const int aDegreeOfDerivative)
+ : CFilter<T>(aSize,aSize >> 1) {
+ CVector<int> *oldData;
+ CVector<int> *newData;
+ CVector<int> *temp;
+ oldData = new CVector<int>(aSize);
+ newData = new CVector<int>(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 <class T>
+CSmooth<T>::CSmooth(float aSigma, float aPrecision)
+ : CFilter<T>(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 <class T>
+CGaussianFirstDerivative<T>::CGaussianFirstDerivative(float aSigma, float aPrecision)
+ : CFilter<T>(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 <class T>
+CGaussianSecondDerivative<T>::CGaussianSecondDerivative(float aSigma, float aPrecision)
+ : CFilter<T>(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 <class T>
+CDerivative<T>::CDerivative(const int aSize)
+ : CFilter<T>(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 <class T>
+CHighOrderDerivative<T>::CHighOrderDerivative(int aOrder, int aSize)
+ : CFilter<T>(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 <class T>
+CGaborReal<T>::CGaborReal(float aFrequency, float aAngle, float aSigma1, float aSigma2)
+ : CFilter2D<T>() {
+ // 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 <class T>
+CGaborImaginary<T>::CGaborImaginary(float aFrequency, float aAngle, float aSigma1, float aSigma2)
+ : CFilter2D<T>() {
+ // 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 <class T>
+inline void filter(CVector<T>& aVector, const CFilter<T>& aFilter) {
+ CVector<T> oldVector(aVector);
+ filter(oldVector,aVector,aFilter);
+}
+
+template <class T>
+void filter(const CVector<T>& aVector, CVector<T>& aResult, const CFilter<T>& 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 <class T>
+inline void boxFilter(CVector<T>& aVector, int aWidth) {
+ CVector<T> aTemp(aVector);
+ boxFilter(aTemp,aVector,aWidth);
+}
+
+template <class T>
+void boxFilter(const CVector<T>& aVector, CVector<T>& 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 <class T>
+inline void filter(CMatrix<T>& aMatrix, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filter(aMatrix,tempMatrix,aFilterX,1);
+ filter(tempMatrix,aMatrix,1,aFilterY);
+}
+
+template <class T>
+inline void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filter(aMatrix,tempMatrix,aFilterX,1);
+ filter(tempMatrix,aResult,1,aFilterY);
+}
+
+template <class T>
+inline void filter(CMatrix<T>& aMatrix, const CFilter<T>& aFilter, const int aDummy) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filter(aMatrix,tempMatrix,aFilter,1);
+ aMatrix = tempMatrix;
+}
+
+template <class T>
+void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter<T>& 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 <class T>
+inline void filter(CMatrix<T>& aMatrix, const int aDummy, const CFilter<T>& aFilter) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filter(aMatrix,tempMatrix,1,aFilter);
+ aMatrix = tempMatrix;
+}
+
+template <class T>
+void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const int aDummy, const CFilter<T>& 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 <class T>
+inline void filter(CMatrix<T>& aMatrix, const CFilter2D<T>& aFilter) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filter(aMatrix,tempMatrix,aFilter);
+ aMatrix = tempMatrix;
+}
+
+template <class T>
+void filter(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter2D<T>& 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 <class T>
+inline void filterMin(CMatrix<T>& aMatrix, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filterMin(aMatrix,aMatrix,tempMatrix,aFilterX,1);
+ CMatrix<T> tmp(aMatrix);
+ filterMin(tempMatrix,tmp,aMatrix,1,aFilterY);
+}
+
+template <class T>
+inline void filterMin(const CMatrix<T>& aMatrix, CMatrix<T>& aResult, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filterMin(aMatrix,aMatrix,tempMatrix,aFilterX,1);
+ filterMin(tempMatrix,aMatrix,aResult,1,aFilterY);
+}
+
+template <class T>
+inline void filterMin(CMatrix<T>& aMatrix, const CFilter<T>& aFilter, const int aDummy) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filterMin(aMatrix,aMatrix,tempMatrix,aFilter,1);
+ aMatrix = tempMatrix;
+}
+
+template <class T>
+void filterMin(const CMatrix<T>& aMatrix, const CMatrix<T>& aOrig, CMatrix<T>& aResult, const CFilter<T>& 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 <class T>
+inline void filterMin(CMatrix<T>& aMatrix, const int aDummy, const CFilter<T>& aFilter) {
+ CMatrix<T> tempMatrix(aMatrix.xSize(),aMatrix.ySize());
+ filterMin(aMatrix, aMatrix,tempMatrix,1,aFilter);
+ aMatrix = tempMatrix;
+}
+
+template <class T>
+void filterMin(const CMatrix<T>& aMatrix, const CMatrix<T>& aOrig, CMatrix<T>& aResult, const int aDummy, const CFilter<T>& 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 <class T>
+inline void boxFilterX(CMatrix<T>& aMatrix, int aWidth) {
+ CMatrix<T> aTemp(aMatrix);
+ boxFilterX(aTemp,aMatrix,aWidth);
+}
+
+template <class T>
+void boxFilterX(const CMatrix<T>& aMatrix, CMatrix<T>& 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 <class T>
+inline void boxFilterY(CMatrix<T>& aMatrix, int aWidth) {
+ CMatrix<T> aTemp(aMatrix);
+ boxFilterY(aTemp,aMatrix,aWidth);
+}
+
+template <class T>
+void boxFilterY(const CMatrix<T>& aMatrix, CMatrix<T>& 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 <class T>
+void recursiveSmoothX(CMatrix<T>& aMatrix, float aSigma) {
+ CVector<T> aVals1(aMatrix.xSize());
+ CVector<T> 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 <class T>
+void recursiveSmoothY(CMatrix<T>& aMatrix, float aSigma) {
+ CVector<T> aVals1(aMatrix.ySize());
+ CVector<T> 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 <class T>
+inline void recursiveSmooth(CMatrix<T>& aMatrix, float aSigma) {
+ recursiveSmoothX(aMatrix,aSigma);
+ recursiveSmoothY(aMatrix,aSigma);
+}
+
+// Linear 3D filtering ---------------------------------------------------------
+
+template <class T>
+inline void filter(CTensor<T>& aTensor, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY, const CFilter<T>& aFilterZ) {
+ CTensor<T> 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 <class T>
+inline void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY, const CFilter<T>& aFilterZ) {
+ CTensor<T> 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 <class T>
+inline void filter(CTensor<T>& aTensor, const CFilter<T>& aFilter, const int aDummy1, const int aDummy2) {
+ CTensor<T> tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize());
+ filter(aTensor,tempTensor,aFilter,1,1);
+ aTensor = tempTensor;
+}
+
+template <class T>
+void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const CFilter<T>& 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 <class T>
+inline void filter(CTensor<T>& aTensor, const int aDummy1, const CFilter<T>& aFilter, const int aDummy2) {
+ CTensor<T> tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize());
+ filter(aTensor,tempTensor,1,aFilter,1);
+ aTensor = tempTensor;
+}
+
+template <class T>
+void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const int aDummy1, const CFilter<T>& 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 <class T>
+inline void filter(CTensor<T>& aTensor, const int aDummy1, const int aDummy2, const CFilter<T>& aFilter) {
+ CTensor<T> tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize());
+ filter(aTensor,tempTensor,1,1,aFilter);
+ aTensor = tempTensor;
+}
+
+template <class T>
+void filter(const CTensor<T>& aTensor, CTensor<T>& aResult, const int aDummy1, const int aDummy2, const CFilter<T>& 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 <class T>
+inline void boxFilterX(CTensor<T>& aTensor, int aWidth) {
+ CTensor<T> aTemp(aTensor);
+ boxFilterX(aTemp,aTensor,aWidth);
+}
+
+template <class T>
+void boxFilterX(const CTensor<T>& aTensor, CTensor<T>& 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 <class T>
+inline void boxFilterY(CTensor<T>& aTensor, int aWidth) {
+ CTensor<T> aTemp(aTensor);
+ boxFilterY(aTemp,aTensor,aWidth);
+}
+
+template <class T>
+void boxFilterY(const CTensor<T>& aTensor, CTensor<T>& 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 <class T>
+inline void boxFilterZ(CTensor<T>& aTensor, int aWidth) {
+ CTensor<T> aTemp(aTensor);
+ boxFilterZ(aTemp,aTensor,aWidth);
+}
+
+template <class T>
+void boxFilterZ(const CTensor<T>& aTensor, CTensor<T>& 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 <class T>
+void recursiveSmoothX(CTensor<T>& aTensor, float aSigma) {
+ CVector<T> aVals1(aTensor.xSize());
+ CVector<T> 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 <class T>
+void recursiveSmoothY(CTensor<T>& aTensor, float aSigma) {
+ CVector<T> aVals1(aTensor.ySize());
+ CVector<T> 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 <class T>
+void recursiveSmoothZ(CTensor<T>& aTensor, float aSigma) {
+ CVector<T> aVals1(aTensor.zSize());
+ CVector<T> 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 <class T>
+inline void filter(CTensor4D<T>& aTensor, const CFilter<T>& aFilterX, const CFilter<T>& aFilterY, const CFilter<T>& aFilterZ, const CFilter<T>& aFilterA) {
+ CTensor4D<T> 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 <class T>
+inline void filter(CTensor4D<T>& aTensor, const CFilter<T>& aFilter, const int aDummy1, const int aDummy2, const int aDummy3) {
+ CTensor4D<T> tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize());
+ filter(aTensor,tempTensor,aFilter,1,1,1);
+ aTensor = tempTensor;
+}
+
+template <class T>
+void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const CFilter<T>& 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 <class T>
+inline void filter(CTensor4D<T>& aTensor, const int aDummy1, const CFilter<T>& aFilter, const int aDummy2, const int aDummy3) {
+ CTensor4D<T> tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize());
+ filter(aTensor,tempTensor,1,aFilter,1,1);
+ aTensor = tempTensor;
+}
+
+template <class T>
+void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const int aDummy1, const CFilter<T>& 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 <class T>
+inline void filter(CTensor4D<T>& aTensor, const int aDummy1, const int aDummy2, const CFilter<T>& aFilter, const int aDummy3) {
+ CTensor4D<T> tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize());
+ filter(aTensor,tempTensor,1,1,aFilter,1);
+ aTensor = tempTensor;
+}
+
+template <class T>
+void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const int aDummy1, const int aDummy2, const CFilter<T>& 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 <class T>
+inline void filter(CTensor4D<T>& aTensor, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter<T>& aFilter) {
+ CTensor4D<T> tempTensor(aTensor.xSize(),aTensor.ySize(),aTensor.zSize(),aTensor.aSize());
+ filter(aTensor,tempTensor,1,1,1,aFilter);
+ aTensor = tempTensor;
+}
+
+template <class T>
+void filter(const CTensor4D<T>& aTensor, CTensor4D<T>& aResult, const int aDummy1, const int aDummy2, const int aDummy3, const CFilter<T>& 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 <class T>
+void recursiveSmoothX(CTensor4D<T>& aTensor, float aSigma) {
+ CVector<T> aVals1(aTensor.xSize());
+ CVector<T> 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 <class T>
+void recursiveSmoothY(CTensor4D<T>& aTensor, float aSigma) {
+ CVector<T> aVals1(aTensor.ySize());
+ CVector<T> aVals2(aTensor.ySize());
+ CVector<T> 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 <class T>
+void recursiveSmoothZ(CTensor4D<T>& aTensor, float aSigma) {
+ CVector<T> aVals1(aTensor.zSize());
+ CVector<T> aVals2(aTensor.zSize());
+ CVector<T> 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 <class T>
+void recursiveSmoothA(CTensor4D<T>& aTensor, float aSigma) {
+ CVector<T> aVals1(aTensor.aSize());
+ CVector<T> aVals2(aTensor.aSize());
+ CVector<T> 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 <class T>
+void osher(CMatrix<T>& aData, int aIterations) {
+ CMatrix<T> 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 <class T>
+inline void osher(const CMatrix<T>& aData, CMatrix<T>& 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 <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <queue>
+#include <stack>
+#ifdef GNU_COMPILER
+ #include <strstream>
+#else
+ #include <sstream>
+#endif
+#include <CVector.h>
+
+template <class T>
+class CMatrix {
+public:
+ // standard constructor
+ inline CMatrix();
+ // constructor
+ inline CMatrix(const int aXSize, const int aYSize);
+ // copy constructor
+ CMatrix(const CMatrix<T>& 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<float>& 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<T>& 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<T>& 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<T>& aWarped, CMatrix<bool>& 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<T>& aWarped, CMatrix<bool>& aOutside, const CMatrix<float>& 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<T>& 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<T>& aLeft, CVector<T>& 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<T>& operator=(const T aValue);
+ // Copies the matrix aCopyFrom to this matrix (size of matrix might change)
+ CMatrix<T>& operator=(const CMatrix<T>& aCopyFrom);
+ // matrix sum
+ CMatrix<T>& operator+=(const CMatrix<T>& aMatrix);
+ // Adds a constant to the matrix
+ CMatrix<T>& operator+=(const T aValue);
+ // matrix difference
+ CMatrix<T>& operator-=(const CMatrix<T>& aMatrix);
+ // matrix product
+ CMatrix<T>& operator*=(const CMatrix<T>& aMatrix);
+ // Multiplication with a scalar
+ CMatrix<T>& operator*=(const T aValue);
+
+ // Comparison of two matrices
+ bool operator==(const CMatrix<T>& 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<T>& 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 <class T> CMatrix<T> abs(const CMatrix<T>& aMatrix);
+// Returns the tranposed matrix
+template <class T> CMatrix<T> trans(const CMatrix<T>& aMatrix);
+// matrix sum
+template <class T> CMatrix<T> operator+(const CMatrix<T>& aM1, const CMatrix<T>& aM2);
+// matrix difference
+template <class T> CMatrix<T> operator-(const CMatrix<T>& aM1, const CMatrix<T>& aM2);
+// matrix product
+template <class T> CMatrix<T> operator*(const CMatrix<T>& aM1, const CMatrix<T>& aM2);
+// Multiplication with a vector
+template <class T> CVector<T> operator*(const CMatrix<T>& aMatrix, const CVector<T>& aVector);
+// Multiplikation with a scalar
+template <class T> CMatrix<T> operator*(const CMatrix<T>& aMatrix, const T aValue);
+template <class T> inline CMatrix<T> operator*(const T aValue, const CMatrix<T>& aMatrix);
+// Provides basic output functionality (only appropriate for small matrices)
+template <class T> std::ostream& operator<<(std::ostream& aStream, const CMatrix<T>& 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 <class T>
+inline CMatrix<T>::CMatrix() {
+ mData = 0; mXSize = mYSize = 0;
+}
+
+// constructor
+template <class T>
+inline CMatrix<T>::CMatrix(const int aXSize, const int aYSize)
+ : mXSize(aXSize), mYSize(aYSize) {
+ mData = new T[aXSize*aYSize];
+}
+
+// copy constructor
+template <class T>
+CMatrix<T>::CMatrix(const CMatrix<T>& 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 <class T>
+CMatrix<T>::CMatrix(const int aXSize, const int aYSize, const T aFillValue)
+ : mXSize(aXSize), mYSize(aYSize) {
+ mData = new T[aXSize*aYSize];
+ fill(aFillValue);
+}
+
+// destructor
+template <class T>
+CMatrix<T>::~CMatrix() {
+ delete [] mData;
+}
+
+// setSize
+template <class T>
+void CMatrix<T>::setSize(int aXSize, int aYSize) {
+ if (mData != 0) delete[] mData;
+ mData = new T[aXSize*aYSize];
+ mXSize = aXSize;
+ mYSize = aYSize;
+}
+
+// downsampleBool
+template <class T>
+void CMatrix<T>::downsampleBool(int aNewXSize, int aNewYSize, float aThreshold) {
+ CMatrix<float> 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 <class T>
+void CMatrix<T>::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<float> 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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::downsample(int aNewXSize, int aNewYSize, CMatrix<float>& 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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::fill(const T aValue) {
+ int wholeSize = mXSize*mYSize;
+ for (register int i = 0; i < wholeSize; i++)
+ mData[i] = aValue;
+}
+
+// fillRect
+template <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::cut(CMatrix<T>& 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 <class T>
+void CMatrix<T>::paste(CMatrix<T>& 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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::applySimilarityTransform(CMatrix<T>& aWarped, CMatrix<bool>& 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 <class T>
+void CMatrix<T>::applyHomography(CMatrix<T>& aWarped, CMatrix<bool>& aOutside, const CMatrix<float>& 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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::connectedComponent (int x, int y) {
+ std::stack<CSegment> aStack;
+ #define PUSH(Y,XL,XR,DY) if (Y+(DY)>=0 && Y+(DY)<mYSize)\
+ {CSegment S; S.y = Y; S.xl = XL; S.xr = XR;S.dy = DY;aStack.push(S);}
+ #define POP(Y,XL,XR,DY) {CSegment& S = aStack.top(); Y = S.y+(DY = S.dy);XL = S.xl; XR = S.xr; aStack.pop();}
+ T aCompValue = operator()(x,y);
+ CMatrix<bool> 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 <class T>
+void CMatrix<T>::append(CMatrix<T>& 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 <class T>
+void CMatrix<T>::inv() {
+ if (mXSize != mYSize) throw ENonquadraticMatrix(mXSize,mYSize);
+ int* p = new int[mXSize];
+ T* hv = new T[mXSize];
+ CMatrix<T>& 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 <class T>
+void CMatrix<T>::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 <class T>
+float CMatrix<T>::scalar(CVector<T>& aLeft, CVector<T>& 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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::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 <class T>
+void CMatrix<T>::readBodoProjectionMatrix(const char* aFilename) {
+ readFromTXT(aFilename,false,4,3);
+}
+
+// operator ()
+template <class T>
+inline T& CMatrix<T>::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 <class T>
+inline CMatrix<T>& CMatrix<T>::operator=(const T aValue) {
+ fill(aValue);
+ return *this;
+}
+
+template <class T>
+CMatrix<T>& CMatrix<T>::operator=(const CMatrix<T>& 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 <class T>
+CMatrix<T>& CMatrix<T>::operator+=(const CMatrix<T>& 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 <class T>
+CMatrix<T>& CMatrix<T>::operator+=(const T aValue) {
+ int wholeSize = mXSize*mYSize;
+ for (int i = 0; i < wholeSize; i++)
+ mData[i] += aValue;
+ return *this;
+}
+
+// operator -=
+template <class T>
+CMatrix<T>& CMatrix<T>::operator-=(const CMatrix<T>& 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 <class T>
+CMatrix<T>& CMatrix<T>::operator*=(const CMatrix<T>& 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 <class T>
+CMatrix<T>& CMatrix<T>::operator*=(const T aValue) {
+ int wholeSize = mXSize*mYSize;
+ for (int i = 0; i < wholeSize; i++)
+ mData[i] *= aValue;
+ return *this;
+}
+
+// min
+template <class T>
+T CMatrix<T>::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 <class T>
+T CMatrix<T>::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 <class T>
+T CMatrix<T>::avg() const {
+ T aAvg = 0;
+ int aSize = mXSize*mYSize;
+ for (int i = 0; i < aSize; i++)
+ aAvg += mData[i];
+ return aAvg/aSize;
+}
+
+// xSize
+template <class T>
+inline int CMatrix<T>::xSize() const {
+ return mXSize;
+}
+
+// ySize
+template <class T>
+inline int CMatrix<T>::ySize() const {
+ return mYSize;
+}
+
+// size
+template <class T>
+inline int CMatrix<T>::size() const {
+ return mXSize*mYSize;
+}
+
+// getVector
+template <class T>
+void CMatrix<T>::getVector(CVector<T>& aVector, int ay) {
+ int aOffset = mXSize*ay;
+ for (int x = 0; x < mXSize; x++)
+ aVector(x) = mData[x+aOffset];
+}
+
+// data()
+template <class T>
+inline T* CMatrix<T>::data() const {
+ return mData;
+}
+
+// N O N - M E M B E R F U N C T I O N S --------------------------------------
+
+// abs
+template <class T>
+CMatrix<T> abs(const CMatrix<T>& aMatrix) {
+ CMatrix<T> 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 <class T>
+CMatrix<T> trans(const CMatrix<T>& aMatrix) {
+ CMatrix<T> 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 <class T>
+CMatrix<T> operator+(const CMatrix<T>& aM1, const CMatrix<T>& aM2) {
+ if ((aM1.xSize() != aM2.xSize()) || (aM1.ySize() != aM2.ySize()))
+ throw EIncompatibleMatrices(aM1.xSize(),aM1.ySize(),aM2.xSize(),aM2.ySize());
+ CMatrix<T> 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 <class T>
+CMatrix<T> operator-(const CMatrix<T>& aM1, const CMatrix<T>& aM2) {
+ if ((aM1.xSize() != aM2.xSize()) || (aM1.ySize() != aM2.ySize()))
+ throw EIncompatibleMatrices(aM1.xSize(),aM1.ySize(),aM2.xSize(),aM2.ySize());
+ CMatrix<T> 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 <class T>
+CMatrix<T> operator*(const CMatrix<T>& aM1, const CMatrix<T>& aM2) {
+ if (aM1.xSize() != aM2.ySize())
+ throw EIncompatibleMatrices(aM1.xSize(),aM1.ySize(),aM2.xSize(),aM2.ySize());
+ CMatrix<T> 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 <class T>
+CVector<T> operator*(const CMatrix<T>& aMatrix, const CVector<T>& aVector) {
+ if (aMatrix.xSize() != aVector.size())
+ throw EIncompatibleMatrices(aMatrix.xSize(),aMatrix.ySize(),1,aVector.size());
+ CVector<T> 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 <class T>
+CMatrix<T> operator*(const CMatrix<T>& aMatrix, const T aValue) {
+ CMatrix<T> 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 <class T>
+inline CMatrix<T> operator*(const T aValue, const CMatrix<T>& aMatrix) {
+ return aMatrix*aValue;
+}
+
+// operator <<
+template <class T>
+std::ostream& operator<<(std::ostream& aStream, const CMatrix<T>& 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 <class T> bool CMatrix<T>::operator==(const CMatrix<T>& aMatrix)
+{
+ if((*this).size()!=aMatrix.size())
+ return false;
+
+ for(int i=0; i<aMatrix.size();i++)
+ if(mData[i] != aMatrix.mData[i])
+ return false;
+ return true;
+}
+
+#endif
diff --git a/consistencyChecker/CTensor.h b/consistencyChecker/CTensor.h new file mode 100644 index 0000000..0f5af5c --- /dev/null +++ b/consistencyChecker/CTensor.h @@ -0,0 +1,1205 @@ +// CTensor
+// A three-dimensional array
+//
+// Author: Thomas Brox
+
+#ifndef CTENSOR_H
+#define CTENSOR_H
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <sstream>
+#include <CMatrix.h>
+#include <NMath.h>
+
+inline int int_min(int x, int& y) { return (x<y)?x:y; }
+inline int int_max(int x, int& y) { return (x<y)?y:x; }
+
+template <class T>
+class CTensor {
+public:
+ // standard constructor
+ inline CTensor();
+ // constructor
+ inline CTensor(const int aXSize, const int aYSize, const int aZSize);
+ // copy constructor
+ CTensor(const CTensor<T>& 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<float>& aConfidence);
+ void downsample(int aNewXSize, int aNewYSize, CTensor<float>& 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<T>& 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<T>& 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<T>& 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<T>& aWarped, CMatrix<bool>& 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<T>& aWarped, CMatrix<bool>& aOutside, const CMatrix<float>& 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<T> operator()(const float ax, const float ay) const;
+ // Fills the tensor with the value aValue (equivalent to fill())
+ inline CTensor<T>& operator=(const T aValue);
+ // Copies the tensor aCopyFrom to this tensor (size of tensor might change)
+ CTensor<T>& operator=(const CTensor<T>& aCopyFrom);
+ // Adds a tensor of same size
+ CTensor<T>& operator+=(const CTensor<T>& aMatrix);
+ // Adds a constant to the tensor
+ CTensor<T>& operator+=(const T aValue);
+ // Multiplication with a scalar
+ CTensor<T>& 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<T> getMatrix(const int az) const;
+ void getMatrix(CMatrix<T>& aMatrix, const int az) const;
+ // Copies the matrix components of aMatrix into the az layer of the tensor
+ void putMatrix(CMatrix<T>& 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 <class T> std::ostream& operator<<(std::ostream& aStream, const CTensor<T>& 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 <class T>
+inline CTensor<T>::CTensor() {
+ mData = 0;
+ mXSize = mYSize = mZSize = 0;
+}
+
+// constructor
+template <class T>
+inline CTensor<T>::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 <class T>
+CTensor<T>::CTensor(const CTensor<T>& 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 <class T>
+CTensor<T>::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 <class T>
+CTensor<T>::~CTensor() {
+ delete[] mData;
+}
+
+// setSize
+template <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::downsample(int aNewXSize, int aNewYSize) {
+ T* mData2 = new T[aNewXSize*aNewYSize*mZSize];
+ int aSize = aNewXSize*aNewYSize;
+ for (int z = 0; z < mZSize; z++) {
+ CMatrix<T> 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 <class T>
+void CTensor<T>::downsample(int aNewXSize, int aNewYSize, CMatrix<float>& aConfidence) {
+ T* mData2 = new T[aNewXSize*aNewYSize*mZSize];
+ int aSize = aNewXSize*aNewYSize;
+ for (int z = 0; z < mZSize; z++) {
+ CMatrix<T> 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 <class T>
+void CTensor<T>::downsample(int aNewXSize, int aNewYSize, CTensor<float>& aConfidence) {
+ T* mData2 = new T[aNewXSize*aNewYSize*mZSize];
+ int aSize = aNewXSize*aNewYSize;
+ CMatrix<float> aConf(mXSize,mYSize);
+ for (int z = 0; z < mZSize; z++) {
+ CMatrix<T> 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 <class T>
+void CTensor<T>::upsample(int aNewXSize, int aNewYSize) {
+ T* mData2 = new T[aNewXSize*aNewYSize*mZSize];
+ int aSize = aNewXSize*aNewYSize;
+ for (int z = 0; z < mZSize; z++) {
+ CMatrix<T> 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 <class T>
+void CTensor<T>::upsampleBilinear(int aNewXSize, int aNewYSize) {
+ T* mData2 = new T[aNewXSize*aNewYSize*mZSize];
+ int aSize = aNewXSize*aNewYSize;
+ for (int z = 0; z < mZSize; z++) {
+ CMatrix<T> 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 <class T>
+void CTensor<T>::fill(const T aValue) {
+ int wholeSize = mXSize*mYSize*mZSize;
+ for (register int i = 0; i < wholeSize; i++)
+ mData[i] = aValue;
+}
+
+// fillRect
+template <class T>
+void CTensor<T>::fillRect(const CVector<T>& 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 <class T>
+void CTensor<T>::cut(CTensor<T>& 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 <class T>
+void CTensor<T>::paste(CTensor<T>& 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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::normalizeEach(T aMin, T aMax, T aInitialMin, T aInitialMax) {
+ for (int k = 0; k < mZSize; k++)
+ normalize(aMin,aMax,k,aInitialMin,aInitialMax);
+}
+
+template <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::applySimilarityTransform(CTensor<T>& aWarped, CMatrix<bool>& 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 <class T>
+void CTensor<T>::applyHomography(CTensor<T>& aWarped, CMatrix<bool>& aOutside, const CMatrix<float>& 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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+void CTensor<T>::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 <class T>
+inline T& CTensor<T>::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 <class T>
+CVector<T> CTensor<T>::operator()(const float ax, const float ay) const {
+ CVector<T> 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 <class T>
+inline CTensor<T>& CTensor<T>::operator=(const T aValue) {
+ fill(aValue);
+ return *this;
+}
+
+template <class T>
+CTensor<T>& CTensor<T>::operator=(const CTensor<T>& 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 <class T>
+CTensor<T>& CTensor<T>::operator+=(const CTensor<T>& 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 <class T>
+CTensor<T>& CTensor<T>::operator+=(const T aValue) {
+ int wholeSize = mXSize*mYSize*mZSize;
+ for (int i = 0; i < wholeSize; i++)
+ mData[i] += aValue;
+ return *this;
+}
+
+// operator *=
+template <class T>
+CTensor<T>& CTensor<T>::operator*=(const T aValue) {
+ int wholeSize = mXSize*mYSize*mZSize;
+ for (int i = 0; i < wholeSize; i++)
+ mData[i] *= aValue;
+ return *this;
+}
+
+// min
+template <class T>
+T CTensor<T>::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 <class T>
+T CTensor<T>::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 <class T>
+T CTensor<T>::avg() const {
+ T aAvg = 0;
+ for (int z = 0; z < mZSize; z++)
+ aAvg += avg(z);
+ return aAvg/mZSize;
+}
+
+template <class T>
+T CTensor<T>::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 <class T>
+inline int CTensor<T>::xSize() const {
+ return mXSize;
+}
+
+// ySize
+template <class T>
+inline int CTensor<T>::ySize() const {
+ return mYSize;
+}
+
+// zSize
+template <class T>
+inline int CTensor<T>::zSize() const {
+ return mZSize;
+}
+
+// size
+template <class T>
+inline int CTensor<T>::size() const {
+ return mXSize*mYSize*mZSize;
+}
+
+// getMatrix
+template <class T>
+CMatrix<T> CTensor<T>::getMatrix(const int az) const {
+ CMatrix<T> 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 <class T>
+void CTensor<T>::getMatrix(CMatrix<T>& 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 <class T>
+void CTensor<T>::putMatrix(CMatrix<T>& 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 <class T>
+inline T* CTensor<T>::data() const {
+ return mData;
+}
+
+// N O N - M E M B E R F U N C T I O N S --------------------------------------
+
+// operator <<
+template <class T>
+std::ostream& operator<<(std::ostream& aStream, const CTensor<T>& 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 <vector> from the STL.
+// In this case you should include this file in a namespace.
+// Example:
+// namespace NTensor4D {
+// #include <CTensor4D.h>
+// }
+// After including other packages you can then write:
+// using namespace NTensor4D;
+
+#ifndef CTENSOR4D_H
+#define CTENSOR4D_H
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#ifdef GNU_COMPILER
+ #include <strstream>
+#else
+ #include <sstream>
+#endif
+#include "CTensor.h"
+
+template <class T>
+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<T>& 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<T>& 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<T> operator()(const float ax, const float ay, const int aa) const;
+ // Fills the tensor with the value aValue (equivalent to fill())
+ inline CTensor4D<T>& operator=(const T aValue);
+ // Copies the tensor aCopyFrom to this tensor (size of tensor might change)
+ CTensor4D<T>& operator=(const CTensor4D<T>& aCopyFrom);
+ // Multiplication with a scalar
+ CTensor4D<T>& operator*=(const T aValue);
+ // Component-wise addition
+ CTensor4D<T>& operator+=(const CTensor4D<T>& 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<T> getTensor3D(const int aa) const;
+ // Removes one dimension and returns the resulting 3D-tensor
+ void getTensor3D(CTensor<T>& 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<T>& aTensor, int aIndex, int aDim = 3);
+ // Removes two dimensions and returns the resulting matrix
+ void getMatrix(CMatrix<T>& aMatrix, int aZIndex, int aAIndex) const;
+ // Copies the components of a 3D-tensor in the aDimth layer of the 4D-tensor
+ void putMatrix(CMatrix<T>& 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 <class T> std::ostream& operator<<(std::ostream& aStream, const CTensor4D<T>& 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 <class T>
+inline CTensor4D<T>::CTensor4D() {
+ mData = 0; mXSize = 0; mYSize = 0; mZSize = 0; mASize = 0;
+}
+
+// constructor
+template <class T>
+inline CTensor4D<T>::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 <class T>
+CTensor4D<T>::CTensor4D(const CTensor4D<T>& 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 <class T>
+CTensor4D<T>::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 <class T>
+CTensor4D<T>::~CTensor4D() {
+ delete[] mData;
+}
+
+// setSize
+template <class T>
+void CTensor4D<T>::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 <class T>
+void CTensor4D<T>::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<T> 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 <class T>
+void CTensor4D<T>::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<T> 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 <class T>
+void CTensor4D<T>::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<T> 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 <class T>
+void CTensor4D<T>::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<T> 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 <class T>
+void CTensor4D<T>::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<T> 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 <class T>
+void CTensor4D<T>::fill(const T aValue) {
+ int wholeSize = mXSize*mYSize*mZSize*mASize;
+ for (register int i = 0; i < wholeSize; i++)
+ mData[i] = aValue;
+}
+
+// cut
+template <class T>
+void CTensor4D<T>::cut(CTensor4D<T>& 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 <class T>
+void CTensor4D<T>::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<float> 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<float> 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 <class T>
+void CTensor4D<T>::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 <class T>
+inline T& CTensor4D<T>::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 <class T>
+CVector<T> CTensor4D<T>::operator()(const float ax, const float ay, const int aa) const {
+ CVector<T> 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 <class T>
+inline CTensor4D<T>& CTensor4D<T>::operator=(const T aValue) {
+ fill(aValue);
+ return *this;
+}
+
+template <class T>
+CTensor4D<T>& CTensor4D<T>::operator=(const CTensor4D<T>& 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 <class T>
+CTensor4D<T>& CTensor4D<T>::operator*=(const T aValue) {
+ int wholeSize = mXSize*mYSize*mZSize*mASize;
+ for (int i = 0; i < wholeSize; i++)
+ mData[i] *= aValue;
+ return *this;
+}
+
+// operator +=
+template <class T>
+CTensor4D<T>& CTensor4D<T>::operator+=(const CTensor4D<T>& 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 <class T>
+inline int CTensor4D<T>::xSize() const {
+
+ return mXSize;
+}
+
+// ySize
+template <class T>
+inline int CTensor4D<T>::ySize() const {
+ return mYSize;
+}
+
+// zSize
+template <class T>
+inline int CTensor4D<T>::zSize() const {
+ return mZSize;
+}
+
+// aSize
+template <class T>
+inline int CTensor4D<T>::aSize() const {
+ return mASize;
+}
+
+// size
+template <class T>
+inline int CTensor4D<T>::size() const {
+ return mXSize*mYSize*mZSize*mASize;
+}
+
+// getTensor3D
+template <class T>
+CTensor<T> CTensor4D<T>::getTensor3D(const int aa) const {
+ CTensor<T> 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 <class T>
+void CTensor4D<T>::getTensor3D(CTensor<T>& 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 <class T>
+void CTensor4D<T>::putTensor3D(CTensor<T>& 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 <class T>
+void CTensor4D<T>::getMatrix(CMatrix<T>& 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 <class T>
+void CTensor4D<T>::putMatrix(CMatrix<T>& 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 <class T>
+inline T* CTensor4D<T>::data() const {
+ return mData;
+}
+
+// N O N - M E M B E R F U N C T I O N S --------------------------------------
+
+// operator <<
+template <class T>
+std::ostream& operator<<(std::ostream& aStream, const CTensor4D<T>& 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 <iostream>
+#include <fstream>
+
+template <class T> class CMatrix;
+template <class T> class CTensor;
+
+template <class T>
+class CVector {
+public:
+ // constructor
+ inline CVector();
+ // constructor
+ inline CVector(const int aSize);
+ // copy constructor
+ CVector(const CVector<T>& 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<T>& 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<T>& homogen();
+ // Remove the last component
+ inline void homogen_nD();
+ // Computes the cross product between this vector and aVector
+ void cross(CVector<T>& 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<T>& operator=(const T aValue);
+ // Copies a vector into this vector (size might change)
+ CVector<T>& operator=(const CVector<T>& aCopyFrom);
+ // Copies values from a matrix to the vector (size might change)
+ CVector<T>& operator=(const CMatrix<T>& aCopyFrom);
+ // Copies values from a tensor to the vector (size might change)
+ CVector<T>& operator=(const CTensor<T>& aCopyFrom);
+ // Adds another vector
+ CVector<T>& operator+=(const CVector<T>& aVector);
+ // Substracts another vector
+ CVector<T>& operator-=(const CVector<T>& aVector);
+ // Multiplies the vector with a scalar
+ CVector<T>& operator*=(const T aValue);
+ // Scalar product
+ T operator*=(const CVector<T>& aVector);
+ // Checks (non-)equivalence to another vector
+ bool operator==(const CVector<T>& aVector);
+ inline bool operator!=(const CVector<T>& 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 <class T> CVector<T> operator+(const CVector<T>& vec1, const CVector<T>& vec2);
+// Substracts two vectors
+template <class T> CVector<T> operator-(const CVector<T>& vec1, const CVector<T>& vec2);
+// Multiplies vector with a scalar
+template <class T> CVector<T> operator*(const CVector<T>& aVector, const T aValue);
+template <class T> CVector<T> operator*(const T aValue, const CVector<T>& aVector);
+// Computes the scalar product of two vectors
+template <class T> T operator*(const CVector<T>& vec1, const CVector<T>& vec2);
+// Computes cross product of two vectors
+template <class T> CVector<T> operator/(const CVector<T>& vec1, const CVector<T>& vec2);
+// Sends the vector to an output stream
+template <class T> std::ostream& operator<<(std::ostream& aStream, const CVector<T>& 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 <class T>
+inline CVector<T>::CVector() : mSize(0) {
+ mData = new T[0];
+}
+
+// constructor
+template <class T>
+inline CVector<T>::CVector(const int aSize)
+ : mSize(aSize) {
+ mData = new T[aSize];
+}
+
+// copy constructor
+template <class T>
+CVector<T>::CVector(const CVector<T>& aCopyFrom)
+ : mSize(aCopyFrom.mSize) {
+ mData = new T[mSize];
+ for (int i = 0; i < mSize; i++)
+ mData[i] = aCopyFrom.mData[i];
+}
+
+// constructor (from array)
+template <class T>
+CVector<T>::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 <class T>
+CVector<T>::CVector(const int aSize, const T aFillValue)
+ : mSize(aSize) {
+ mData = new T[aSize];
+ fill(aFillValue);
+}
+
+// destructor
+template <class T>
+CVector<T>::~CVector() {
+ delete[] mData;
+}
+
+// setSize
+template <class T>
+void CVector<T>::setSize(int aSize) {
+ if (mData != 0) delete[] mData;
+ mData = new T[aSize];
+ mSize = aSize;
+}
+
+// fill
+template <class T>
+void CVector<T>::fill(const T aValue) {
+ for (register int i = 0; i < mSize; i++)
+ mData[i] = aValue;
+}
+
+// append
+template <class T>
+void CVector<T>::append(CVector<T>& 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 <class T>
+void CVector<T>::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 <class T>
+void CVector<T>::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<class T>
+void CVector<T>::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<class T>
+void CVector<T>::writeToTXT(char* aFilename) {
+ std::ofstream aStream(aFilename);
+ for (int i = 0; i < mSize; i++)
+ aStream << mData[i] << std::endl;
+}
+
+// sum
+template <class T>
+T CVector<T>::sum() {
+ T val = mData[0];
+ for (int i = 1; i < mSize; i++)
+ val += mData[i];
+ return val;
+}
+
+// min
+template <class T>
+T CVector<T>::min() {
+ T bestValue = mData[0];
+ for (int i = 1; i < mSize; i++)
+ if (mData[i] < bestValue) bestValue = mData[i];
+ return bestValue;
+}
+
+// max
+template <class T>
+T CVector<T>::max() {
+ T bestValue = mData[0];
+ for (int i = 1; i < mSize; i++)
+ if (mData[i] > bestValue) bestValue = mData[i];
+ return bestValue;
+}
+
+// norm
+template <class T>
+T CVector<T>::norm() {
+ T aSum = 0.0;
+ for (int i = 0; i < mSize; i++)
+ aSum += mData[i]*mData[i];
+ return sqrt(aSum);
+}
+
+// homogen
+template <class T>
+CVector<T>& CVector<T>::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 <class T>
+inline void CVector<T>::homogen_nD() {
+ mSize--;
+}
+
+// cross
+template <class T>
+void CVector<T>::cross(CVector<T>& 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 <class T>
+inline T& CVector<T>::operator()(const int aIndex) const {
+ #ifdef _DEBUG
+ if (aIndex >= mSize || aIndex < 0)
+ throw EVectorRangeOverflow(aIndex);
+ #endif
+ return mData[aIndex];
+}
+
+// operator[]
+template <class T>
+inline T& CVector<T>::operator[](const int aIndex) const {
+ return operator()(aIndex);
+}
+
+// operator=
+template <class T>
+inline CVector<T>& CVector<T>::operator=(const T aValue) {
+ fill(aValue);
+ return *this;
+}
+
+template <class T>
+CVector<T>& CVector<T>::operator=(const CVector<T>& 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 <class T>
+CVector<T>& CVector<T>::operator=(const CMatrix<T>& 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 <class T>
+CVector<T>& CVector<T>::operator=(const CTensor<T>& 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 <class T>
+CVector<T>& CVector<T>::operator+=(const CVector<T>& 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 <class T>
+CVector<T>& CVector<T>::operator-=(const CVector<T>& 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 <class T>
+CVector<T>& CVector<T>::operator*=(const T aValue) {
+ for (int i = 0; i < mSize; i++)
+ mData[i] *= aValue;
+ return *this;
+}
+
+template <class T>
+T CVector<T>::operator*=(const CVector<T>& 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 <class T>
+bool CVector<T>::operator==(const CVector<T>& aVector) {
+ if (mSize != aVector.size()) return false;
+ int i = 0;
+ while (i < mSize && aVector(i) == mData[i])
+ i++;
+ return (i == mSize);
+}
+
+// operator !=
+template <class T>
+inline bool CVector<T>::operator!=(const CVector<T>& aVector) {
+ return !((*this)==aVector);
+}
+
+// size
+template <class T>
+inline int CVector<T>::size() const {
+ return mSize;
+}
+
+// N O N - M E M B E R F U N C T I O N S -------------------------------------
+
+// operator +
+template <class T>
+CVector<T> operator+(const CVector<T>& vec1, const CVector<T>& vec2) {
+ #ifdef _DEBUG
+ if (vec1.size() != vec2.size()) throw EVectorIncompatibleSize(vec1.size(),vec2.size());
+ #endif
+ CVector<T> result(vec1.size());
+ for (int i = 0; i < vec1.size(); i++)
+ result(i) = vec1[i]+vec2[i];
+ return result;
+}
+
+// operator -
+template <class T>
+CVector<T> operator-(const CVector<T>& vec1, const CVector<T>& vec2) {
+ #ifdef _DEBUG
+ if (vec1.size() != vec2.size()) throw EVectorIncompatibleSize(vec1.size(),vec2.size());
+ #endif
+ CVector<T> result(vec1.size());
+ for (int i = 0; i < vec1.size(); i++)
+ result(i) = vec1(i)-vec2(i);
+ return result;
+}
+
+// operator *
+template <class T>
+CVector<T> operator*(const T aValue, const CVector<T>& aVector) {
+ CVector<T> result(aVector.size());
+ for (int i = 0; i < aVector.size(); i++)
+ result(i) = aValue*aVector(i);
+ return result;
+}
+
+template <class T>
+CVector<T> operator*(const CVector<T>& aVector, const T aValue) {
+ return operator*(aValue,aVector);
+}
+
+template <class T>
+T operator*(const CVector<T>& vec1, const CVector<T>& 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 <class T>
+CVector<T> operator/(const CVector<T>& vec1, const CVector<T>& vec2) {
+ CVector<T> 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 <class T>
+std::ostream& operator<<(std::ostream& aStream, const CVector<T>& 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 <math.h>
+#include <stdlib.h>
+#include <NMath.h>
+
+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<float>& aMatrix, CVector<float>& aEigenvalues, CMatrix<float>& 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<float> 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<float>& aEigenvectors, const CVector<float>& aEigenvalues, CMatrix<float>& 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<float>& U, CMatrix<float>& S, CMatrix<float>& 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<float> 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<float>& U, const CMatrix<float>& S, const CMatrix<float>& 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<float>& A, CVector<float>& b) {
+ int i,j,k;
+ float sigma,s,beta,sum;
+ CVector<float> 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<float> leastSquares(CMatrix<float>& A, CVector<float>& b) {
+ CVector<float> 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<float>& A, int aReg) {
+ if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize());
+ CVector<float> eVals(A.xSize());
+ CMatrix<float> 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<float>& A) {
+ if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize());
+ CVector<float> 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<float>& L, CVector<float>& aIn, CVector<float>& 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<float>& L, CMatrix<float>& aIn, CMatrix<float>& aOut) {
+ CVector<float> 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<float>& L, CVector<float>& aIn, CVector<float>& 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<float>& L, CMatrix<float>& aIn, CMatrix<float>& aOut) {
+ CVector<float> 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<float>& L, CMatrix<float>& aInv) {
+ aInv = 0;
+ // Compute the inverse of L
+ CMatrix<float> 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<float>& A) {
+ CMatrix<float> Rx(4,4,0);
+ CMatrix<float> Ry(4,4,0);
+ CMatrix<float> 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<float>& T, CMatrix<float>& fRBM) {
+ T.setSize(6);
+ CMatrix<double> dRBM(4,4);
+ for (int i = 0; i < 16; i++)
+ dRBM.data()[i] = fRBM.data()[i];
+ CVector<double> omega;
+ double theta;
+ CVector<double> v;
+ CMatrix<double> 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<double> 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<double> 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<double> 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<double> 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 <math.h>
+#include <stdlib.h>
+#include <CVector.h>
+#include <CMatrix.h>
+
+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<float>& aMatrix, CVector<float>& aEigenvalues, CMatrix<float>& aEigenvectors, bool aOrdering = true);
+ // Computes the principal axis backtransformation
+ void PABacktransformation(const CMatrix<float>& aEigenVectors, const CVector<float>& aEigenValues, CMatrix<float>& 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<float>& U, CMatrix<float>& S, CMatrix<float>& V, bool aOrdering = true, int aIterations = 20);
+ // Reassembles A = USV^T, Result in U
+ void svdBack(CMatrix<float>& U, const CMatrix<float>& S, const CMatrix<float>& V);
+ // Applies the Householder method to A and b, i.e., A is transformed into an upper triangular matrix
+ void householder(CMatrix<float>& A, CVector<float>& b);
+ // Computes least squares solution of an overdetermined linear system Ax=b using the Householder method
+ CVector<float> leastSquares(CMatrix<float>& A, CVector<float>& b);
+ // Inverts a square matrix by eigenvalue decomposition,
+ // eigenvalues smaller than aReg are replaced by aReg
+ void invRegularized(CMatrix<float>& 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<float>& A);
+ // Solves L*aOut = aIn when L is a lower triangular matrix (e.g. result from cholesky)
+ void triangularSolve(CMatrix<float>& L, CVector<float>& aIn, CVector<float>& aOut);
+ void triangularSolve(CMatrix<float>& L, CMatrix<float>& aIn, CMatrix<float>& aOut);
+ // Solves L^T*aOut = aIn when L is a lower triangular matrix (e.g. result from cholesky)
+ void triangularSolveTransposed(CMatrix<float>& L, CVector<float>& aIn, CVector<float>& aOut);
+ void triangularSolveTransposed(CMatrix<float>& L, CMatrix<float>& aIn, CMatrix<float>& aOut);
+ // Computes the inverse of a matrix, given its cholesky decomposition L (lower triangle)
+ void choleskyInv(const CMatrix<float>& L, CMatrix<float>& 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<float>& A);
+ // Transforms a rigid body motion in matrix representation to a twist representation
+ void RBM2Twist(CVector<float> &T, CMatrix<float>& 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 <algorithm> +#include <assert.h> +#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<float>& 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<float>& flow1, const CTensor<float>& flow2, CMatrix<float>& reliable, int argc, char** args) { + int xSize = flow1.xSize(), ySize = flow1.ySize(); + int size = xSize * ySize; + CTensor<float> dx(xSize,ySize,2); + CTensor<float> dy(xSize,ySize,2); + CDerivative<float> derivative(3); + NFilter::filter(flow1,dx,derivative,1,1); + NFilter::filter(flow1,dy,1,derivative,1); + CMatrix<float> 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<float> 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<float> reliable(xSize, ySize, 255.0f); + checkConsistency(flow1, flow2, reliable, argc, args); + + if (SMOOTH_STRENGH > 0) { + CSmooth<float> 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 <filePattern> <outputFolder> [<startNumber> [<stepSize>]]" + 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 |