void mexFunction(int nout, mxArray *out[], int nin, const mxArray *in[]) { int M,N,S,smin,K ; const int* dimensions ; const double* P_pt ; const double* D_pt ; double threshold = 0.01 ; /*0.02 ;*/ double r = 10.0 ; double* result ; enum {IN_P=0,IN_D,IN_SMIN,IN_THRESHOLD,IN_R} ; enum {OUT_Q=0} ; /* ----------------------------------------------------------------- ** Check the arguments ** -------------------------------------------------------------- */ if (nin < 3) { mexErrMsgTxt("At least three input arguments required."); } else if (nout > 1) { mexErrMsgTxt("Too many output arguments."); } if( !uIsRealMatrix(in[IN_P],3,-1) ) { mexErrMsgTxt("P must be a 3xK real matrix") ; } if( !mxIsDouble(in[IN_D]) || mxGetNumberOfDimensions(in[IN_D]) != 3) { mexErrMsgTxt("G must be a three dimensional real array.") ; } if( !uIsRealScalar(in[IN_SMIN]) ) { mexErrMsgTxt("SMIN must be a real scalar.") ; } if(nin >= 4) { if(!uIsRealScalar(in[IN_THRESHOLD])) { mexErrMsgTxt("THRESHOLD must be a real scalar.") ; } threshold = *mxGetPr(in[IN_THRESHOLD]) ; } if(nin >= 5) { if(!uIsRealScalar(in[IN_R])) { mexErrMsgTxt("R must be a real scalar.") ; } r = *mxGetPr(in[IN_R]) ; } dimensions = mxGetDimensions(in[IN_D]) ; M = dimensions[0] ; N = dimensions[1] ; S = dimensions[2] ; smin = (int)(*mxGetPr(in[IN_SMIN])) ; if(S < 3 || M < 3 || N < 3) { mexErrMsgTxt("All dimensions of DOG must be not less than 3.") ; } K = mxGetN(in[IN_P]) ; P_pt = mxGetPr(in[IN_P]) ; D_pt = mxGetPr(in[IN_D]) ; /* If the input array is empty, then output an empty array as well. */ if( K == 0) { out[OUT_Q] = mxDuplicateArray(in[IN_P]) ; return ; } /* ----------------------------------------------------------------- * Do the job * -------------------------------------------------------------- */ { double* buffer = (double*) mxMalloc(K*3*sizeof(double)) ; double* buffer_iterator = buffer ; int p ; const int yo = 1 ; const int xo = M ; const int so = M*N ; for(p = 0 ; p < K ; ++p) { int x = ((int)*P_pt++) ; int y = ((int)*P_pt++) ; int s = ((int)*P_pt++) - smin ; int iter ; double b[3] ; /* Local maxima extracted from the DOG * have coorrinates 1<=x<=N-2, 1<=y<=M-2 * and 1<=s-mins<=S-2. This is also the range of the points * that we can refine. */ if(x < 1 || x > N-2 || y < 1 || y > M-2 || s < 1 || s > S-2) { continue ; } #define at(dx,dy,ds) (*(pt + (dx)*xo + (dy)*yo + (ds)*so)) { const double* pt = D_pt + y*yo + x*xo + s*so ; double Dx=0,Dy=0,Ds=0,Dxx=0,Dyy=0,Dss=0,Dxy=0,Dxs=0,Dys=0 ; int dx = 0 ; int dy = 0 ; int j, i, jj, ii ; for(iter = 0 ; iter < max_iter ; ++iter) { double A[3*3] ; #define Aat(i,j) (A[(i)+(j)*3]) x += dx ; y += dy ; pt = D_pt + y*yo + x*xo + s*so ; /* Compute the gradient. */ Dx = 0.5 * (at(+1,0,0) - at(-1,0,0)) ; Dy = 0.5 * (at(0,+1,0) - at(0,-1,0)); Ds = 0.5 * (at(0,0,+1) - at(0,0,-1)) ; /* Compute the Hessian. */ Dxx = (at(+1,0,0) + at(-1,0,0) - 2.0 * at(0,0,0)) ; Dyy = (at(0,+1,0) + at(0,-1,0) - 2.0 * at(0,0,0)) ; Dss = (at(0,0,+1) + at(0,0,-1) - 2.0 * at(0,0,0)) ; Dxy = 0.25 * ( at(+1,+1,0) + at(-1,-1,0) - at(-1,+1,0) - at(+1,-1,0) ) ; Dxs = 0.25 * ( at(+1,0,+1) + at(-1,0,-1) - at(-1,0,+1) - at(+1,0,-1) ) ; Dys = 0.25 * ( at(0,+1,+1) + at(0,-1,-1) - at(0,-1,+1) - at(0,+1,-1) ) ; /* Solve linear system. */ Aat(0,0) = Dxx ; Aat(1,1) = Dyy ; Aat(2,2) = Dss ; Aat(0,1) = Aat(1,0) = Dxy ; Aat(0,2) = Aat(2,0) = Dxs ; Aat(1,2) = Aat(2,1) = Dys ; b[0] = - Dx ; b[1] = - Dy ; b[2] = - Ds ; /* Gauss elimination */ for(j = 0 ; j < 3 ; ++j) { double maxa = 0 ; double maxabsa = 0 ; int maxi = -1 ; double tmp ; /* look for the maximally stable pivot */ for (i = j ; i < 3 ; ++i) { double a = Aat (i,j) ; double absa = abs (a) ; if (absa > maxabsa) { maxa = a ; maxabsa = absa ; maxi = i ; } } /* if singular give up */ if (maxabsa < 1e-10f) { b[0] = 0 ; b[1] = 0 ; b[2] = 0 ; break ; } i = maxi ; /* swap j-th row with i-th row and normalize j-th row */ for(jj = j ; jj < 3 ; ++jj) { tmp = Aat(i,jj) ; Aat(i,jj) = Aat(j,jj) ; Aat(j,jj) = tmp ; Aat(j,jj) /= maxa ; } tmp = b[j] ; b[j] = b[i] ; b[i] = tmp ; b[j] /= maxa ; /* elimination */ for (ii = j+1 ; ii < 3 ; ++ii) { double x = Aat(ii,j) ; for (jj = j ; jj < 3 ; ++jj) { Aat(ii,jj) -= x * Aat(j,jj) ; } b[ii] -= x * b[j] ; } } /* backward substitution */ for (i = 2 ; i > 0 ; --i) { double x = b[i] ; for (ii = i-1 ; ii >= 0 ; --ii) { b[ii] -= x * Aat(ii,i) ; } } /* If the translation of the keypoint is big, move the keypoint * and re-iterate the computation. Otherwise we are all set. */ dx= ((b[0] > 0.6 && x < N-2) ? 1 : 0 ) + ((b[0] < -0.6 && x > 1 ) ? -1 : 0 ) ; dy= ((b[1] > 0.6 && y < M-2) ? 1 : 0 ) + ((b[1] < -0.6 && y > 1 ) ? -1 : 0 ) ; if( dx == 0 && dy == 0 ) break ; } { double val = at(0,0,0) + 0.5 * (Dx * b[0] + Dy * b[1] + Ds * b[2]) ; double score = (Dxx+Dyy)*(Dxx+Dyy) / (Dxx*Dyy - Dxy*Dxy) ; double xn = x + b[0] ; double yn = y + b[1] ; double sn = s + b[2] ; if(fabs(val) > threshold && score < (r+1)*(r+1)/r && score >= 0 && fabs(b[0]) < 1.5 && fabs(b[1]) < 1.5 && fabs(b[2]) < 1.5 && xn >= 0 && xn <= N-1 && yn >= 0 && yn <= M-1 && sn >= 0 && sn <= S-1) { *buffer_iterator++ = xn ; *buffer_iterator++ = yn ; *buffer_iterator++ = sn+smin ; } } } } /* Copy the result into an array. */ { int NL = (buffer_iterator - buffer)/3 ; out[OUT_Q] = mxCreateDoubleMatrix(3, NL, mxREAL) ; result = mxGetPr(out[OUT_Q]); memcpy(result, buffer, sizeof(double) * 3 * NL) ; } mxFree(buffer) ; } }
void mexFunction(int nout, mxArray *out[], int nin, const mxArray *in[]) { int M, N, S, H,K; const int* dimensions ; const double* P_pt ; const double* D_pt ; double* result ; enum {IN_P=0, IN_D} ; enum {OUT_Q=0} ; /* ----------------------------------------------------------------- ** Check the arguments ** -------------------------------------------------------------- */ if (nin < 2) { mexErrMsgTxt("At least 2 input arguments required."); } else if (nout > 1) { mexErrMsgTxt("Too many output arguments."); } if( !mxIsDouble(in[IN_D]) || mxGetNumberOfDimensions(in[IN_D]) != 3) { mexErrMsgTxt("G must be a three dimensional real array.") ; } dimensions = mxGetDimensions(in[IN_D]) ; M = dimensions[0] ; N = dimensions[1] ; S = dimensions[2] ; if(S < 3 || M < 3 || N < 3) { mexErrMsgTxt("All dimensions must be not less than 3.") ; } K = mxGetN(in[IN_P]) ; H = mxGetM(in[IN_P]) ; P_pt = mxGetPr(in[IN_P]) ; D_pt = mxGetPr(in[IN_D]) ; /* If the input array is empty, then output an empty array as well. */ if( K == 0) { out[OUT_Q] = mxDuplicateArray(in[IN_P]) ; return ; } /* ----------------------------------------------------------------- * Do the job * -------------------------------------------------------------- */ { double* buffer = (double*) mxMalloc(K*H*sizeof(double)) ; double* buffer_iterator = buffer ; int p; const int yo = 1 ; const int xo = M ; const int so = M*N ; for(p = 0 ; p < K ; ++p) { double b[3] ; int x = ((int)*P_pt++) ; int y = ((int)*P_pt++) ; int s = ((int)*P_pt++) ; if(x < 1 || x > N-2 || y < 1 || y > M-2 || s < 1 || s > S-2) { continue ; } const double* pt = D_pt + y*yo + x*xo + s*so ; double Dx=0, Dy=0, Ds=0, Dxx=0, Dyy=0, Dss=0, Dxy=0, Dxs=0,Dys=0; double A[3*3] ; pt = D_pt + y*yo + x*xo + s*so ; /* Compute the gradient. */ Dx = 0.5 * (at(+1, 0,0) - at(-1, 0,0)) ; Dy = 0.5 * (at(0, +1,0) - at(0, -1, 0)); Ds = 0.5 * (at(0, 0,+1) - at(0, 0, -1)) ; /* Compute the Hessian. */ Dxx = (at(+1, 0, 0) + at(-1, 0, 0) - 2.0 * at(0, 0, 0)) ; Dyy = (at(0, +1, 0) + at(0, -1, 0) - 2.0 * at(0, 0, 0)) ; Dss = (at(0, 0, +1) + at(0, 0, -1) - 2.0 * at(0, 0, 0)) ; Dxy = 0.25 * ( at(+1, +1,0) + at(-1, -1, 0) - at(-1, +1,0) - at(+1, -1,0) ) ; Dxs = 0.25 * ( at(+1, 0, +1) + at(-1, 0, -1) - at(-1, 0, +1) - at(+1, 0, -1) ) ; Dys = 0.25 * ( at(0, +1, +1) + at(0, -1,-1) - at(0, -1, +1) - at(0, +1, -1) ) ; /* Solve linear system. */ Aat(0, 0) = Dxx ; Aat(1, 1) = Dyy ; Aat(2, 2) = Dss ; Aat(0, 1) = Aat(1, 0) = Dxy ; Aat(0, 2) = Aat(2, 0) = Dxs ; Aat(1, 2) = Aat(2, 1) = Dys ; b[0] = - Dx ; b[1] = - Dy ; b[2] = - Ds ; int ret = Gaussian_Elimination(A, 3, b); double xn = x ; double yn = y ; double sn = s ; if(ret==0) { xn+=b[0]; yn+=b[1]; sn+=b[2]; } *buffer_iterator++ = xn ; *buffer_iterator++ = yn ; *buffer_iterator++ = sn ; } int NL = (buffer_iterator - buffer)/H ; out[OUT_Q] = mxCreateDoubleMatrix(H, NL, mxREAL) ; result = mxGetPr(out[OUT_Q]); memcpy(result, buffer, sizeof(double) * H * NL) ; mxFree(buffer) ; } }
VL_EXPORT vl_bool vl_gaussian_elimination (double * A, vl_size numRows, vl_size numColumns) { vl_index i, j, ii, jj ; assert(A) ; assert(numRows <= numColumns) ; #define Aat(i,j) A[(i) + (j)*numRows] /* Gauss elimination */ for(j = 0 ; j < (signed)numRows ; ++j) { double maxa = 0 ; double maxabsa = 0 ; vl_index maxi = -1 ; double tmp ; #if 0 { vl_index iii, jjj ; for (iii = 0 ; iii < 2 ; ++iii) { for (jjj = 0 ; jjj < 3 ; ++jjj) { VL_PRINTF("%5.2g ", Aat(iii,jjj)) ; } VL_PRINTF("\n") ; } VL_PRINTF("\n") ; } #endif /* look for the maximally stable pivot */ for (i = j ; i < (signed)numRows ; ++i) { double a = Aat(i,j) ; double absa = vl_abs_d (a) ; if (absa > maxabsa) { maxa = a ; maxabsa = absa ; maxi = i ; } } i = maxi ; /* if singular give up */ if (maxabsa < 1e-10) return VL_ERR_OVERFLOW ; /* swap j-th row with i-th row and normalize j-th row */ for(jj = j ; jj < (signed)numColumns ; ++jj) { tmp = Aat(i,jj) ; Aat(i,jj) = Aat(j,jj) ; Aat(j,jj) = tmp ; Aat(j,jj) /= maxa ; } #if 0 { vl_index iii, jjj ; VL_PRINTF("after swap %d %d\n", j, i); for (iii = 0 ; iii < 2 ; ++iii) { for (jjj = 0 ; jjj < 3 ; ++jjj) { VL_PRINTF("%5.2g ", Aat(iii,jjj)) ; } VL_PRINTF("\n") ; } VL_PRINTF("\n") ; } #endif /* elimination */ for (ii = j+1 ; ii < (signed)numRows ; ++ii) { double x = Aat(ii,j) ; for (jj = j ; jj < (signed)numColumns ; ++jj) { Aat(ii,jj) -= x * Aat(j,jj) ; } } #if 0 { VL_PRINTF("after elimination\n"); vl_index iii, jjj ; for (iii = 0 ; iii < 2 ; ++iii) { for (jjj = 0 ; jjj < 3 ; ++jjj) { VL_PRINTF("%5.2g ", Aat(iii,jjj)) ; } VL_PRINTF("\n") ; } VL_PRINTF("\n") ; } #endif } /* backward substitution */ for (i = numRows - 1 ; i > 0 ; --i) { /* substitute in all rows above */ for (ii = i - 1 ; ii >= 0 ; --ii) { double x = Aat(ii,i) ; /* j = numRows */ for (j = numRows ; j < (signed)numColumns ; ++j) { Aat(ii,j) -= x * Aat(i,j) ; } } } #if 0 { VL_PRINTF("after substitution\n"); vl_index iii, jjj ; for (iii = 0 ; iii < 2 ; ++iii) { for (jjj = 0 ; jjj < 3 ; ++jjj) { VL_PRINTF("%5.2g ", Aat(iii,jjj)) ; } VL_PRINTF("\n") ; } VL_PRINTF("\n") ; } #endif return VL_ERR_OK ; }