Refactoring
This commit is contained in:
Andrey Tkachenko
parent
35b9685eeb
commit
4b7f0433e9
207
src/Test.cpp
207
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,90 +83,50 @@ void normalize(Mat* mat, int x_from, int x_to, int y)
|
||||
for (int i = x_from; i < x_to; i++) {
|
||||
data = mat->ptr<uchar>(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;
|
||||
|
||||
for (int i = 0; i < MATCH_SIZE_X; i++) {
|
||||
if (x - j - i < 0) {
|
||||
return 999;
|
||||
}
|
||||
|
||||
for (int g = 0; g < MATCH_SIZE_Y; g++) {
|
||||
if (y + g >= params.left->rows) {
|
||||
return 999;
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
int getPixelColor(int cursor)
|
||||
{
|
||||
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 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;
|
||||
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 = WINDOW_START; i < WINDOW_SIZE; i++) {
|
||||
tmp = match(params, x, y, i);
|
||||
tmpSmoothed = tmp + (0 * abs(cursor - 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;
|
||||
@ -163,59 +139,66 @@ void calcDepthMapMy2(StereoData ¶ms) {
|
||||
}
|
||||
}
|
||||
|
||||
if (matched) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
cursor = closest;
|
||||
|
||||
/*if (cursor-precursor > 0 && cursor-precursor < 10) {
|
||||
for (int i = precursor; i < cursor; i++) {
|
||||
putPixel(params.stereo, x + i, y, 0);
|
||||
putPixel(params.stereo, x - cursor, y, cursor);
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
//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) {
|
||||
void calcLine(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 = 0; x < params.stereo->cols; x++) {
|
||||
closest = closestMatch(params, cursor, x, y);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
//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);
|
||||
//}
|
||||
//}
|
||||
|
||||
preprecursor = precursor;
|
||||
precursor = cursor;
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
void calcDepthMapMy2(StereoData ¶ms) {
|
||||
for (int y = 0; y < params.stereo->rows; y++) {
|
||||
calcLine(params, y);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user