diff --git a/tracking/utility/affparam2geom.m b/tracking/utility/affparam2geom.m index bef3e6d..ec965d9 100644 --- a/tracking/utility/affparam2geom.m +++ b/tracking/utility/affparam2geom.m @@ -1,39 +1,62 @@ -function q = affparam2geom(p) -% function q = affparam2geom(p) -% -% The functions affparam2geom and affparam2mat convert a 'geometric' -% affine parameter to/from a matrix form (2x3 matrix). -% -% affparam2geom converts a 2x3 matrix to 6 affine parameters -% (x, y, th, scale, aspect, skew), and affparam2mat does the inverse. -% -% p(6) : [p(1) p(3) p(4); p(2) p(5) p(6)] -% q(6) : [dx dy sc th sr phi] -% -% Reference "Multiple View Geometry in Computer Vision" by Richard -% Hartley and Andrew Zisserman. - -% Copyright (C) Jongwoo Lim and David Ross. All rights reserved. - -A = [ p(3), p(4); p(5), p(6) ]; -%% A = USVt = (UVt)(VSVt) = R(th)R(-phi)SR(phi) -[U,S,V] = svd(A); -if (det(U) < 0) - U = U(:,2:-1:1); V = V(:,2:-1:1); S = S(2:-1:1,2:-1:1); -end -q(1) = p(1); q(2) = p(2); -q(4) = atan2(U(2,1)*V(1,1)+U(2,2)*V(1,2), U(1,1)*V(1,1)+U(1,2)*V(1,2)); - -phi = atan2(V(1,2),V(1,1)); -if (phi <= -pi/2) - c = cos(-pi/2); s = sin(-pi/2); - R = [c -s; s c]; V = V * R; S = R'*S*R; -end -if (phi >= pi/2) - c = cos(pi/2); s = sin(pi/2); - R = [c -s; s c]; V = V * R; S = R'*S*R; -end -q(3) = S(1,1); -q(5) = S(2,2)/S(1,1); -q(6) = atan2(V(1,2),V(1,1)); -q = reshape(q, size(p)); +function q = affparam2geom(p) +% function q = affparam2geom(p) +% +% 输入 ： p，原始参数矩阵； +% 输出 ： q，具有几何意义的参数； +% The functions affparam2geom and affparam2mat convert a 'geometric' +% affine parameter to/from a matrix form (2x3 matrix). +% +% affparam2geom converts a 2x3 matrix to 6 affine parameters +% (x, y, th, scale, aspect, skew), and affparam2mat does the inverse. +% +% p(6) : [p(1) p(3) p(4); p(2) p(5) p(6)] +% +% x' p(3) p(4) p(1) x +% y' = p(5) p(6) p(2) * y +% 1 0 0 1 1 +% +% p(3) p(4) +% A = +% p(5) p(6) +% +% q(6) : [dx dy sc th sr phi] +% dx,dy : 平移变换 +% sc,sr : 尺度变换 +% th ： 旋转变换 +% phi ： 错切变换 +% +% Reference "Multiple View Geometry in Computer Vision" by Richard +% Hartley and Andrew Zisserman. + +% Copyright (C) Jongwoo Lim and David Ross. All rights reserved. + +A = [ p(3), p(4); p(5), p(6) ]; +%% A = USVt = (UVt)(VSVt) = R(th)R(-phi)SR(phi) +[U,S,V] = svd(A); +if (det(U) < 0) + U = U(:,2:-1:1); V = V(:,2:-1:1); S = S(2:-1:1,2:-1:1); +end +%%*******平移变换*******%% +q(1) = p(1); +q(2) = p(2); +%%*******平移变换*******%% + +q(4) = atan2(U(2,1)*V(1,1)+U(2,2)*V(1,2), U(1,1)*V(1,1)+U(1,2)*V(1,2)); + +phi = atan2(V(1,2),V(1,1)); +if (phi <= -pi/2) + c = cos(-pi/2); s = sin(-pi/2); + R = [c -s; s c]; V = V * R; S = R'*S*R; +end +if (phi >= pi/2) + c = cos(pi/2); s = sin(pi/2); + R = [c -s; s c]; V = V * R; S = R'*S*R; +end + +%%*******尺度变换*******%% +q(3) = S(1,1); +q(5) = S(2,2)/S(1,1); +%%*******尺度变换*******%% +q(6) = atan2(V(1,2),V(1,1)); + +q = reshape(q, size(p)); diff --git a/tracking/utility/affparam2mat.m b/tracking/utility/affparam2mat.m index e316da6..74d0b64 100644 --- a/tracking/utility/affparam2mat.m +++ b/tracking/utility/affparam2mat.m @@ -1,30 +1,33 @@ -function q = affparam2mat(p) -% function q = affparam2mat(p) -% -% The functions affparam2geom and affparam2mat convert a 'geometric' -% affine parameter to/from a matrix form (2x3 matrix). -% -% affparam2geom converts a 2x3 matrix to 6 affine parameters -% (x, y, th, scale, aspect, skew), and affparam2mat does the inverse. -% -% p(6,n) : [dx dy sc th sr phi]' -% q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)] -% -% Reference "Multiple View Geometry in Computer Vision" by Richard -% Hartley and Andrew Zisserman. - -% Copyright (C) Jongwoo Lim and David Ross. All rights reserved. - - -sz = size(p); -if (length(p(:)) == 6) - p = p(:); -end -s = p(3,:); th = p(4,:); r = p(5,:); phi = p(6,:); -cth = cos(th); sth = sin(th); cph = cos(phi); sph = sin(phi); -ccc = cth.*cph.*cph; ccs = cth.*cph.*sph; css = cth.*sph.*sph; -scc = sth.*cph.*cph; scs = sth.*cph.*sph; sss = sth.*sph.*sph; -q(1,:) = p(1,:); q(2,:) = p(2,:); -q(3,:) = s.*(ccc +scs +r.*(css -scs)); q(4,:) = s.*(r.*(ccs -scc) -ccs -sss); -q(5,:) = s.*(scc -ccs +r.*(ccs +sss)); q(6,:) = s.*(r.*(ccc +scs) -scs +css); -q = reshape(q, sz); +function q = affparam2mat(p) +% function q = affparam2mat(p) +% +% The functions affparam2geom and affparam2mat convert a 'geometric' +% affine parameter to/from a matrix form (2x3 matrix). +% +% 输入 ： q，具有几何意义的参数； +% 输出 ： p，原始参数矩阵； +% +% affparam2geom converts a 2x3 matrix to 6 affine parameters +% (x, y, th, scale, aspect, skew), and affparam2mat does the inverse. +% +% p(6,n) : [dx dy sc th sr phi]' +% q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)] +% +% Reference "Multiple View Geometry in Computer Vision" by Richard +% Hartley and Andrew Zisserman. + +% Copyright (C) Jongwoo Lim and David Ross. All rights reserved. + + +sz = size(p); +if (length(p(:)) == 6) + p = p(:); +end +s = p(3,:); th = p(4,:); r = p(5,:); phi = p(6,:); +cth = cos(th); sth = sin(th); cph = cos(phi); sph = sin(phi); +ccc = cth.*cph.*cph; ccs = cth.*cph.*sph; css = cth.*sph.*sph; +scc = sth.*cph.*cph; scs = sth.*cph.*sph; sss = sth.*sph.*sph; +q(1,:) = p(1,:); q(2,:) = p(2,:); +q(3,:) = s.*(ccc +scs +r.*(css -scs)); q(4,:) = s.*(r.*(ccs -scc) -ccs -sss); +q(5,:) = s.*(scc -ccs +r.*(ccs +sss)); q(6,:) = s.*(r.*(ccc +scs) -scs +css); +q = reshape(q, sz); diff --git a/tracking/utility/affparaminv.m b/tracking/utility/affparaminv.m index ef86a12..e161d40 100644 --- a/tracking/utility/affparaminv.m +++ b/tracking/utility/affparaminv.m @@ -1,17 +1,17 @@ -function q = affparaminv(p,q) -% function q = affparaminv(p[, q]) -% -% p(6,n) : [dx dy sc th sr phi]' -% q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)] - -% Copyright (C) Jongwoo Lim and David Ross. All rights reserved. - -if (length(p) == 6) - p = p(:); -end -if (nargin > 1) - q = inv([p(3) p(4); p(5) p(6)]) * [q(1)-p(1) q(3:4); q(2)-p(2) q(5:6)]; -else - q = inv([p(3) p(4); p(5) p(6)]) * [-p(1) 1 0; -p(2) 0 1]; -end -q = q([1,2,3,5,4,6]); +function q = affparaminv(p,q) +% function q = affparaminv(p[, q]) +% +% p(6,n) : [dx dy sc th sr phi]' +% q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)] + +% Copyright (C) Jongwoo Lim and David Ross. All rights reserved. + +if (length(p) == 6) + p = p(:); +end +if (nargin > 1) + q = inv([p(3) p(4); p(5) p(6)]) * [q(1)-p(1) q(3:4); q(2)-p(2) q(5:6)]; +else + q = inv([p(3) p(4); p(5) p(6)]) * [-p(1) 1 0; -p(2) 0 1]; +end +q = q([1,2,3,5,4,6]); diff --git a/tracking/utility/interp2.cpp b/tracking/utility/interp2.cpp index 914d76f..df08b3e 100644 --- a/tracking/utility/interp2.cpp +++ b/tracking/utility/interp2.cpp @@ -1,110 +1,110 @@ -// interp2.cpp -// -// Copyright (C) Jongwoo Lim. -// All rights reserved. -// -////////////////////////////////////////////////////////////////////// - -#include "mex.h" -#include - -//#include "cvbase/cvimage.h" - -#define ARR(A,r,c,nr,nc) (((r)<0 || (r)>=(nr) || (c)<0 || (c)>=(nc))? 0 : (A)[(c)*(nr)+(r)]) - -inline double interp(double *img, int w, int h, double x, double y) -{ - register int x0 = (int) x, y0 = (int) y, x1 = x0+1, y1 = y0+1; - register double rx0 = x-x0, rx1 = 1-rx0, ry = y-y0; - return ((rx1*ARR(img,y0,x0,h,w) + rx0*ARR(img,y0,x1,h,w))*(1-ry) + - (rx1*ARR(img,y1,x0,h,w) + rx0*ARR(img,y1,x1,h,w))*ry); -} - -//----------------------------------------------------------------------------- - -void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) -{ - // check inputs - if (nrhs < 3 || nrhs > 6) - mexErrMsgTxt("invalid number of inputs: [x,y,]z,xi,yi[,method]"); - if (nlhs != 1) - mexErrMsgTxt("invalid number of outputs: zi"); - - const char *_argin[] = { "x","y","z","xi","yi","method" }; - const char **argin = (nrhs < 4)? &_argin[2] : _argin; - char buf[1024]; - - // Check input data - const mxArray *X = NULL, *Y = NULL, *Z, *XI, *YI; - if (nrhs < 4) - Z = prhs[0], XI = prhs[1], YI = prhs[2]; - else - X = prhs[0], Y = prhs[1], Z = prhs[2], XI = prhs[3], YI = prhs[4]; - - if (nrhs == 4 || nrhs == 6) - nrhs--; // Ignore method - - for (int ri = 0; ri < nrhs; ri++) { - if (!prhs[ri]) - { sprintf(buf,"invalid mxArray(%s)", argin[ri]); mexErrMsgTxt(buf); } - if (!mxIsDouble(prhs[ri])) - { sprintf(buf,"data must be double (%s)", argin[ri]); mexErrMsgTxt(buf); } - } - if (mxGetNumberOfDimensions(Z) != 2) - mexErrMsgTxt("invalid Z dimension: should be 2"); - // Need to check whether dims of X,Y,Z matche - if (mxGetNumberOfElements(XI) != mxGetNumberOfElements(YI)) - mexErrMsgTxt("XI and YI does not match"); - - // Build output data - const int *dimz = mxGetDimensions(Z), *dim = mxGetDimensions(XI); - int ndim = mxGetNumberOfDimensions(XI); - - register double dx = -1, dy = -1, sx = 1, sy = 1; - if (nrhs >= 4) { - int nx = mxGetNumberOfElements(X), ny = mxGetNumberOfElements(Y); - double *pX = mxGetPr(X), *pY = mxGetPr(Y); - if (nx == dimz[1] && ny == dimz[0]) - dx = -pX[0], sx = (dimz[1]-1)/(pX[nx-1]-pX[0]), - dy = -pY[0], sy = (dimz[0]-1)/(pY[ny-1]-pY[0]); - else if (nx == ny && ny == mxGetNumberOfElements(Z)) - dx = -pX[0], sx = (dimz[1]-1)/(pX[(dimz[1]-1)*dimz[0]]-pX[0]), - dy = -pY[0], sy = (dimz[0]-1)/(pY[dimz[0]-1]-pY[0]); - else - mexErrMsgTxt("X and Y do not match with Z"); - } - if (mxIsComplex(Z)) { - mxArray *ZI = plhs[0] = mxCreateNumericArray(ndim, dim, mxDOUBLE_CLASS, mxCOMPLEX); - - // Load input data and fill output array - double *pZr = mxGetPr(Z), *pZi = mxGetPi(Z), *pXI = mxGetPr(XI), *pYI = mxGetPr(YI); - double *pZIr = mxGetPr(ZI), *pZIi = mxGetPi(ZI); - -// cvImage imgr(pZr, dimz[0], dimz[1]), imgi(pZi, dimz[0], dimz[1]); - int h = dimz[0], w = dimz[1]; - for (int i = 0, n = mxGetNumberOfElements(XI); i < n; i++) { - pZIr[i] = interp(pZr, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy)); - pZIi[i] = interp(pZi, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy)); -// cvtPoint p(sy*(pYI[i]+dy),sx*(pXI[i]+dx)); -// pZIr[i] = imgr.interp(p); -// pZIi[i] = imgi.interp(p); - } - } - else { - mxArray *ZI = plhs[0] = mxCreateNumericArray(ndim, dim, mxDOUBLE_CLASS, mxREAL); - - // Load input data and fill output array - double *pZ = mxGetPr(Z), *pXI = mxGetPr(XI), *pYI = mxGetPr(YI); - double *pZI = mxGetPr(ZI); - -// cvImage img(pZ, dimz[0], dimz[1]); - int h = dimz[0], w = dimz[1]; - for (int i = 0, n = mxGetNumberOfElements(XI); i < n; i++) { - pZI[i] = interp(pZ, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy)); -// cvtPoint p(sy*(pYI[i]+dy),sx*(pXI[i]+dx)); -// pZI[i] = img.interp(p); - } - } -} - -//----------------------------------------------------------------------------- +// interp2.cpp +// +// Copyright (C) Jongwoo Lim. +// All rights reserved. +// +////////////////////////////////////////////////////////////////////// + +#include "mex.h" +#include + +//#include "cvbase/cvimage.h" + +#define ARR(A,r,c,nr,nc) (((r)<0 || (r)>=(nr) || (c)<0 || (c)>=(nc))? 0 : (A)[(c)*(nr)+(r)]) + +inline double interp(double *img, int w, int h, double x, double y) +{ + register int x0 = (int) x, y0 = (int) y, x1 = x0+1, y1 = y0+1; + register double rx0 = x-x0, rx1 = 1-rx0, ry = y-y0; + return ((rx1*ARR(img,y0,x0,h,w) + rx0*ARR(img,y0,x1,h,w))*(1-ry) + + (rx1*ARR(img,y1,x0,h,w) + rx0*ARR(img,y1,x1,h,w))*ry); +} + +//----------------------------------------------------------------------------- + +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) +{ + // check inputs + if (nrhs < 3 || nrhs > 6) + mexErrMsgTxt("invalid number of inputs: [x,y,]z,xi,yi[,method]"); + if (nlhs != 1) + mexErrMsgTxt("invalid number of outputs: zi"); + + const char *_argin[] = { "x","y","z","xi","yi","method" }; + const char **argin = (nrhs < 4)? &_argin[2] : _argin; + char buf[1024]; + + // Check input data + const mxArray *X = NULL, *Y = NULL, *Z, *XI, *YI; + if (nrhs < 4) + Z = prhs[0], XI = prhs[1], YI = prhs[2]; + else + X = prhs[0], Y = prhs[1], Z = prhs[2], XI = prhs[3], YI = prhs[4]; + + if (nrhs == 4 || nrhs == 6) + nrhs--; // Ignore method + + for (int ri = 0; ri < nrhs; ri++) { + if (!prhs[ri]) + { sprintf(buf,"invalid mxArray(%s)", argin[ri]); mexErrMsgTxt(buf); } + if (!mxIsDouble(prhs[ri])) + { sprintf(buf,"data must be double (%s)", argin[ri]); mexErrMsgTxt(buf); } + } + if (mxGetNumberOfDimensions(Z) != 2) + mexErrMsgTxt("invalid Z dimension: should be 2"); + // Need to check whether dims of X,Y,Z matche + if (mxGetNumberOfElements(XI) != mxGetNumberOfElements(YI)) + mexErrMsgTxt("XI and YI does not match"); + + // Build output data + const int *dimz = mxGetDimensions(Z), *dim = mxGetDimensions(XI); + int ndim = mxGetNumberOfDimensions(XI); + + register double dx = -1, dy = -1, sx = 1, sy = 1; + if (nrhs >= 4) { + int nx = mxGetNumberOfElements(X), ny = mxGetNumberOfElements(Y); + double *pX = mxGetPr(X), *pY = mxGetPr(Y); + if (nx == dimz[1] && ny == dimz[0]) + dx = -pX[0], sx = (dimz[1]-1)/(pX[nx-1]-pX[0]), + dy = -pY[0], sy = (dimz[0]-1)/(pY[ny-1]-pY[0]); + else if (nx == ny && ny == mxGetNumberOfElements(Z)) + dx = -pX[0], sx = (dimz[1]-1)/(pX[(dimz[1]-1)*dimz[0]]-pX[0]), + dy = -pY[0], sy = (dimz[0]-1)/(pY[dimz[0]-1]-pY[0]); + else + mexErrMsgTxt("X and Y do not match with Z"); + } + if (mxIsComplex(Z)) { + mxArray *ZI = plhs[0] = mxCreateNumericArray(ndim, dim, mxDOUBLE_CLASS, mxCOMPLEX); + + // Load input data and fill output array + double *pZr = mxGetPr(Z), *pZi = mxGetPi(Z), *pXI = mxGetPr(XI), *pYI = mxGetPr(YI); + double *pZIr = mxGetPr(ZI), *pZIi = mxGetPi(ZI); + +// cvImage imgr(pZr, dimz[0], dimz[1]), imgi(pZi, dimz[0], dimz[1]); + int h = dimz[0], w = dimz[1]; + for (int i = 0, n = mxGetNumberOfElements(XI); i < n; i++) { + pZIr[i] = interp(pZr, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy)); + pZIi[i] = interp(pZi, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy)); +// cvtPoint p(sy*(pYI[i]+dy),sx*(pXI[i]+dx)); +// pZIr[i] = imgr.interp(p); +// pZIi[i] = imgi.interp(p); + } + } + else { + mxArray *ZI = plhs[0] = mxCreateNumericArray(ndim, dim, mxDOUBLE_CLASS, mxREAL); + + // Load input data and fill output array + double *pZ = mxGetPr(Z), *pXI = mxGetPr(XI), *pYI = mxGetPr(YI); + double *pZI = mxGetPr(ZI); + +// cvImage img(pZ, dimz[0], dimz[1]); + int h = dimz[0], w = dimz[1]; + for (int i = 0, n = mxGetNumberOfElements(XI); i < n; i++) { + pZI[i] = interp(pZ, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy)); +// cvtPoint p(sy*(pYI[i]+dy),sx*(pXI[i]+dx)); +// pZI[i] = img.interp(p); + } + } +} + +//----------------------------------------------------------------------------- diff --git a/tracking/utility/warpimg.m b/tracking/utility/warpimg.m index 109f02d..c47b210 100644 --- a/tracking/utility/warpimg.m +++ b/tracking/utility/warpimg.m @@ -1,29 +1,30 @@ -function wimg = warpimg(img, p, sz) -% function wimg = warpimg(img, p, sz) -% -% img(h,w) -% p(6,n) : mat format -% sz(th,tw) -% - -%% Copyright (C) 2005 Jongwoo Lim and David Ross. -%% All rights reserved. - - -if (nargin < 3) - sz = size(img); -end -if (size(p,1) == 1) - p = p(:); -end -w = sz(2); h = sz(1); n = size(p,2); -%[x,y] = meshgrid(1:w, 1:h); -[x,y] = meshgrid([1:w]-w/2+0.5, [1:h]-h/2); -pos = reshape(cat(2, ones(h*w,1),x(:),y(:)) ... - * [p(1,:) p(2,:); p(3:4,:) p(5:6,:)], [h,w,n,2]); -cn=size(img,3); -wimg=zeros([sz cn]); -for i=1:cn - wimg(:,:,i) = squeeze(interp2(img(:,:,i), pos(:,:,:,1), pos(:,:,:,2))); -end -wimg(find(isnan(wimg))) = 0; +function wimg = warpimg(img, p, sz) +% function wimg = warpimg(img, p, sz) +% +% img(h,w) : 原始图像 +% p(6,n) : 仿射参数 mat format +% sz(th,tw) ： 提取块大小 +% + +%% Copyright (C) 2005 Jongwoo Lim and David Ross. +%% All rights reserved. + +if (nargin < 3) + sz = size(img); +end +if (size(p,1) == 1) + p = p(:); +end +w = sz(2); h = sz(1); n = size(p,2); +[x,y] = meshgrid([1:w]-w/2, [1:h]-h/2); +pos = reshape(cat(2, ones(h*w,1),x(:),y(:)) ... + * [p(1,:) p(2,:); p(3:4,:) p(5:6,:)], [h,w,n,2]); +wimg = squeeze(interp2(img, pos(:,:,:,1), pos(:,:,:,2))); + %%pos(:,:,:,1) : x坐标矩阵 + %%pos(:,:,:,2) ： y坐标矩阵 +wimg(find(isnan(wimg))) = 0; %%去掉个别坏点 + +% B = SQUEEZE(A) returns an array B with the same elements as +% A but with all the singleton dimensions removed. A singleton +% is a dimension such that size(A,dim)==1; +