From 4b7f0433e97396aa76b1b42fdd894ce50d4a6b59 Mon Sep 17 00:00:00 2001 From: Andrey Tkachenko Date: Fri, 20 Jun 2014 20:55:43 +0400 Subject: [PATCH] Refactoring --- src/Test.cpp | 229 ++++++++++++++++++++++++--------------------------- 1 file changed, 106 insertions(+), 123 deletions(-) diff --git a/src/Test.cpp b/src/Test.cpp index aa68d23..5fd840a 100644 --- a/src/Test.cpp +++ b/src/Test.cpp @@ -21,11 +21,10 @@ typedef struct { #define WINDOW_START 0 #define WINDOW_SIZE 80 -#define MATCH_SIZE_X 2 -#define MATCH_SIZE_Y 2 -#define ERROR_LEVEL 1 -#define Y_CORRECTION 0 -#define COLOR_SCALE 1 +#define MATCH_SIZE_X 1 +#define MATCH_SIZE_Y 1 +#define ERROR_LEVEL 300 +#define COLOR_SCALE 0 int getPixelValue(Mat* data, int x, int y, int offset = -1) { @@ -45,9 +44,26 @@ int getPixelValue(Mat* data, int x, int y, int offset = -1) return value; } -void normalize(Mat* mat, int x_from, int x_to, int y) + +int getPixelColor(int cursor) { - int n = x_to - x_from, val; + float val = ((1.0 + cursor) / WINDOW_SIZE); + return (val * val) * 255.0; +} + +void putPixel(Mat* mat, int x, int y, int cursor) +{ + uchar* data; + + if (x >= 0 && x < mat->cols && y >= 0 && y < mat->rows) { + data = mat->ptr(y, x); + data[0] = getPixelColor(cursor); + } +} + +void normalize(StereoData& params, int x_from, int x_to, int y) +{ + /*int n = x_to - x_from, val; int sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0; float a, b; int tmp; @@ -67,155 +83,122 @@ void normalize(Mat* mat, int x_from, int x_to, int y) for (int i = x_from; i < x_to; i++) { data = mat->ptr(y, i); //data[0] = a * i ; - } + }*/ } -int match(StereoData ¶ms, int x, int y, int j) { - int error = 0; +int match(StereoData ¶ms, int x, int y, int j, bool backwise=false) { + int error = 0, offset; int Rvalue, Gvalue, Bvalue; + int sign=backwise?-1:1; + int MX2 = MATCH_SIZE_X >> 1, MY2 = MATCH_SIZE_Y >> 1; - for (int i = 0; i < MATCH_SIZE_X; i++) { - if (x + j + i >= params.right->cols) { + + for (int i = -MX2; i <= MX2; i++) { + offset = x + (j*sign) + (i*sign); + if (offset < 0 || offset >= params.right->cols) { return 999; } - for (int g = 0; g < MATCH_SIZE_Y; g++) { + for (int g = -MY2; g <= MY2; g++) { if (y + g >= params.right->rows) { return 999; } - Rvalue = abs(getPixelValue(params.left, x + i, y + g, 0) - getPixelValue(params.right, x + j + i, y + g, 0)); - Gvalue = abs(getPixelValue(params.left, x + i, y + g, 1) - getPixelValue(params.right, x + j + i, y + g, 1)); - Bvalue = abs(getPixelValue(params.left, x + i, y + g, 2) - getPixelValue(params.right, x + j + i, y + g, 2)); + Rvalue = abs(getPixelValue(params.left, x + (i*sign), y + g, 0) - getPixelValue(params.right, offset, y + g, 0)); + Gvalue = abs(getPixelValue(params.left, x + (i*sign), y + g, 1) - getPixelValue(params.right, offset, y + g, 1)); + Bvalue = abs(getPixelValue(params.left, x + (i*sign), y + g, 2) - getPixelValue(params.right, offset, y + g, 2)); - error += Rvalue * COLOR_SCALE; - error += Gvalue * COLOR_SCALE; - error += Bvalue * COLOR_SCALE; + error += Rvalue << COLOR_SCALE; + error += Gvalue << COLOR_SCALE; + error += Bvalue << COLOR_SCALE; } } return error; } -int rmatch(StereoData ¶ms, int x, int y, int j) { - int error = 0; +int closestMatch(StereoData& params, int cursor, int x, int y, bool backwise = false) { + int tmp, tmpSmoothed=999, minErrorValue=999, matched=0; + int closest=999; - for (int i = 0; i < MATCH_SIZE_X; i++) { - if (x - j - i < 0) { - return 999; + for (int i = WINDOW_START; i < WINDOW_SIZE; i++) { + tmp = match(params, x, y, i, backwise); + + if (tmp > ERROR_LEVEL) continue; + + tmpSmoothed = tmp + (0.1 * abs(cursor - i)); + if (tmpSmoothed < minErrorValue) { + minErrorValue = tmpSmoothed; + closest = i; + matched = 1; + } else if (tmpSmoothed == minErrorValue) { + if (abs(cursor - i) < abs(cursor - closest)) { + closest = i; + matched++; + } } + } - for (int g = 0; g < MATCH_SIZE_Y; g++) { - if (y + g >= params.left->rows) { - return 999; + return matched ? closest : -1; +} + +void calcLineBackwise(StereoData& params, int y) { + int closest, diff; + int cursor = WINDOW_SIZE/2; + int precursor = WINDOW_SIZE/2; + int preprecursor = WINDOW_SIZE/2; + + for (int x = params.stereo->cols - 1; x >= 0 ; x--) { + closest = closestMatch(params, cursor, x, y, true); + + if (closest != -1) { + diff = closest - cursor; + + if (diff > 0 /*&& diff < 5*/) { + for (int i = cursor; i < closest; i++) { + putPixel(params.stereo, x - i, y, 0); + } } - error += abs(getPixelValue(params.right, x - i, y + g, 0) - getPixelValue(params.left, x - j - i, y + g, 0)); - error += abs(getPixelValue(params.right, x - i, y + g, 1) - getPixelValue(params.left, x - j - i, y + g, 1)); - error += abs(getPixelValue(params.right, x - i, y + g, 2) - getPixelValue(params.left, x - j - i, y + g, 2)); + cursor = closest; + putPixel(params.stereo, x - cursor, y, cursor); } + + preprecursor = precursor; + precursor = cursor; } - - return error; } -int getPixelColor(int cursor) -{ - float val = ((1.0 + cursor) / WINDOW_SIZE); - return (val * val) * 255.0; -} +void calcLine(StereoData& params, int y) { + int closest, diff; + int cursor = WINDOW_SIZE/2; + int precursor = WINDOW_SIZE/2; + int preprecursor = WINDOW_SIZE/2; -void putPixel(Mat* mat, int x, int y, int cursor) -{ - uchar* data; + for (int x = 0; x < params.stereo->cols; x++) { + closest = closestMatch(params, cursor, x, y); - if (x >= 0 && x < mat->cols && y >= 0 && y < mat->rows) { - data = mat->ptr(y, x); - data[0] = getPixelColor(cursor); + if (closest != -1) { + diff = closest - cursor; + + if (diff > 0 && diff < 5) { + for (int i = cursor; i <= closest; i++) { + putPixel(params.stereo, x + i, y, 0); + } + } + + cursor = closest; + putPixel(params.stereo, x + cursor, y, cursor); + } + + preprecursor = precursor; + precursor = cursor; } } void calcDepthMapMy2(StereoData ¶ms) { - int x, y, cursor, closest, tmp; - int minErrorValue, tmpSmoothed, matched; - int precursor, preprecursor; - - for (y = 0; y < params.stereo->rows; y++) { - cursor = WINDOW_SIZE/2; - precursor = WINDOW_SIZE/2; - preprecursor = WINDOW_SIZE/2; - for (x = 0; x < params.stereo->cols; x++) { - closest = 999; minErrorValue = 999;matched=0; - minErrorValue=999; - - for (int i = WINDOW_START; i < WINDOW_SIZE; i++) { - tmp = match(params, x, y, i); - tmpSmoothed = tmp + (0 * abs(cursor - i)); - if (tmpSmoothed < minErrorValue) { - minErrorValue = tmpSmoothed; - closest = i; - matched = 1; - } else if (tmpSmoothed == minErrorValue) { - if (abs(cursor - i) < abs(cursor - closest)) { - closest = i; - matched++; - } - } - } - - if (matched) { - cursor = closest; - - /*if (cursor-precursor > 0 && cursor-precursor < 10) { - for (int i = precursor; i < cursor; i++) { - putPixel(params.stereo, x + i, y, 0); - } - }*/ - } - - //if (cursor - precursor != 0) { - putPixel(params.stereo, x + cursor, y, cursor); - //} - - preprecursor = precursor; - precursor = cursor; - } - - - /*cursor = 25;precursor=25; - for (x = params.stereo->cols; x >= 0; x--) { - closest = 999; minErrorValue = 999;matched=0; - minErrorValue=999; - - for (int i = WINDOW_START; i < WINDOW_SIZE; i++) { - tmp = rmatch(params, x, y, i); - tmpSmoothed = tmp + (0.0 * abs(cursor - i)); - if (tmpSmoothed < minErrorValue) { - minErrorValue = tmpSmoothed; - closest = i; - matched = 1; - } else if (tmpSmoothed == minErrorValue) { - if (abs(cursor - i) < abs(cursor - closest)) { - closest = i; - matched++; - } - } - } - - if (matched) { - cursor = closest; - } - - //if (abs(cursor - precursor)) { - //if (abs(getPixelColor(cursor) - getPixelValue(params.stereo, x, y)) < 50) { - putPixel(params.stereo, x, y, cursor); - //} else { - /// putPixel(params.stereo, x, y, 0); - //} - //} - - precursor=cursor; - }*/ + for (int y = 0; y < params.stereo->rows; y++) { + calcLine(params, y); } }