// This routine evaluates the Lagrange interpolating polynomial, // defined over a set of data points (x_i,y_i), i=0,...,n, at a point z. // // Usage: p = lagrange(x, y, z); // // inputs: x Mat vector of length n+1, containing the interpolation nodes // y Mat vector of length n+1, containing the interpolation data // z double location to evaluate polynomial // outputs: p value of p(z) // double lagrange2D(Mat &x, Mat &y, Mat&z, double a,double b) { // check input arguments (lengths of x and y) if (x.Rows()*x.Cols() != y.Rows()*y.Cols()) { cerr << "lagrange2d error: x and y have different lengths!\n"; return 0.0; } // get m int m = x.Rows()*x.Cols() - 1; // get n int n = y.Rows()*y.Cols() - 1; // evaluate p double p = 0.0; // initialize result for(int i=0;i<=m;i++){ for (int j=0; j<=n; j++) // loop over data values p += z(i,j)*lagrange_basis2D(x, i, a)*lagrange_basis2D(y, j, b); // update result with next term } // return final result return p; } // end of function
Mat trans(const Mat &m) { Int i,j; Mat result(m.Cols(), m.Rows()); for (i = 0; i < m.Rows(); i++) for (j = 0; j < m.Cols(); j++) result.Elt(j,i) = m.Elt(i,j); return(result); }
Mat operator * (const Mat &m, const Mat &n) { Assert(m.Cols() == n.Rows(), "(Mat::*m) matrix cols don't match"); Mat result(m.Rows(), n.Cols()); Int i; for (i = 0; i < m.Rows(); i++) result[i] = m[i] * n; return(result); }
void ColorSpace::BGRAtoRGB(const Mat& color1, Mat& color2) { color2.Create (color1.Rows(), color1.Cols(), MAT_TBYTE3); int d=0; uchar* psrc=color1.data.ptr[0]; uchar* pdst=color2.data.ptr[0]; int datalen=color1.Rows()*color1.Cols()*3; for (int i=0; i<datalen; i+=3, d+=4) { pdst[i]=psrc[d+2]; pdst[i+1]=psrc[d+1]; pdst[i+2]=psrc[d]; } }
// dot-product of x and y double Dot(Mat &x, Mat &y) { // check that array sizes match if (y.Rows() != x.Rows() || y.Cols() != x.Cols()) { fprintf(stderr,"Dot error, matrix size mismatch\n"); fprintf(stderr," Mat 1 is %li x %li, Mat 2 is %li x %li\n", x.Rows(), x.Cols(), y.Rows(), y.Cols()); return 0.0; } // perform operation and return double sum=0.0; for (long int j=0; j<x.Cols(); j++) for (long int i=0; i<x.Rows(); i++) sum += x(i,j)*y(i,j); return sum; }
void ColorSpace::GraytoRGB (const Mat& gray, Mat& color) { if (gray.Channels()==3) { color.Create (gray, TRUE); return; } if (gray.SizeObject() != color.SizeObject() || color.Channels()==3) { color.Release(); color.Create (gray.SizeObject(), (TYPE)CVLIB_MAKETYPE(gray.Type(), 3)); } int nH = color.Rows(), nW = color.Cols(); int elemsize=CVLIB_ELEM_SIZE(gray.Type()); for (int i = 0; i < nH; i ++) { uchar* pcolor=color.data.ptr[i]; uchar* pgray=gray.data.ptr[i]; for (int k=0; k<nW; k++) { memcpy (&pcolor[3*k*elemsize], &pgray[k*elemsize], elemsize); memcpy (&pcolor[(3*k+1)*elemsize], &pgray[k*elemsize], elemsize); memcpy (&pcolor[(3*k+2)*elemsize], &pgray[k*elemsize], elemsize); } } }
// Utility function to evaluate a given Lagrange basis function at a point. // // Usage: l = lagrange_basis(x, i, z); // // inputs: x Mat vector of length n+1, containing the interpolation nodes // i integer indicating which Lagrange basis function to evaluate // z double location to evaluate basis function // outputs: p value of l(z) // double lagrange_basis(Mat &x, int i, double z) { double l = 1.0; // initialize basis function double *xd = x.get_data(); // access data array (for increased speed) for (int j=0; j<x.Rows()*x.Cols(); j++) if (j != i) l *= (z - xd[j]) / (xd[i] - xd[j]); return l; }
// copy constructor Mat::Mat(const Mat& A) { nrows = A.Rows(); ncols = A.Cols(); data = new double[ncols*nrows]; own_data = true; for (long int i=0; i<nrows*ncols; i++) data[i] = A.data[i]; }
// compute the dot-product of two compatible vectors x and y double Dot(Mat &x, Mat &y) { // check that array sizes match if (y.Rows() != x.Rows() || y.Cols() != x.Cols()) { cerr << "Dot error, matrix size mismatch\n"; cerr << " Mat 1 is " << x.Rows() << " x " << x.Cols() << ", Mat 2 is " << y.Rows() << " x " << y.Cols() << endl; return 0.0; } // perform operation and return double sum=0.0; for (long int j=0; j<x.Cols(); j++) for (long int i=0; i<x.Rows(); i++) sum += x(i,j)*y(i,j); return sum; }
Mat operator - (const Mat &m) { Mat result(m.Rows(), m.Cols()); Int i; for (i = 0; i < m.Rows(); i++) result[i] = -m[i]; return(result); }
Mat operator / (const Mat &m, Real s) { Int i; Mat result(m.Rows(), m.Cols()); for (i = 0; i < m.Rows(); i++) result[i] = m[i] / s; return(result); }
// solves a linear system A*x = b, returning x as a new Mat Mat Solve(Mat &A, Mat &b) { // check that matrix sizes match if (A.Rows() != A.Rows() || A.Rows() != A.Cols() || b.Cols() != 1) { fprintf(stderr,"Solve error, illegal matrix/vector dimensions\n"); fprintf(stderr," Mat is %li x %li, rhs is %li x %li\n", A.Rows(), A.Cols(), b.Rows(), b.Cols()); Mat *x = new Mat(0,0); return *x; } // create new Mat for output Mat *x = new Mat(A.Rows(),1); // call existing Solve routine for computations if (Solve(A, *x, b) != 0) fprintf(stderr,"Solve Warning: error in Solve call\n"); // return result return *x; }
// performs backwards substitution on the linear system U*x = b, returning x as a new Mat; // leaves U and b untouched Mat BackSub(Mat &U, Mat &b) { // check that matrix sizes match if (U.Rows() != b.Rows() || U.Rows() != U.Cols() || b.Cols() != 1) { fprintf(stderr,"BackSub error, illegal matrix/vector dimensions\n"); fprintf(stderr," Mat is %li x %li, rhs is %li x %li\n", U.Rows(), U.Cols(), b.Rows(), b.Cols()); Mat *x = new Mat(0,0); return *x; } // create new Mat for output Mat *x = new Mat(U.Rows(),1); // call existing BackSub routine for computations if (BackSub(U, *x, b) != 0) fprintf(stderr,"BackSub Warning: error in BackSub call\n"); // return result return *x; }
// This routine evaluates the Lagrange interpolating polynomial, // defined over a set of data points (x_i,y_i), i=0,...,n, at a point z. // // Usage: p = lagrange(x, y, z); // // inputs: x Mat vector of length n+1, containing the interpolation nodes // y Mat vector of length n+1, containing the interpolation data // z double location to evaluate polynomial // outputs: p value of p(z) // double lagrange(Mat &x, Mat &y, double z) { // check input arguments (lengths of x and y) if (x.Rows()*x.Cols() != y.Rows()*y.Cols()) { cerr << "lagrange error: x and y have different lengths!\n"; return 0.0; } // get n int n = x.Rows()*x.Cols() - 1; // evaluate p double p = 0.0; // initialize result for (int i=0; i<=n; i++) // loop over data values p += y(i)*lagrange_basis(x, i, z); // update result with next term // return final result return p; } // end of function
// performs backwards substitution on the linear system U*x = b, returning x as a new Mat; // leaves U and b untouched Mat BackSub(Mat &U, Mat &b) { // check that matrix sizes match if (U.Rows() != b.Rows() || U.Rows() != U.Cols() || b.Cols() != 1) { cerr << "BackSub error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << U.Rows() << " x " << U.Cols() << ", rhs is " << b.Rows() << " x " << b.Cols() << endl; Mat *x = new Mat(0,0); return *x; } // create new Mat for output Mat *x = new Mat(U.Rows(),1); // call existing BackSub routine for computations if (BackSub(U, *x, b) != 0) cerr << "BackSub Warning: error in BackSub call\n"; // return result return *x; }
Mat operator - (const Mat &m, const Mat &n) { Assert(n.Rows() == m.Rows(), "(Mat::-) matrix rows don't match"); Mat result(m.Rows(), m.Cols()); Int i; for (i = 0; i < m.Rows(); i++) result[i] = m[i] - n[i]; return(result); }
// solves a linear system A*x = b, returning x as a new Mat Mat Solve(Mat &A, Mat &b) { // check that matrix sizes match if (A.Rows() != A.Rows() || A.Rows() != A.Cols() || b.Cols() != 1) { cerr << "Solve error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << A.Rows() << " x " << A.Cols() << ", rhs is " << b.Rows() << " x " << b.Cols() << endl; Mat *x = new Mat(0,0); return *x; } // create new Mat for output Mat *x = new Mat(A.Rows(),1); // call existing Solve routine for computations if (Solve(A, *x, b) != 0) cerr << "Solve: error in in-place Solve call\n"; // return result return *x; }
// performs forwards substitution on the linear system L*x = b, returning x as a new Mat; // leaves L and b untouched Mat FwdSub(Mat &L, Mat &b) { // check that matrix sizes match if (L.Rows() != b.Rows() || L.Rows() != L.Cols() || b.Cols() != 1) { cerr << "FwdSub error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << L.Rows() << " x " << L.Cols() << ", rhs is " << b.Rows() << " x " << b.Cols() << endl; Mat *x = new Mat(0,0); return *x; } // create new Mat for output Mat *x = new Mat(L.Rows(),1); // call existing FwdSub routine for computations if (FwdSub(L, *x, b) != 0) cerr << "FwdSub Warning: error in FwdSub call\n"; // return result return *x; }
Vec operator * (const Vec &v, const Mat &m) // v * m { Assert(v.Elts() == m.Rows(), "(Mat::v*m) vector/matrix sizes don't match"); Vec result(m.Cols(), vl_zero); Int i; for (i = 0; i < m.Rows(); i++) result += m[i] * v[i]; return(result); }
Vec operator * (const Mat &m, const Vec &v) { Assert(m.Cols() == v.Elts(), "(Mat::*v) matrix and vector sizes don't match"); Int i; Vec result(m.Rows()); for (i = 0; i < m.Rows(); i++) result[i] = dot(v, m[i]); return(result); }
// performs backwards substitution on the linear system U*x = b, filling in the input Mat x int BackSub(Mat &U, Mat &x, Mat &b) { // check that matrix sizes match if (U.Rows() != b.Rows() || U.Rows() != U.Cols() || b.Cols() != 1 || x.Rows() != U.Rows() || x.Cols() != 1) { fprintf(stderr,"BackSub error, illegal matrix/vector dimensions\n"); fprintf(stderr," Mat is %li x %li, sol is %li x %li, rhs is %li x %li\n", U.Rows(), U.Cols(), x.Rows(), x.Cols(), b.Rows(), b.Cols()); return 1; } // copy b into x x = b; // perform column-oriented Backwards Substitution algorithm for (long int j=U.Rows()-1; j>=0; j--) { // solve for this row of solution x(j) = x(j)/U(j,j); // update all subsequent rhs for (long int i=0; i<j; i++) x(i) -= U(i,j)*x(j); } // return success return 0; }
void ColorSpace::RGBtoGray (const Mat& colorImg, Mat& mgray) { if (colorImg.Channels()==1) { mgray.Create (colorImg, TRUE); return; } if (mgray.SizeObject() != colorImg.SizeObject() || mgray.Channels()==3) { mgray.Release(); mgray.Create (colorImg.SizeObject(), colorImg.Type()); } int nH = colorImg.Rows(), nW = colorImg.Cols(); for (int i = 0; i < nH; i ++) { uchar* pcolor=colorImg.data.ptr[i]; uchar* pgray=mgray.data.ptr[i]; for (int j = 0, k=0; k<nW; j+=3, k++) pgray[k] = (uchar)((299 * pcolor[j] + 587 * pcolor[j+1] + 114 * pcolor[j+2]) / 1000); } }
// performs backwards substitution on the linear system U*x = b, filling in the input Mat x int BackSub(Mat &Umat, Mat &xvec, Mat &bvec) { // check that matrix sizes match if (Umat.Rows() != bvec.Rows() || Umat.Rows() != Umat.Cols() || bvec.Cols() != 1 || xvec.Rows() != Umat.Rows() || xvec.Cols() != 1) { cerr << "BackSub error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << Umat.Rows() << " x " << Umat.Cols() << ", rhs is " << bvec.Rows() << " x " << bvec.Cols() << ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl; return 1; } // get the matrix size long int n = Umat.Rows(); // access the data arrays double *U = Umat.get_data(); double *x = xvec.get_data(); double *b = bvec.get_data(); // copy b into x xvec = bvec; // analyze matrix for typical nonzero magnitude double Umax = Umat.MaxNorm(); // perform column-oriented Backwards Substitution algorithm for (long int j=n-1; j>=0; j--) { // check for nonzero matrix diagonal if (fabs(U[IDX(j,j,n)]) < STOL*Umax) { cerr << "BackSub error: numerically singular matrix!\n"; return 1; } // solve for this row of solution x[j] /= U[IDX(j,j,n)]; // update all remaining rhs for (long int i=0; i<j; i++) x[i] -= U[IDX(i,j,n)]*x[j]; } // return success return 0; }
// performs forwards substitution on the linear system L*x = b, filling in the input Mat x int FwdSub(Mat &Lmat, Mat &xvec, Mat &bvec) { // check that matrix sizes match if (Lmat.Rows() != bvec.Rows() || Lmat.Rows() != Lmat.Cols() || bvec.Cols() != 1 || xvec.Rows() != Lmat.Rows() || xvec.Cols() != 1) { cerr << "FwdSub error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << Lmat.Rows() << " x " << Lmat.Cols() << ", rhs is " << bvec.Rows() << " x " << bvec.Cols() << ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl; return 1; } // get the matrix size long int n = Lmat.Rows(); // access the data arrays double *L = Lmat.get_data(); double *x = xvec.get_data(); double *b = bvec.get_data(); // copy b into x xvec = bvec; // analyze matrix for typical nonzero magnitude double Lmax = Lmat.MaxNorm(); // perform column-oriented Forwards Substitution algorithm for (long int j=0; j<n; j++) { // check for nonzero matrix diagonal if (fabs(L[IDX(j,j,n)]) < STOL*Lmax) { cerr << "FwdSub error: singular matrix!\n"; return 1; } // solve for this row of solution x[j] /= L[IDX(j,j,n)]; // update all remaining rhs for (long int i=j+1; i<n; i++) x[i] -= L[IDX(i,j,n)]*x[j]; } // return success return 0; }
// solves a linear system A*x = b, filling in the input Mat x int Solve(Mat &Amat, Mat &xvec, Mat &bvec) { // check that matrix sizes match if (Amat.Rows() != bvec.Rows() || Amat.Rows() != Amat.Cols() || bvec.Cols() != 1 || xvec.Rows() != Amat.Rows() || xvec.Cols() != 1) { cerr << "Solve error, illegal matrix/vector dimensions\n"; cerr << " Mat is " << Amat.Rows() << " x " << Amat.Cols() << ", rhs is " << bvec.Rows() << " x " << bvec.Cols() << ", solution is " << xvec.Rows() << " x " << xvec.Cols() << endl; return 1; } // create temporary variables long int i, j, k, p, n; double m, tmp, Amax; // access the data arrays double *A = Amat.get_data(); double *b = bvec.get_data(); // determine maximum absolute entry in A (for singularity check later) Amax = Amat.MaxNorm(); // perform Gaussian elimination to convert A,b to an upper-triangular system n = Amat.Rows(); for (k=0; k<n-1; k++) { // loop over diagonals // find the pivot row p p=k; for (i=k; i<n; i++) if (fabs(A[IDX(i,k,n)]) > fabs(A[IDX(p,k,n)])) p=i; // swap rows in A for (j=k; j<n; j++) { tmp = A[IDX(p,j,n)]; A[IDX(p,j,n)] = A[IDX(k,j,n)]; A[IDX(k,j,n)] = tmp; } // swap rows in b tmp = b[p]; b[p] = b[k]; b[k] = tmp; // check for nonzero matrix diagonal if (fabs(A[IDX(k,k,n)]) < STOL*Amax) { cerr << "Solve error: numerically singular matrix!\n"; return 1; } // perform elimination using row k for (i=k+1; i<n; i++) // store multipliers in column below pivot A[IDX(i,k,n)] /= A[IDX(k,k,n)]; for (j=k+1; j<n; j++) // loop over columns of A, to right of pivot for (i=k+1; i<n; i++) // update rows in column A[IDX(i,j,n)] -= A[IDX(i,k,n)]*A[IDX(k,j,n)]; for (i=k+1; i<n; i++) // update entries in b b[i] -= A[IDX(i,k,n)]*b[k]; } // check for singularity at end (only need to check final diagonal entry) if (fabs(A[IDX(n-1,n-1,n)]) < STOL*Amax) { cerr << "Solve error: numerically singular matrix!\n"; return 1; } // check for singularity at end (only need to check final diagonal entry) if (fabs(A[IDX(n-1,n-1,n)]) < STOL*Amax) { cerr << "Solve error: numerically singular matrix!\n"; return 1; } // perform Backwards Substitution on result if (BackSub(Amat, xvec, bvec) != 0) { cerr << "Solve: error in BackSub call\n"; return 1; } // return success return 0; }
Void Mat::SetSize(const Mat &m) { SetSize(m.Rows(), m.Cols()); }
// solves a linear system A*x = b, filling in the input Mat x int Solve(Mat &A, Mat &x, Mat &b) { // create temporary variables long int i, j, k, p, n; double tmp, Amax; // check that matrix sizes match if (A.Rows() != b.Rows() || A.Rows() != A.Cols() || b.Cols() != 1 || x.Rows() != A.Rows() || x.Cols() != 1) { fprintf(stderr,"Solve error, illegal matrix/vector dimensions\n"); fprintf(stderr," Mat is %li x %li, sol is %li x %li, rhs is %li x %li\n", A.Rows(), A.Cols(), x.Rows(), x.Cols(), b.Rows(), b.Cols()); return 1; } // determine maximum absolute entry in A (for singularity check later) Amax = A.MaxNorm(); // perform Gaussian elimination to convert A,b to an upper-triangular system n = A.Rows(); for (k=0; k<n-1; k++) { // loop over diagonals // find the pivot row p p=k; for (i=k; i<n; i++) if (fabs(A(i,k)) > fabs(A(p,k))) p=i; // swap rows in A for (j=k; j<n; j++) { tmp = A(p,j); A(p,j) = A(k,j); A(k,j) = tmp; } // swap rows in b tmp = b(p); b(p) = b(k); b(k) = tmp; // check for singular matrix if (fabs(A(k,k)) < 1.e-13*Amax) { fprintf(stderr,"Solve error: numerically singular matrix!\n"); return 1; } // perform elimination on remaining submatrix of A using row k for (j=k+1; j<n; j++) for (i=k+1; i<n; i++) A(i,j) = A(i,j) - A(i,k)/A(k,k)*A(k,j); // perform elimination on remainder of b using row k for (i=k+1; i<n; i++) b(i) -= A(i,k)/A(k,k)*b(k); } // check for singularity at end (only need to check final diagonal entry) if (fabs(A(n-1,n-1)) < 1.e-13*Amax) { fprintf(stderr,"Solve error: numerically singular matrix!\n"); return 1; } // perform Backwards Substitution on result if (BackSub(A, x, b) != 0) { fprintf(stderr,"Solve error in BackSub call\n"); return 1; } // return success return 0; }