1 files changed, 62 insertions, 0 deletions

diff --git a/SD-VBS/common/toolbox/lagrcv/goodFeaturesToTrack.cc b/SD-VBS/common/toolbox/lagrcv/goodFeaturesToTrack.cc
new file mode 100755
index 0000000..376a096
--- /dev/null
+++ b/SD-VBS/common/toolbox/lagrcv/goodFeaturesToTrack.cc
@@ -0,0 +1,62 @@
+
+/* compile with 
+   mex goodFeaturesToTrack.cc -I/usr/local/opencv/include -L/usr/local/opencv/lib -lcxcore -lcv
+*/
+
+#include "mex.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+#include <stdio.h>
+#include <math.h>
+
+#define MAX_CORNERS 500
+
+IplImage *eigtemp = NULL, *temp2 = NULL;
+
+// TODO: add number of corners parameter
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
+  // usage: [ features numvalid ] = 
+  //   goodFeaturesToTrack(image, quality, mindist, mask)
+  // image must be single-channel, 8-bit
+  // quality = minimum acceptable ratio of eigenvalues
+  // mindist = minimum distance between corners
+  // mask (optional) = bitmap mask "region of interest" (MUST BE uint8 TYPE!)  
+
+  char *image_pr = (char*)mxGetPr(prhs[0]);
+  //  int imdims[] = { (int)d_imdims[0], (int)d_imdims[1] };
+  const int *imdims = mxGetDimensions(prhs[0]);
+  double quality = mxGetScalar(prhs[1]);
+  double mindist = mxGetScalar(prhs[2]);
+  bool use_roi = nrhs > 3;
+
+  plhs[0] = mxCreateNumericMatrix(2, MAX_CORNERS, mxSINGLE_CLASS, mxREAL);
+  plhs[1] = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
+
+  CvPoint2D32f *corner_coords = (CvPoint2D32f*)mxGetPr(plhs[0]);
+  int *corner_count = (int*)mxGetPr(plhs[1]);
+  *corner_count = MAX_CORNERS;
+
+  if (eigtemp == NULL) {
+    eigtemp = cvCreateImage(cvSize(imdims[0],imdims[1]), IPL_DEPTH_32F, 1);
+    temp2 = cvCreateImage(cvSize(imdims[0],imdims[1]), IPL_DEPTH_32F, 1);
+  }
+
+  IplImage *image = 
+    cvCreateImageHeader(cvSize(imdims[0], imdims[1]), IPL_DEPTH_8U, 1);
+  image->imageData = image_pr;
+
+  IplImage *roimask = NULL;
+  if (use_roi) {
+    roimask = cvCreateImage(cvSize(imdims[0],imdims[1]), IPL_DEPTH_8U, 1);
+    roimask->imageData = (char*)mxGetPr(prhs[3]);
+  }
+
+  cvGoodFeaturesToTrack(image, 
+                        eigtemp, temp2, 
+                        corner_coords,
+                        corner_count,
+                        quality, 
+                        mindist, 
+                        roimask
+                        );
+}