From 7702fa4d61b31e2516ad3af566174e1ea2543ce1 Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Thu, 14 Jul 2011 15:30:28 +0000 Subject: [PATCH] added improved version of the variational stereo correspondence algorithm by Sergey Kosov --- .../include/opencv2/contrib/contrib.hpp | 5 +- modules/contrib/src/stereovar.cpp | 218 +++++++++++++----- samples/cpp/stereo_match.cpp | 47 ++-- 3 files changed, 193 insertions(+), 77 deletions(-) diff --git a/modules/contrib/include/opencv2/contrib/contrib.hpp b/modules/contrib/include/opencv2/contrib/contrib.hpp index 45acfa78e0..9907a94d71 100644 --- a/modules/contrib/include/opencv2/contrib/contrib.hpp +++ b/modules/contrib/include/opencv2/contrib/contrib.hpp @@ -568,7 +568,7 @@ namespace cv { public: // Flags - enum {USE_INITIAL_DISPARITY = 1, USE_EQUALIZE_HIST = 2, USE_SMART_ID = 4, USE_MEDIAN_FILTERING = 8}; + enum {USE_INITIAL_DISPARITY = 1, USE_EQUALIZE_HIST = 2, USE_SMART_ID = 4, USE_AUTO_PARAMS = 8, USE_MEDIAN_FILTERING = 16}; enum {CYCLE_O, CYCLE_V}; enum {PENALIZATION_TICHONOV, PENALIZATION_CHARBONNIER, PENALIZATION_PERONA_MALIK}; @@ -598,7 +598,8 @@ namespace cv CV_PROP_RW int flags; private: - void FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level); + void autoParams(); + void FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level); void VCycle_MyFAS(Mat &I1_h, Mat &I2_h, Mat &I2x_h, Mat &u_h, int level); void VariationalSolver(Mat &I1_h, Mat &I2_h, Mat &I2x_h, Mat &u_h, int level); }; diff --git a/modules/contrib/src/stereovar.cpp b/modules/contrib/src/stereovar.cpp index cc6c53b60d..df5a62981a 100755 --- a/modules/contrib/src/stereovar.cpp +++ b/modules/contrib/src/stereovar.cpp @@ -53,7 +53,7 @@ namespace cv { -StereoVar::StereoVar() : levels(3), pyrScale(0.5), nIt(3), minDisp(0), maxDisp(16), poly_n(5), poly_sigma(1.2), fi(1000.0f), lambda(0.0f), penalization(PENALIZATION_TICHONOV), cycle(CYCLE_V), flags(USE_SMART_ID) +StereoVar::StereoVar() : levels(3), pyrScale(0.5), nIt(5), minDisp(0), maxDisp(16), poly_n(3), poly_sigma(0), fi(25.0f), lambda(0.03f), penalization(PENALIZATION_TICHONOV), cycle(CYCLE_V), flags(USE_SMART_ID | USE_AUTO_PARAMS) { } @@ -65,111 +65,172 @@ StereoVar::~StereoVar() { } -static Mat diffX(Mat &img) +static Mat diffX(Mat &src) +{ + register int x, y, cols = src.cols - 1; + Mat dst(src.size(), src.type()); + for(y = 0; y < src.rows; y++){ + const float* pSrc = src.ptr(y); + float* pDst = dst.ptr(y); + for (x = 0; x <= cols - 8; x += 8) { + __m128 a0 = _mm_loadu_ps(pSrc + x); + __m128 b0 = _mm_loadu_ps(pSrc + x + 1); + __m128 a1 = _mm_loadu_ps(pSrc + x + 4); + __m128 b1 = _mm_loadu_ps(pSrc + x + 5); + b0 = _mm_sub_ps(b0, a0); + b1 = _mm_sub_ps(b1, a1); + _mm_storeu_ps(pDst + x, b0); + _mm_storeu_ps(pDst + x + 4, b1); + } + for( ; x < cols; x++) pDst[x] = pSrc[x+1] - pSrc[x]; + pDst[cols] = 0.f; + } + return dst; +} + +static Mat getGradient(Mat &src) { - // TODO try pointers or assm register int x, y; - Mat dst(img.size(), img.type()); + Mat dst(src.size(), src.type()); dst.setTo(0); - for (x = 0; x < img.cols - 1; x++) - for (y = 0; y < img.rows; y++) - dst.at(y, x) = img.at(y, x + 1) - img.at(y ,x); + for (y = 0; y < src.rows - 1; y++) { + float *pSrc = src.ptr(y); + float *pSrcF = src.ptr(y + 1); + float *pDst = dst.ptr(y); + for (x = 0; x < src.cols - 1; x++) + pDst[x] = fabs(pSrc[x + 1] - pSrc[x]) + fabs(pSrcF[x] - pSrc[x]); + } return dst; } -static Mat Gradient(Mat &img) +static Mat getG_c(Mat &src, float l) { - Mat sobel, sobelX, sobelY; - img.copyTo(sobelX); - img.copyTo(sobelY); - Sobel(img, sobelX, sobelX.type(), 1, 0, 1); - Sobel(img, sobelY, sobelY.type(), 0, 1, 1); - sobelX = abs(sobelX); - sobelY = abs(sobelY); - add(sobelX, sobelY, sobel); - sobelX.release(); - sobelY.release(); - return sobel; + Mat dst(src.size(), src.type()); + for (register int y = 0; y < src.rows; y++) { + float *pSrc = src.ptr(y); + float *pDst = dst.ptr(y); + for (register int x = 0; x < src.cols; x++) + pDst[x] = 0.5f*l / sqrtf(l*l + pSrc[x]*pSrc[x]); + } + return dst; } -static float g_c(Mat z, int x, int y, float l) +static Mat getG_p(Mat &src, float l) { - return 0.5f*l / sqrtf(l*l + z.at(y,x)*z.at(y,x)); -} - -static float g_p(Mat z, int x, int y, float l) -{ - return 0.5f*l*l / (l*l + z.at(y,x)*z.at(y,x)) ; + Mat dst(src.size(), src.type()); + for (register int y = 0; y < src.rows; y++) { + float *pSrc = src.ptr(y); + float *pDst = dst.ptr(y); + for (register int x = 0; x < src.cols; x++) + pDst[x] = 0.5f*l*l / (l*l + pSrc[x]*pSrc[x]); + } + return dst; } void StereoVar::VariationalSolver(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level) { register int n, x, y; float gl = 1, gr = 1, gu = 1, gd = 1, gc = 4; + Mat g_c, g_p; Mat U; - Mat Sobel; u.copyTo(U); int N = nIt; float l = lambda; float Fi = fi; - double scale = pow(pyrScale, (double) level); - if (flags & USE_SMART_ID) { + + if (flags & USE_SMART_ID) { + double scale = pow(pyrScale, (double) level) * (1 + pyrScale); N = (int) (N / scale); - Fi /= (float) scale; - l *= (float) scale; } + + double scale = pow(pyrScale, (double) level); + Fi /= (float) scale; + l *= (float) scale; + + int width = u.cols - 1; + int height = u.rows - 1; for (n = 0; n < N; n++) { - if (penalization != PENALIZATION_TICHONOV) {if(!Sobel.empty()) Sobel.release(); Sobel = Gradient(U);} - for (x = 1; x < u.cols - 1; x++) { - for (y = 1 ; y < u.rows - 1; y++) { + if (penalization != PENALIZATION_TICHONOV) { + Mat gradient = getGradient(U); + switch (penalization) { + case PENALIZATION_CHARBONNIER: g_c = getG_c(gradient, l); break; + case PENALIZATION_PERONA_MALIK: g_p = getG_p(gradient, l); break; + } + gradient.release(); + } + for (y = 1 ; y < height; y++) { + float *pU = U.ptr(y); + float *pUu = U.ptr(y + 1); + float *pUd = U.ptr(y - 1); + float *pu = u.ptr(y); + float *pI1 = I1.ptr(y); + float *pI2 = I2.ptr(y); + float *pI2x = I2x.ptr(y); + float *pG_c = NULL, *pG_cu = NULL, *pG_cd = NULL; + float *pG_p = NULL, *pG_pu = NULL, *pG_pd = NULL; + switch (penalization) { + case PENALIZATION_CHARBONNIER: + pG_c = g_c.ptr(y); + pG_cu = g_c.ptr(y + 1); + pG_cd = g_c.ptr(y - 1); + break; + case PENALIZATION_PERONA_MALIK: + pG_p = g_p.ptr(y); + pG_pu = g_p.ptr(y + 1); + pG_pd = g_p.ptr(y - 1); + break; + } + for (x = 1; x < width; x++) { switch (penalization) { case PENALIZATION_CHARBONNIER: - gc = g_c(Sobel, x, y, l); - gl = gc + g_c(Sobel, x - 1, y, l); - gr = gc + g_c(Sobel, x + 1, y, l); - gu = gc + g_c(Sobel, x, y + 1, l); - gd = gc + g_c(Sobel, x, y - 1, l); - gc = gl + gr + gu + gd; + gc = pG_c[x]; + gl = gc + pG_c[x - 1]; + gr = gc + pG_c[x + 1]; + gu = gc + pG_cu[x]; + gd = gc + pG_cd[x]; + gc = gl + gr + gu + gd; break; case PENALIZATION_PERONA_MALIK: - gc = g_p(Sobel, x, y, l); - gl = gc + g_p(Sobel, x - 1, y, l); - gr = gc + g_p(Sobel, x + 1, y, l); - gu = gc + g_p(Sobel, x, y + 1, l); - gd = gc + g_p(Sobel, x, y - 1, l); + gc = pG_p[x]; + gl = gc + pG_p[x - 1]; + gr = gc + pG_p[x + 1]; + gu = gc + pG_pu[x]; + gd = gc + pG_pd[x]; gc = gl + gr + gu + gd; break; } float fi = Fi; if (maxDisp > minDisp) { - if (U.at(y,x) > maxDisp * scale) {fi*=1000; U.at(y,x) = static_cast(maxDisp * scale);} - if (U.at(y,x) < minDisp * scale) {fi*=1000; U.at(y,x) = static_cast(minDisp * scale);} + if (pU[x] > maxDisp * scale) {fi *= 1000; pU[x] = static_cast(maxDisp * scale);} + if (pU[x] < minDisp * scale) {fi *= 1000; pU[x] = static_cast(minDisp * scale);} } - int A = (int) (U.at(y,x)); - int neg = 0; if (U.at(y,x) <= 0) neg = -1; + int A = static_cast(pU[x]); + int neg = 0; if (pU[x] <= 0) neg = -1; - if (x + A >= u.cols) - u.at(y, x) = U.at(y, u.cols - A - 1); + if (x + A > width) + pu[x] = pU[width - A]; else if (x + A + neg < 0) - u.at(y, x) = U.at(y, - A + 2); + pu[x] = pU[- A + 2]; else { - u.at(y, x) = A + (I2x.at(y, x + A + neg) * (I1.at(y, x) - I2.at(y, x + A)) - + fi * (gr * U.at(y, x + 1) + gl * U.at(y, x - 1) + gu * U.at(y + 1, x) + gd * U.at(y - 1, x) - gc * A)) - / (I2x.at(y, x + A + neg) * I2x.at(y, x + A + neg) + gc * fi) ; + pu[x] = A + (pI2x[x + A + neg] * (pI1[x] - pI2[x + A]) + + fi * (gr * pU[x + 1] + gl * pU[x - 1] + gu * pUu[x] + gd * pUd[x] - gc * A)) + / (pI2x[x + A + neg] * pI2x[x + A + neg] + gc * fi) ; } - }//y + }// x + pu[0] = pu[1]; + pu[width] = pu[width - 1]; + }// y + for (x = 0; x <= width; x++) { u.at(0, x) = u.at(1, x); - u.at(u.rows - 1, x) = u.at(u.rows - 2, x); - }//x - for (y = 0; y < u.rows; y++) { - u.at(y, 0) = u.at(y, 1); - u.at(y, u.cols - 1) = u.at(y, u.cols - 2); + u.at(height, x) = u.at(height - 1, x); } u.copyTo(U); + if (!g_c.empty()) g_c.release(); + if (!g_p.empty()) g_p.release(); }//n } @@ -251,15 +312,43 @@ void StereoVar::FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level) u_h.release(); level--; + if ((flags & USE_AUTO_PARAMS) && (level < levels / 3)) { + penalization = PENALIZATION_PERONA_MALIK; + fi *= 100; + flags -= USE_AUTO_PARAMS; + autoParams(); + } if (flags & USE_MEDIAN_FILTERING) medianBlur(u, u, 3); if (level >= 0) FMG(I1, I2, I2x, u, level); } +void StereoVar::autoParams() +{ + int maxD = MAX(labs(maxDisp), labs(minDisp)); + + if (!maxD) pyrScale = 0.85; + else if (maxD < 8) pyrScale = 0.5; + else if (maxD < 64) pyrScale = 0.5 + static_cast(maxD - 8) * 0.00625; + else pyrScale = 0.85; + + if (maxD) { + levels = 0; + while ( pow(pyrScale, levels) * maxD > 1.5) levels ++; + levels++; + } + + switch(penalization) { + case PENALIZATION_TICHONOV: cycle = CYCLE_V; break; + case PENALIZATION_CHARBONNIER: cycle = CYCLE_O; break; + case PENALIZATION_PERONA_MALIK: cycle = CYCLE_O; break; + } +} + void StereoVar::operator ()( const Mat& left, const Mat& right, Mat& disp ) { CV_Assert(left.size() == right.size() && left.type() == right.type()); CvSize imgSize = left.size(); - int MaxD = MAX(std::abs(minDisp), std::abs(maxDisp)); + int MaxD = MAX(labs(minDisp), labs(maxDisp)); int SignD = 1; if (MIN(minDisp, maxDisp) < 0) SignD = -1; if (minDisp >= maxDisp) {MaxD = 256; SignD = 1;} @@ -290,6 +379,11 @@ void StereoVar::operator ()( const Mat& left, const Mat& right, Mat& disp ) GaussianBlur(rightgray, rightgray, cvSize(poly_n, poly_n), poly_sigma); } + if (flags & USE_AUTO_PARAMS) { + penalization = PENALIZATION_TICHONOV; + autoParams(); + } + Mat I1, I2; leftgray.convertTo(I1, CV_32FC1); rightgray.convertTo(I2, CV_32FC1); diff --git a/samples/cpp/stereo_match.cpp b/samples/cpp/stereo_match.cpp index 245c9ed389..08f67e3239 100644 --- a/samples/cpp/stereo_match.cpp +++ b/samples/cpp/stereo_match.cpp @@ -20,7 +20,7 @@ void print_help() { printf("\nDemo stereo matching converting L and R images into disparity and point clouds\n"); printf("\nUsage: stereo_match [--algorithm=bm|sgbm|hh|var] [--blocksize=]\n" - "[--max-disparity=] [-i ] [-e ]\n" + "[--max-disparity=] [--scale=scale_factor>] [-i ] [-e ]\n" "[--no-display] [-o ] [-p ]\n"); } @@ -40,18 +40,18 @@ void saveXYZ(const char* filename, const Mat& mat) fclose(fp); } - int main(int argc, char** argv) { const char* algorithm_opt = "--algorithm="; const char* maxdisp_opt = "--max-disparity="; const char* blocksize_opt = "--blocksize="; const char* nodisplay_opt = "--no-display="; + const char* scale_opt = "--scale="; if(argc < 3) { print_help(); - return 0; + return 0; } const char* img1_filename = 0; const char* img2_filename = 0; @@ -64,6 +64,7 @@ int main(int argc, char** argv) int alg = STEREO_SGBM; int SADWindowSize = 0, numberOfDisparities = 0; bool no_display = false; + float scale = 1.f; StereoBM bm; StereoSGBM sgbm; @@ -111,6 +112,14 @@ int main(int argc, char** argv) return -1; } } + else if( strncmp(argv[i], scale_opt, strlen(scale_opt)) == 0 ) + { + if( sscanf( argv[i] + strlen(scale_opt), "%f", &scale ) != 1 || scale < 0 ) + { + printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n"); + return -1; + } + } else if( strcmp(argv[i], nodisplay_opt) == 0 ) no_display = true; else if( strcmp(argv[i], "-i" ) == 0 ) @@ -149,6 +158,17 @@ int main(int argc, char** argv) int color_mode = alg == STEREO_BM ? 0 : -1; Mat img1 = imread(img1_filename, color_mode); Mat img2 = imread(img2_filename, color_mode); + + if( scale != 1.f ) + { + Mat temp1, temp2; + int method = scale < 1 ? INTER_AREA : INTER_CUBIC; + resize(img1, temp1, Size(), scale, scale, method); + img1 = temp1; + resize(img2, temp2, Size(), scale, scale, method); + img2 = temp2; + } + Size img_size = img1.size(); Rect roi1, roi2; @@ -224,18 +244,18 @@ int main(int argc, char** argv) sgbm.disp12MaxDiff = 1; sgbm.fullDP = alg == STEREO_HH; - var.levels = 6; - var.pyrScale = 0.6; - var.nIt = 3; - var.minDisp = -numberOfDisparities; + var.levels = 3; // ignored with USE_AUTO_PARAMS + var.pyrScale = 0.5; // ignored with USE_AUTO_PARAMS + var.nIt = 25; + var.minDisp = -numberOfDisparities; var.maxDisp = 0; var.poly_n = 3; var.poly_sigma = 0.0; - var.fi = 5.0f; - var.lambda = 0.1; - var.penalization = var.PENALIZATION_TICHONOV; - var.cycle = var.CYCLE_V; - var.flags = var.USE_SMART_ID | var.USE_INITIAL_DISPARITY | 1 * var.USE_MEDIAN_FILTERING ; + var.fi = 15.0f; + var.lambda = 0.03f; + var.penalization = var.PENALIZATION_TICHONOV; // ignored with USE_AUTO_PARAMS + var.cycle = var.CYCLE_V; // ignored with USE_AUTO_PARAMS + var.flags = var.USE_SMART_ID | var.USE_AUTO_PARAMS | var.USE_INITIAL_DISPARITY | var.USE_MEDIAN_FILTERING ; Mat disp, disp8; //Mat img1p, img2p, dispp; @@ -245,8 +265,9 @@ int main(int argc, char** argv) int64 t = getTickCount(); if( alg == STEREO_BM ) bm(img1, img2, disp); - else if( alg == STEREO_VAR ) + else if( alg == STEREO_VAR ) { var(img1, img2, disp); + } else if( alg == STEREO_SGBM || alg == STEREO_HH ) sgbm(img1, img2, disp); t = getTickCount() - t;