//! // Sliding window signal processing (and linear algebra toolkit). // // supported operations: //
//
• Savitzky-Golay smoothing. //
• computing a numerical derivative based of Savitzky-Golay smoothing. //
• required linear algebra support for SG smoothing using STL based // vector/matrix classes //
// // \brief Linear Algebra "Toolkit". // // system headers #include #include // for size_t #include // for fabs #include #include "sgsmooth.h" //! default convergence static const double TINY_FLOAT = 1.0e-300; //! comfortable array of doubles typedef std::vector float_vect; //! comfortable array of ints; typedef std::vector int_vect; // savitzky golay smoothing. static float_vect sg_smooth(const float_vect &v, const int w, const int deg); //! numerical derivative based on savitzky golay smoothing. static float_vect sg_derivative(const float_vect &v, const int w, const int deg, const double h=1.0); // c-callable interface to for tcl plugin code extern "C" double *calc_sgsmooth(const int ndat, double *input, const int window, const int order) { float_vect in(ndat); int i; #if defined(_OPENMP) #pragma omp parallel for private(i) schedule(static) #endif for (i=0; i < ndat; ++i) { in[i] = input[i]; } float_vect out = sg_smooth(in, window, order); #if defined(_OPENMP) #pragma omp parallel for private(i) schedule(static) #endif for (i=0; i < ndat; ++i) { input[i] = out[i]; } return input; } extern "C" double *calc_sgsderiv(const int ndat, double *input, const int window, const int order, const double delta) { float_vect in(ndat); int i; #if defined(_OPENMP) #pragma omp parallel for private(i) schedule(static) #endif for (i=0; i < ndat; ++i) { in[i] = input[i]; } float_vect out = sg_derivative(in, window, order, delta); #if defined(_OPENMP) #pragma omp parallel for private(i) schedule(static) #endif for (i=0; i < ndat; ++i) { input[i] = out[i]; } return input; } /*! matrix class. * * This is a matrix class derived from a vector of float_vects. Note that * the matrix elements indexed [row][column] with indices starting at 0 (c * style). Also note that because of its design looping through rows should * be faster than looping through columns. * * \brief two dimensional floating point array */ class float_mat : public std::vector { private: //! disable the default constructor explicit float_mat() {}; //! disable assignment operator until it is implemented. float_mat &operator =(const float_mat &) { return *this; }; public: //! constructor with sizes float_mat(const size_t rows, const size_t cols, const double def=0.0); //! copy constructor for matrix float_mat(const float_mat &m); //! copy constructor for vector float_mat(const float_vect &v); //! use default destructor // ~float_mat() {}; //! get size size_t nr_rows(void) const { return size(); }; //! get size size_t nr_cols(void) const { return front().size(); }; }; // constructor with sizes float_mat::float_mat(const size_t rows,const size_t cols,const double defval) : std::vector(rows) { int i; for (i = 0; i < rows; ++i) { (*this)[i].resize(cols, defval); } if ((rows < 1) || (cols < 1)) { char buffer[1024]; sprintf(buffer, "cannot build matrix with %d rows and %d columns\n", rows, cols); sgs_error(buffer); } } // copy constructor for matrix float_mat::float_mat(const float_mat &m) : std::vector(m.size()) { float_mat::iterator inew = begin(); float_mat::const_iterator iold = m.begin(); for (/* empty */; iold < m.end(); ++inew, ++iold) { const size_t oldsz = iold->size(); inew->resize(oldsz); const float_vect oldvec(*iold); *inew = oldvec; } } // copy constructor for vector float_mat::float_mat(const float_vect &v) : std::vector(1) { const size_t oldsz = v.size(); front().resize(oldsz); front() = v; } ////////////////////// // Helper functions // ////////////////////// //! permute() orders the rows of A to match the integers in the index array. void permute(float_mat &A, int_vect &idx) { int_vect i(idx.size()); int j,k; for (j = 0; j < A.nr_rows(); ++j) { i[j] = j; } // loop over permuted indices for (j = 0; j < A.nr_rows(); ++j) { if (i[j] != idx[j]) { // search only the remaining indices for (k = j+1; k < A.nr_rows(); ++k) { if (i[k] ==idx[j]) { std::swap(A[j],A[k]); // swap the rows and i[k] = i[j]; // the elements of i[j] = idx[j]; // the ordered index. break; // next j } } } } } /*! \brief Implicit partial pivoting. * * The function looks for pivot element only in rows below the current * element, A[idx[row]][column], then swaps that row with the current one in * the index map. The algorithm is for implicit pivoting (i.e., the pivot is * chosen as if the max coefficient in each row is set to 1) based on the * scaling information in the vector scale. The map of swapped indices is * recorded in swp. The return value is +1 or -1 depending on whether the * number of row swaps was even or odd respectively. */ static int partial_pivot(float_mat &A, const size_t row, const size_t col, float_vect &scale, int_vect &idx, double tol) { if (tol <= 0.0) tol = TINY_FLOAT; int swapNum = 1; // default pivot is the current position, [row,col] int pivot = row; double piv_elem = fabs(A[idx[row]][col]) * scale[idx[row]]; // loop over possible pivots below current int j; for (j = row + 1; j < A.nr_rows(); ++j) { const double tmp = fabs(A[idx[j]][col]) * scale[idx[j]]; // if this elem is larger, then it becomes the pivot if (tmp > piv_elem) { pivot = j; piv_elem = tmp; } } #if 0 if(piv_elem < tol) { sgs_error("partial_pivot(): Zero pivot encountered.\n") #endif if(pivot > row) { // bring the pivot to the diagonal j = idx[row]; // reorder swap array idx[row] = idx[pivot]; idx[pivot] = j; swapNum = -swapNum; // keeping track of odd or even swap } return swapNum; } /*! \brief Perform backward substitution. * * Solves the system of equations A*b=a, ASSUMING that A is upper * triangular. If diag==1, then the diagonal elements are additionally * assumed to be 1. Note that the lower triangular elements are never * checked, so this function is valid to use after a LU-decomposition in * place. A is not modified, and the solution, b, is returned in a. */ static void lu_backsubst(float_mat &A, float_mat &a, bool diag=false) { int r,c,k; for (r = (A.nr_rows() - 1); r >= 0; --r) { for (c = (A.nr_cols() - 1); c > r; --c) { for (k = 0; k < A.nr_cols(); ++k) { a[r][k] -= A[r][c] * a[c][k]; } } if(!diag) { for (k = 0; k < A.nr_cols(); ++k) { a[r][k] /= A[r][r]; } } } } /*! \brief Perform forward substitution. * * Solves the system of equations A*b=a, ASSUMING that A is lower * triangular. If diag==1, then the diagonal elements are additionally * assumed to be 1. Note that the upper triangular elements are never * checked, so this function is valid to use after a LU-decomposition in * place. A is not modified, and the solution, b, is returned in a. */ static void lu_forwsubst(float_mat &A, float_mat &a, bool diag=true) { int r,k,c; for (r = 0;r < A.nr_rows(); ++r) { for(c = 0; c < r; ++c) { for (k = 0; k < A.nr_cols(); ++k) { a[r][k] -= A[r][c] * a[c][k]; } } if(!diag) { for (k = 0; k < A.nr_cols(); ++k) { a[r][k] /= A[r][r]; } } } } /*! \brief Performs LU factorization in place. * * This is Crout's algorithm (cf., Num. Rec. in C, Section 2.3). The map of * swapped indeces is recorded in idx. The return value is +1 or -1 * depending on whether the number of row swaps was even or odd * respectively. idx must be preinitialized to a valid set of indices * (e.g., {1,2, ... ,A.nr_rows()}). */ static int lu_factorize(float_mat &A, int_vect &idx, double tol=TINY_FLOAT) { if ( tol <= 0.0) tol = TINY_FLOAT; if ((A.nr_rows() == 0) || (A.nr_rows() != A.nr_cols())) { sgs_error("lu_factorize(): cannot handle empty " "or nonsquare matrices.\n"); return 0; } float_vect scale(A.nr_rows()); // implicit pivot scaling int i,j; for (i = 0; i < A.nr_rows(); ++i) { double maxval = 0.0; for (j = 0; j < A.nr_cols(); ++j) { if (fabs(A[i][j]) > maxval) maxval = fabs(A[i][j]); } if (maxval == 0.0) { sgs_error("lu_factorize(): zero pivot found.\n"); return 0; } scale[i] = 1.0 / maxval; } int swapNum = 1; int c,r; for (c = 0; c < A.nr_cols() ; ++c) { // loop over columns swapNum *= partial_pivot(A, c, c, scale, idx, tol); // bring pivot to diagonal for(r = 0; r < A.nr_rows(); ++r) { // loop over rows int lim = (r < c) ? r : c; for (j = 0; j < lim; ++j) { A[idx[r]][c] -= A[idx[r]][j] * A[idx[j]][c]; } if (r > c) A[idx[r]][c] /= A[idx[c]][c]; } } permute(A,idx); return swapNum; } /*! \brief Solve a system of linear equations. * Solves the inhomogeneous matrix problem with lu-decomposition. Note that * inversion may be accomplished by setting a to the identity_matrix. */ static float_mat lin_solve(const float_mat &A, const float_mat &a, double tol=TINY_FLOAT) { float_mat B(A); float_mat b(a); int_vect idx(B.nr_rows()); int j; for (j = 0; j < B.nr_rows(); ++j) { idx[j] = j; // init row swap label array } lu_factorize(B,idx,tol); // get the lu-decomp. permute(b,idx); // sort the inhomogeneity to match the lu-decomp lu_forwsubst(B,b); // solve the forward problem lu_backsubst(B,b); // solve the backward problem return b; } /////////////////////// // related functions // /////////////////////// //! Returns the inverse of a matrix using LU-decomposition. static float_mat invert(const float_mat &A) { const int n = A.size(); float_mat E(n, n, 0.0); float_mat B(A); int i; for (i = 0; i < n; ++i) { E[i][i] = 1.0; } return lin_solve(B, E); } //! returns the transposed matrix. static float_mat transpose(const float_mat &a) { float_mat res(a.nr_cols(), a.nr_rows()); int i,j; for (i = 0; i < a.nr_rows(); ++i) { for (j = 0; j < a.nr_cols(); ++j) { res[j][i] = a[i][j]; } } return res; } //! matrix multiplication. float_mat operator *(const float_mat &a, const float_mat &b) { float_mat res(a.nr_rows(), b.nr_cols()); if (a.nr_cols() != b.nr_rows()) { sgs_error("incompatible matrices in multiplication\n"); return res; } int i,j,k; for (i = 0; i < a.nr_rows(); ++i) { for (j = 0; j < b.nr_cols(); ++j) { double sum(0.0); for (k = 0; k < a.nr_cols(); ++k) { sum += a[i][k] * b[k][j]; } res[i][j] = sum; } } return res; } //! calculate savitzky golay coefficients. static float_vect sg_coeff(const float_vect &b, const size_t deg) { const size_t rows(b.size()); const size_t cols(deg + 1); float_mat A(rows, cols); float_vect res(rows); // generate input matrix for least squares fit int i,j; for (i = 0; i < rows; ++i) { for (j = 0; j < cols; ++j) { A[i][j] = pow(double(i), double(j)); } } float_mat c(invert(transpose(A) * A) * (transpose(A) * transpose(b))); for (i = 0; i < b.size(); ++i) { res[i] = c[0][0]; for (j = 1; j <= deg; ++j) { res[i] += c[j][0] * pow(double(i), double(j)); } } return res; } /*! \brief savitzky golay smoothing. * * This method means fitting a polynome of degree 'deg' to a sliding window * of width 2w+1 throughout the data. The needed coefficients are * generated dynamically by doing a least squares fit on a "symmetric" unit * vector of size 2w+1, e.g. for w=2 b=(0,0,1,0,0). evaluating the polynome * yields the sg-coefficients. at the border non symmectric vectors b are * used. */ float_vect sg_smooth(const float_vect &v, const int width, const int deg) { float_vect res(v.size(), 0.0); if ((width < 1) || (deg < 1) || (v.size() < (2 * width + 2))) { sgs_error("sgsmooth: parameter error.\n"); return res; } const int window = 2 * width + 1; const int endidx = v.size() - 1; // handle border cases first because we need different coefficients int i,j; #if defined(_OPENMP) #pragma omp parallel for private(i,j) schedule(static) #endif for (i = 0; i < width; ++i) { float_vect b1(window, 0.0); b1[i] = 1.0; const float_vect c1(sg_coeff(b1, deg)); for (j = 0; j < window; ++j) { res[i] += c1[j] * v[j]; res[endidx - i] += c1[j] * v[endidx - j]; } } // now loop over rest of data. reusing the "symmetric" coefficients. float_vect b2(window, 0.0); b2[width] = 1.0; const float_vect c2(sg_coeff(b2, deg)); #if defined(_OPENMP) #pragma omp parallel for private(i,j) schedule(static) #endif for (i = 0; i <= (v.size() - window); ++i) { for (j = 0; j < window; ++j) { res[i + width] += c2[j] * v[i + j]; } } return res; } /*! least squares fit a polynome of degree 'deg' to data in 'b'. * then calculate the first derivative and return it. */ static float_vect lsqr_fprime(const float_vect &b, const int deg) { const int rows(b.size()); const int cols(deg + 1); float_mat A(rows, cols); float_vect res(rows); // generate input matrix for least squares fit int i,j; for (i = 0; i < rows; ++i) { for (j = 0; j < cols; ++j) { A[i][j] = pow(double(i), double(j)); } } float_mat c(invert(transpose(A) * A) * (transpose(A) * transpose(b))); for (i = 0; i < b.size(); ++i) { res[i] = c[1][0]; for (j = 1; j < deg; ++j) { res[i] += c[j + 1][0] * double(j+1) * pow(double(i), double(j)); } } return res; } /*! \brief savitzky golay smoothed numerical derivative. * * This method means fitting a polynome of degree 'deg' to a sliding window * of width 2w+1 throughout the data. * * In contrast to the sg_smooth function we do a brute force attempt by * always fitting the data to a polynome of degree 'deg' and using the * result. */ float_vect sg_derivative(const float_vect &v, const int width, const int deg, const double h) { float_vect res(v.size(), 0.0); if ((width < 1) || (deg < 1) || (v.size() < (2 * width + 2))) { sgs_error("sgsderiv: parameter error.\n"); return res; } const int window = 2 * width + 1; // handle border cases first because we do not repeat the fit // lower part float_vect b(window, 0.0); int i,j; for (i = 0; i < window; ++i) { b[i] = v[i] / h; } const float_vect c(lsqr_fprime(b, deg)); for (j = 0; j <= width; ++j) { res[j] = c[j]; } // upper part. direction of fit is reversed for (i = 0; i < window; ++i) { b[i] = v[v.size() - 1 - i] / h; } const float_vect d(lsqr_fprime(b, deg)); for (i = 0; i <= width; ++i) { res[v.size() - 1 - i] = -d[i]; } // now loop over rest of data. wasting a lot of least squares calcs // since we only use the middle value. #if defined(_OPENMP) #pragma omp parallel for private(i,j) schedule(static) #endif for (i = 1; i < (v.size() - window); ++i) { for (j = 0; j < window; ++j) { b[j] = v[i + j] / h; } res[i + width] = lsqr_fprime(b, deg)[width]; } return res; } // Local Variables: // mode: c++ // c-basic-offset: 4 // fill-column: 76 // indent-tabs-mode: nil // End: