From 324e89ab3aec7e8d93e66c78b7cf65b3ed5b29f3 Mon Sep 17 00:00:00 2001 From: James Bowman Date: Thu, 21 Apr 2011 00:21:42 +0000 Subject: [PATCH] datamatrix --- .../include/opencv2/objdetect/objdetect.hpp | 15 + modules/objdetect/src/datamatrix.cpp | 421 ++++++++++++++++++ modules/objdetect/src/followblk.h | 5 + modules/python/src/cv.cpp | 22 + 4 files changed, 463 insertions(+) create mode 100644 modules/objdetect/src/datamatrix.cpp create mode 100644 modules/objdetect/src/followblk.h diff --git a/modules/objdetect/include/opencv2/objdetect/objdetect.hpp b/modules/objdetect/include/opencv2/objdetect/objdetect.hpp index 7e7eae993..f67f67da6 100644 --- a/modules/objdetect/include/opencv2/objdetect/objdetect.hpp +++ b/modules/objdetect/include/opencv2/objdetect/objdetect.hpp @@ -617,6 +617,21 @@ protected: } +/****************************************************************************************\ +* Datamatrix * +\****************************************************************************************/ + +typedef unsigned char uint8; + +class DatamatrixCode { +public: + char msg[4]; + CvMat *original; + CvMat *corners; +}; +#include +std::deque findcodes(CvMat *im); + #endif #endif diff --git a/modules/objdetect/src/datamatrix.cpp b/modules/objdetect/src/datamatrix.cpp new file mode 100644 index 000000000..47414e418 --- /dev/null +++ b/modules/objdetect/src/datamatrix.cpp @@ -0,0 +1,421 @@ +#include +#include "precomp.hpp" +#include +using namespace std; + +#undef NDEBUG +#include + +class Sampler { +public: + CvMat *im; + CvPoint o; + CvPoint c, cc; + CvMat *perim; + CvPoint fcoord(float fx, float fy); + CvPoint coord(int ix, int iy); + Sampler() {} + Sampler(CvMat *_im, CvPoint _o, CvPoint _c, CvPoint _cc); + uint8 getpixel(int ix, int iy); + int isinside(int x, int y); + int overlap(Sampler &other); + int hasbars(); + void timing(); + CvMat *extract(); +}; + +class code { // used in this file only +public: + char msg[4]; + CvMat *original; + Sampler sa; +}; + +#include "followblk.h" + +#define dethresh 0.92f +#define eincO (2 * dethresh) // e increment orthogonal +#define eincD (1.414 * dethresh) // e increment diagonal + +static const float eincs[] = { + eincO, eincD, + eincO, eincD, + eincO, eincD, + eincO, eincD, + 999 }; + +#define Ki(x) _mm_set_epi32((x),(x),(x),(x)) +#define Kf(x) _mm_set_ps((x),(x),(x),(x)) +#define _mm_abs_ps(x) (__m128)_mm_and_ps((__m128)(x), (__m128)Ki(0x7fffffff)) + +static void writexy(CvMat *m, int r, CvPoint p) +{ + int *pdst = (int*)cvPtr2D(m, r, 0); + pdst[0] = p.x; + pdst[1] = p.y; +} + +Sampler::Sampler(CvMat *_im, CvPoint _o, CvPoint _c, CvPoint _cc) +{ + im = _im; + o = _o; + c = _c; + cc = _cc; + perim = cvCreateMat(4, 1, CV_32SC2); + writexy(perim, 0, fcoord(-.2,-.2)); + writexy(perim, 1, fcoord(-.2,1.2)); + writexy(perim, 2, fcoord(1.2,1.2)); + writexy(perim, 3, fcoord(1.2,-.2)); + // printf("Sampler %d,%d %d,%d %d,%d\n", o.x, o.y, c.x, c.y, cc.x, cc.y); +} + +CvPoint Sampler::fcoord(float fx, float fy) +{ + CvPoint r; + r.x = (int)(o.x + fx * (cc.x - o.x) + fy * (c.x - o.x)); + r.y = (int)(o.y + fx * (cc.y - o.y) + fy * (c.y - o.y)); + return r; +} + +CvPoint Sampler::coord(int ix, int iy) +{ + return fcoord(0.05 + 0.1 * ix, 0.05 + 0.1 * iy); +} + +uint8 Sampler::getpixel(int ix, int iy) +{ + CvPoint pt = coord(ix, iy); + // printf("%d,%d\n", pt.x, pt.y); + return *cvPtr2D(im, pt.y, pt.x); +} + +int Sampler::isinside(int x, int y) +{ + CvPoint2D32f fp; + fp.x = x; + fp.y = y; + return cvPointPolygonTest(perim, fp, 0) < 0; +} + +int Sampler::overlap(Sampler &other) +{ + for (int i = 0; i < 4; i++) { + CvScalar p; + p = cvGet2D(other.perim, i, 0); + if (isinside((int)p.val[0], (int)p.val[1])) + return 1; + p = cvGet2D(perim, i, 0); + if (other.isinside((int)p.val[0], (int)p.val[1])) + return 1; + } + return 0; +} + +int Sampler::hasbars() +{ + return getpixel(9, 1) > getpixel(9, 0); +} + +void Sampler::timing() +{ + uint8 dark = getpixel(9, 0); + for (int i = 1; i < 3; i += 2) { + uint8 light = getpixel(9, i); + // if (light <= dark) + // goto endo; + dark = getpixel(9, i + 1); + // if (up <= down) + // goto endo; + } +} + +CvMat *Sampler::extract() +{ + // return a 10x10 CvMat for the current contents, 0 is black, 255 is white + // Sampler has (0,0) at bottom left, so invert Y + CvMat *r = cvCreateMat(10, 10, CV_8UC1); + for (int x = 0; x < 10; x++) + for (int y = 0; y < 10; y++) + *cvPtr2D(r, 9 - y, x) = (getpixel(x, y) < 128) ? 0 : 255; + return r; +} + +static void apron(CvMat *v) +{ + int r = v->rows; + int c = v->cols; + memset(cvPtr2D(v, 0, 0), 0x22, c); + memset(cvPtr2D(v, 1, 0), 0x22, c); + memset(cvPtr2D(v, r - 2, 0), 0x22, c); + memset(cvPtr2D(v, r - 1, 0), 0x22, c); + int y; + for (y = 2; y < r - 2; y++) { + uchar *lp = cvPtr2D(v, y, 0); + lp[0] = 0x22; + lp[1] = 0x22; + lp[c-2] = 0x22; + lp[c-1] = 0x22; + } +} + +static void cfollow(CvMat *src, CvMat *dst) +{ + int sx, sy; + uint8 *vpd = cvPtr2D(src, 0, 0); + for (sy = 0; sy < src->rows; sy++) { + short *wr = (short*)cvPtr2D(dst, sy, 0); + for (sx = 0; sx < src->cols; sx++) { + int x = sx; + int y = sy; + float e = 0; + int ontrack = true; + int dir; + + while (ontrack) { + dir = vpd[y * src->step + x]; + int xd = ((dir & 0xf) - 2); + int yd = ((dir >> 4) - 2); + e += (dir == 0x22) ? 999 : ((dir & 1) ? eincD : eincO); + x += xd; + y += yd; + if (e > 10.) { + float d = ((x - sx) * (x - sx)) + ((y - sy) * (y - sy)); + ontrack = d > (e * e); + } + } + if ((24 <= e) && (e < 999)) { + // printf("sx=%d, sy=%d, x=%d, y=%d\n", sx, sy, x, y); + *wr++ = x - sx; + *wr++ = y - sy; + } else { + *wr++ = 0; + *wr++ = 0; + } + } + } +} + +static uint8 gf256mul(uint8 a, uint8 b) +{ + return Alog[(Log[a] + Log[b]) % 255]; +} + +static int decode(Sampler &sa, code &cc) +{ + uint8 binary[8] = {0,0,0,0,0,0,0,0}; + uint8 b = 0; + + for (int i = 0; i < 64; i++) { + b = (b << 1) + (sa.getpixel(pickup[i].x, pickup[i].y) <= 128); + if ((i & 7) == 7) { + binary[i >> 3] = b; + b = 0; + } + } + + // Compute the 5 RS codewords for the 3 datawords + + uint8 c[5] = {0,0,0,0,0}; + { + int i, j; + uint8 a[5] = {228, 48, 15, 111, 62}; + int k = 5; + for (i = 0; i < 3; i++) { + uint8 t = binary[i] ^ c[4]; + for (j = k - 1; j != -1; j--) { + if (t == 0) + c[j] = 0; + else + c[j] = gf256mul(t, a[j]); + if (j > 0) + c[j] = c[j - 1] ^ c[j]; + } + } + } + + if ((c[4] == binary[3]) && + (c[3] == binary[4]) && + (c[2] == binary[5]) && + (c[1] == binary[6]) && + (c[0] == binary[7])) { + uint8 x = 0xff & (binary[0] - 1); + uint8 y = 0xff & (binary[1] - 1); + uint8 z = 0xff & (binary[2] - 1); + cc.msg[0] = x; + cc.msg[1] = y; + cc.msg[2] = z; + cc.msg[3] = 0; + cc.sa = sa; + cc.original = sa.extract(); + return 1; + } else { + return 0; + } +} + +static deque trailto(CvMat *v, int x, int y, CvMat *terminal) +{ + CvPoint np; + /* Return the last 10th of the trail of points following v from (x,y) + * to terminal + */ + + int ex = x + ((short*)cvPtr2D(terminal, y, x))[0]; + int ey = y + ((short*)cvPtr2D(terminal, y, x))[1]; + deque r; + while ((x != ex) || (y != ey)) { + np.x = x; + np.y = y; + r.push_back(np); + int dir = *cvPtr2D(v, y, x); + int xd = ((dir & 0xf) - 2); + int yd = ((dir >> 4) - 2); + x += xd; + y += yd; + } + + int l = r.size() * 9 / 10; + while (l--) + r.pop_front(); + return r; +} + +deque findcodes(CvMat *im) +{ + int r = im->rows; + int c = im->cols; + +#define SAMESIZE(nm, ty) CvMat *nm = cvCreateMat(r, c, ty); + + SAMESIZE(thresh, CV_8UC1) + SAMESIZE(vecpic, CV_8UC1) + SAMESIZE(vc, CV_8UC1) + SAMESIZE(vcc, CV_8UC1) + SAMESIZE(cxy, CV_16SC2) + SAMESIZE(ccxy, CV_16SC2) + + cvAdaptiveThreshold(im, thresh, 255.0, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 13); + { + int x, y; + int sstride = thresh->step; + int sw = thresh->cols; // source width + for (y = 2; y < thresh->rows - 2; y++) { + uint8 *ps = cvPtr2D(thresh, y, 0); + uint8 *pd = cvPtr2D(vecpic, y, 0); + uint8 *pvc = cvPtr2D(vc, y, 0); + uint8 *pvcc = cvPtr2D(vcc, y, 0); + for (x = 0; x < sw; x++) { + uint8 v = + (0x01 & ps[-2 * sstride]) | + (0x02 & ps[-sstride + 1]) | + (0x04 & ps[2]) | + (0x08 & ps[sstride + 1]) | + (0x10 & ps[2 * sstride]) | + (0x20 & ps[sstride - 1]) | + (0x40 & ps[-2]) | + (0x80 & ps[-sstride -1]); + *pd++ = v; + *pvc++ = cblk[v]; + *pvcc++ = ccblk[v]; + ps++; + } + } + apron(vc); + apron(vcc); + } + + cfollow(vc, cxy); + cfollow(vcc, ccxy); + + deque candidates; + { + int x, y; + int r = cxy->rows; + int c = cxy->cols; + for (y = 0; y < r; y++) { + __m64 *cd = (__m64 *)cvPtr2D(cxy, y, 0); + __m64 *ccd = (__m64 *)cvPtr2D(ccxy, y, 0); + for (x = 0; x < c; x += 4) { + __m128 cyxyxA = _mm_cvtpi16_ps(*cd++); + __m128 cyxyxB = _mm_cvtpi16_ps(*cd++); + __m128 cx = _mm_shuffle_ps(cyxyxA, cyxyxB, _MM_SHUFFLE(0, 2, 0, 2)); + __m128 cy = _mm_shuffle_ps(cyxyxA, cyxyxB, _MM_SHUFFLE(1, 3, 1, 3)); + __m128 cmag = _mm_sqrt_ps(cx * cx + cy * cy); + __m128 crmag = _mm_rcp_ps(cmag); + __m128 ncx = cx * crmag; + __m128 ncy = cy * crmag; + + __m128 ccyxyxA = _mm_cvtpi16_ps(*ccd++); + __m128 ccyxyxB = _mm_cvtpi16_ps(*ccd++); + __m128 ccx = _mm_shuffle_ps(ccyxyxA, ccyxyxB, _MM_SHUFFLE(0, 2, 0, 2)); + __m128 ccy = _mm_shuffle_ps(ccyxyxA, ccyxyxB, _MM_SHUFFLE(1, 3, 1, 3)); + __m128 ccmag = _mm_sqrt_ps(ccx * ccx + ccy * ccy); + __m128 ccrmag = _mm_rcp_ps(ccmag); + __m128 nccx = ccx * ccrmag; + __m128 nccy = ccy * ccrmag; + + __m128 dot = ncx * nccx + ncy * nccy; + // iscand = (cmag > 30) & (ccmag > 30) & (numpy.minimum(cmag, ccmag) * 1.1 > numpy.maximum(cmag, ccmag)) & (abs(dot) < 0.25) + __m128 iscand = _mm_and_ps(_mm_cmpgt_ps(cmag, Kf(30)), _mm_cmpgt_ps(ccmag, Kf(30))); + iscand = _mm_and_ps(iscand, _mm_cmpgt_ps(_mm_min_ps(cmag, ccmag) * Kf(1.1), _mm_max_ps(cmag, ccmag))); + iscand = _mm_and_ps(iscand, _mm_cmplt_ps(_mm_abs_ps(dot), Kf(0.25))); + + unsigned int result[4]; + *(__m128*)result = iscand; + int ix; + CvPoint np; + for (ix = 0; ix < 4; ix++) { + if (result[ix]) { + np.x = x + ix; + np.y = y; + candidates.push_back(np); + } + } + } + } + } + + deque codes; + size_t i, j, k; + while (!candidates.empty()) { + CvPoint o = candidates.front(); + candidates.pop_front(); + deque ptc = trailto(vc, o.x, o.y, cxy); + deque ptcc = trailto(vcc, o.x, o.y, ccxy); + for (j = 0; j < ptc.size(); j++) { + for (k = 0; k < ptcc.size(); k++) { + code cc; + Sampler sa(im, o, ptc[j], ptcc[k]); + for (i = 0; i < codes.size(); i++) { + if (sa.overlap(codes[i].sa)) + goto endo; + } + if (codes.size() > 0) { + printf("searching for more\n"); + } + if (decode(sa, cc)) { + codes.push_back(cc); + goto endo; + } + } + } +endo: ; // end search for this o + } + + cvFree(&thresh); + cvFree(&vecpic); + cvFree(&vc); + cvFree(&vcc); + cvFree(&cxy); + cvFree(&ccxy); + + deque rc; + for (i = 0; i < codes.size(); i++) { + DatamatrixCode cc; + strcpy(cc.msg, codes[i].msg); + cc.original = codes[i].original; + cc.corners = codes[i].sa.perim; + rc.push_back(cc); + } + return rc; +} diff --git a/modules/objdetect/src/followblk.h b/modules/objdetect/src/followblk.h new file mode 100644 index 000000000..5277a7e81 --- /dev/null +++ b/modules/objdetect/src/followblk.h @@ -0,0 +1,5 @@ +unsigned char cblk[256] = { 34,19,36,36,51,19,51,51,66,19,36,36,66,19,66,66,49,19,36,36,51,19,51,51,49,19,36,36,49,19,49,49,32,19,36,36,51,19,51,51,66,19,36,36,66,19,66,66,32,19,36,36,51,19,51,51,32,19,36,36,32,19,32,32,17,19,36,36,51,19,51,51,66,19,36,36,66,19,66,66,49,19,36,36,51,19,51,51,49,19,36,36,49,19,49,49,17,19,36,36,51,19,51,51,66,19,36,36,66,19,66,66,17,19,36,36,51,19,51,51,17,19,36,36,17,19,17,17,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,66,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,49,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,66,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,32,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,66,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,49,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,66,2,19,2,36,2,19,2,51,2,19,2,36,2,19,2,34 }; +unsigned char ccblk[256] = { 34,17,2,17,19,19,2,17,36,36,2,36,19,19,2,17,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,17,66,66,2,66,19,19,2,66,36,36,2,36,19,19,2,66,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,17,49,49,2,49,19,19,2,49,36,36,2,36,19,19,2,49,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,49,66,66,2,66,19,19,2,66,36,36,2,36,19,19,2,66,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,17,32,32,2,32,19,19,2,32,36,36,2,36,19,19,2,32,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,32,66,66,2,66,19,19,2,66,36,36,2,36,19,19,2,66,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,32,49,49,2,49,19,19,2,49,36,36,2,36,19,19,2,49,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,49,66,66,2,66,19,19,2,66,36,36,2,36,19,19,2,66,51,51,2,51,19,19,2,51,36,36,2,36,19,19,2,34 }; +static const CvPoint pickup[64] = { {7,6},{8,6},{7,5},{8,5},{1,5},{7,4},{8,4},{1,4},{1,8},{2,8},{1,7},{2,7},{3,7},{1,6},{2,6},{3,6},{3,2},{4,2},{3,1},{4,1},{5,1},{3,8},{4,8},{5,8},{6,1},{7,1},{6,8},{7,8},{8,8},{6,7},{7,7},{8,7},{4,7},{5,7},{4,6},{5,6},{6,6},{4,5},{5,5},{6,5},{2,5},{3,5},{2,4},{3,4},{4,4},{2,3},{3,3},{4,3},{8,3},{1,3},{8,2},{1,2},{2,2},{8,1},{1,1},{2,1},{5,4},{6,4},{5,3},{6,3},{7,3},{5,2},{6,2},{7,2} }; +static const uint8 Alog[256] = { 1,2,4,8,16,32,64,128,45,90,180,69,138,57,114,228,229,231,227,235,251,219,155,27,54,108,216,157,23,46,92,184,93,186,89,178,73,146,9,18,36,72,144,13,26,52,104,208,141,55,110,220,149,7,14,28,56,112,224,237,247,195,171,123,246,193,175,115,230,225,239,243,203,187,91,182,65,130,41,82,164,101,202,185,95,190,81,162,105,210,137,63,126,252,213,135,35,70,140,53,106,212,133,39,78,156,21,42,84,168,125,250,217,159,19,38,76,152,29,58,116,232,253,215,131,43,86,172,117,234,249,223,147,11,22,44,88,176,77,154,25,50,100,200,189,87,174,113,226,233,255,211,139,59,118,236,245,199,163,107,214,129,47,94,188,85,170,121,242,201,191,83,166,97,194,169,127,254,209,143,51,102,204,181,71,142,49,98,196,165,103,206,177,79,158,17,34,68,136,61,122,244,197,167,99,198,161,111,222,145,15,30,60,120,240,205,183,67,134,33,66,132,37,74,148,5,10,20,40,80,160,109,218,153,31,62,124,248,221,151,3,6,12,24,48,96,192,173,119,238,241,207,179,75,150,1 }; +static const uint8 Log[256] = { -255,255,1,240,2,225,241,53,3,38,226,133,242,43,54,210,4,195,39,114,227,106,134,28,243,140,44,23,55,118,211,234,5,219,196,96,40,222,115,103,228,78,107,125,135,8,29,162,244,186,141,180,45,99,24,49,56,13,119,153,212,199,235,91,6,76,220,217,197,11,97,184,41,36,223,253,116,138,104,193,229,86,79,171,108,165,126,145,136,34,9,74,30,32,163,84,245,173,187,204,142,81,181,190,46,88,100,159,25,231,50,207,57,147,14,67,120,128,154,248,213,167,200,63,236,110,92,176,7,161,77,124,221,102,218,95,198,90,12,152,98,48,185,179,42,209,37,132,224,52,254,239,117,233,139,22,105,27,194,113,230,206,87,158,80,189,172,203,109,175,166,62,127,247,146,66,137,192,35,252,10,183,75,216,31,83,33,73,164,144,85,170,246,65,174,61,188,202,205,157,143,169,82,72,182,215,191,251,47,178,89,151,101,94,160,123,26,112,232,21,51,238,208,131,58,69,148,18,15,16,68,17,121,149,129,19,155,59,249,70,214,250,168,71,201,156,64,60,237,130,111,20,93,122,177,150 }; diff --git a/modules/python/src/cv.cpp b/modules/python/src/cv.cpp index 0280fcd9f..7c321703f 100644 --- a/modules/python/src/cv.cpp +++ b/modules/python/src/cv.cpp @@ -3677,6 +3677,27 @@ static PyObject *pycvClipLine(PyObject *self, PyObject *args) } } +static PyObject *pyfindcodes(PyObject *self, PyObject *args) +{ + PyObject *pyim; + if (!PyArg_ParseTuple(args, "O", &pyim)) + return NULL; + + CvMat *image; + if (!convert_to_CvMat(pyim, &image, "image")) return NULL; + + std::deque codes = findcodes(image); + + PyObject *pycodes = PyList_New(codes.size()); + int i; + for (i = 0; i < codes.size(); i++) { + DatamatrixCode *pc = &codes[i]; + PyList_SetItem(pycodes, i, Py_BuildValue("(sOO)", pc->msg, FROM_CvMat(pc->corners), FROM_CvMat(pc->original))); + } + + return pycodes; +} + static PyObject *temp_test(PyObject *self, PyObject *args) { #if 0 @@ -3971,6 +3992,7 @@ static PyMethodDef methods[] = { //{"_HOGDetect", (PyCFunction)pycvHOGDetect, METH_KEYWORDS, "_HOGDetect(image, svm_classifier, win_stride=block_stride, locations=None, padding=(0,0), win_size=(64,128), block_size=(16,16), block_stride=(8,8), cell_size=(8,8), nbins=9, gammaCorrection=true) -> list_of_points"}, //{"_HOGDetectMultiScale", (PyCFunction)pycvHOGDetectMultiScale, METH_KEYWORDS, "_HOGDetectMultiScale(image, svm_classifier, win_stride=block_stride, scale=1.05, group_threshold=2, padding=(0,0), win_size=(64,128), block_size=(16,16), block_stride=(8,8), cell_size=(8,8), nbins=9, gammaCorrection=true) -> list_of_points"}, + {"findcodes", pyfindcodes, METH_VARARGS}, {"temp_test", temp_test, METH_VARARGS}, #include "generated1.i"