Refactoring
This commit is contained in:
Andrey Tkachenko
parent
35b9685eeb
commit
4b7f0433e9
229
src/Test.cpp
229
src/Test.cpp
@ -21,11 +21,10 @@ typedef struct {
|
|||||||
|
|
||||||
#define WINDOW_START 0
|
#define WINDOW_START 0
|
||||||
#define WINDOW_SIZE 80
|
#define WINDOW_SIZE 80
|
||||||
#define MATCH_SIZE_X 2
|
#define MATCH_SIZE_X 1
|
||||||
#define MATCH_SIZE_Y 2
|
#define MATCH_SIZE_Y 1
|
||||||
#define ERROR_LEVEL 1
|
#define ERROR_LEVEL 300
|
||||||
#define Y_CORRECTION 0
|
#define COLOR_SCALE 0
|
||||||
#define COLOR_SCALE 1
|
|
||||||
|
|
||||||
int getPixelValue(Mat* data, int x, int y, int offset = -1)
|
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;
|
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;
|
int sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;
|
||||||
float a, b;
|
float a, b;
|
||||||
int tmp;
|
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++) {
|
for (int i = x_from; i < x_to; i++) {
|
||||||
data = mat->ptr<uchar>(y, i);
|
data = mat->ptr<uchar>(y, i);
|
||||||
//data[0] = a * i ;
|
//data[0] = a * i ;
|
||||||
}
|
}*/
|
||||||
}
|
}
|
||||||
|
|
||||||
int match(StereoData ¶ms, int x, int y, int j) {
|
int match(StereoData ¶ms, int x, int y, int j, bool backwise=false) {
|
||||||
int error = 0;
|
int error = 0, offset;
|
||||||
int Rvalue, Gvalue, Bvalue;
|
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;
|
return 999;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int g = 0; g < MATCH_SIZE_Y; g++) {
|
for (int g = -MY2; g <= MY2; g++) {
|
||||||
if (y + g >= params.right->rows) {
|
if (y + g >= params.right->rows) {
|
||||||
return 999;
|
return 999;
|
||||||
}
|
}
|
||||||
|
|
||||||
Rvalue = abs(getPixelValue(params.left, x + i, y + g, 0) - getPixelValue(params.right, x + j + i, y + g, 0));
|
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, y + g, 1) - getPixelValue(params.right, x + j + i, y + g, 1));
|
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, y + g, 2) - getPixelValue(params.right, x + j + i, y + g, 2));
|
Bvalue = abs(getPixelValue(params.left, x + (i*sign), y + g, 2) - getPixelValue(params.right, offset, y + g, 2));
|
||||||
|
|
||||||
error += Rvalue * COLOR_SCALE;
|
error += Rvalue << COLOR_SCALE;
|
||||||
error += Gvalue * COLOR_SCALE;
|
error += Gvalue << COLOR_SCALE;
|
||||||
error += Bvalue * COLOR_SCALE;
|
error += Bvalue << COLOR_SCALE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return error;
|
return error;
|
||||||
}
|
}
|
||||||
|
|
||||||
int rmatch(StereoData ¶ms, int x, int y, int j) {
|
int closestMatch(StereoData& params, int cursor, int x, int y, bool backwise = false) {
|
||||||
int error = 0;
|
int tmp, tmpSmoothed=999, minErrorValue=999, matched=0;
|
||||||
|
int closest=999;
|
||||||
|
|
||||||
for (int i = 0; i < MATCH_SIZE_X; i++) {
|
for (int i = WINDOW_START; i < WINDOW_SIZE; i++) {
|
||||||
if (x - j - i < 0) {
|
tmp = match(params, x, y, i, backwise);
|
||||||
return 999;
|
|
||||||
|
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++) {
|
return matched ? closest : -1;
|
||||||
if (y + g >= params.left->rows) {
|
}
|
||||||
return 999;
|
|
||||||
|
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));
|
cursor = closest;
|
||||||
error += abs(getPixelValue(params.right, x - i, y + g, 1) - getPixelValue(params.left, x - j - i, y + g, 1));
|
putPixel(params.stereo, x - cursor, y, cursor);
|
||||||
error += abs(getPixelValue(params.right, x - i, y + g, 2) - getPixelValue(params.left, x - j - i, y + g, 2));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
preprecursor = precursor;
|
||||||
|
precursor = cursor;
|
||||||
}
|
}
|
||||||
|
|
||||||
return error;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int getPixelColor(int cursor)
|
void calcLine(StereoData& params, int y) {
|
||||||
{
|
int closest, diff;
|
||||||
float val = ((1.0 + cursor) / WINDOW_SIZE);
|
int cursor = WINDOW_SIZE/2;
|
||||||
return (val * val) * 255.0;
|
int precursor = WINDOW_SIZE/2;
|
||||||
}
|
int preprecursor = WINDOW_SIZE/2;
|
||||||
|
|
||||||
void putPixel(Mat* mat, int x, int y, int cursor)
|
for (int x = 0; x < params.stereo->cols; x++) {
|
||||||
{
|
closest = closestMatch(params, cursor, x, y);
|
||||||
uchar* data;
|
|
||||||
|
|
||||||
if (x >= 0 && x < mat->cols && y >= 0 && y < mat->rows) {
|
if (closest != -1) {
|
||||||
data = mat->ptr(y, x);
|
diff = closest - cursor;
|
||||||
data[0] = getPixelColor(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) {
|
void calcDepthMapMy2(StereoData ¶ms) {
|
||||||
int x, y, cursor, closest, tmp;
|
for (int y = 0; y < params.stereo->rows; y++) {
|
||||||
int minErrorValue, tmpSmoothed, matched;
|
calcLine(params, y);
|
||||||
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;
|
|
||||||
}*/
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user