added improved version of the variational stereo correspondence algorithm by Sergey Kosov

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);
     };

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)
 {
-	// TODO try pointers or assm
-	register int x, y;
-	Mat dst(img.size(), img.type());
-	dst.setTo(0);
-	for (x = 0; x < img.cols - 1; x++)
-		for (y = 0; y < img.rows; y++)
-			dst.at<float>(y, x) = img.at<float>(y, x + 1) - img.at<float>(y ,x);
-	return dst;
+	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<float>(y);
+        float* pDst = dst.ptr<float>(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 Gradient(Mat &img)
+static Mat getGradient(Mat &src)
 {
-	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;
+	register int x, y;
+	Mat dst(src.size(), src.type());
+	dst.setTo(0);
+	for (y = 0; y < src.rows - 1; y++) {
+		float *pSrc = src.ptr<float>(y);
+		float *pSrcF = src.ptr<float>(y + 1);
+		float *pDst = dst.ptr<float>(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 float g_c(Mat z, int x, int y, float l)
+static Mat getG_c(Mat &src, float l)
 {
-	return 0.5f*l / sqrtf(l*l + z.at<float>(y,x)*z.at<float>(y,x));
+	Mat dst(src.size(), src.type());
+	for (register int y = 0; y < src.rows; y++) {
+		float *pSrc = src.ptr<float>(y);
+		float *pDst = dst.ptr<float>(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_p(Mat z, int x, int y, float l)
+static Mat getG_p(Mat &src, float l)
 {
-	return 0.5f*l*l / (l*l + z.at<float>(y,x)*z.at<float>(y,x)) ;
+	Mat dst(src.size(), src.type());
+	for (register int y = 0; y < src.rows; y++) {
+		float *pSrc = src.ptr<float>(y);
+		float *pDst = dst.ptr<float>(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<float>(y);
+			float *pUu	= U.ptr<float>(y + 1);
+			float *pUd	= U.ptr<float>(y - 1);
+			float *pu	= u.ptr<float>(y);
+			float *pI1	= I1.ptr<float>(y);
+			float *pI2	= I2.ptr<float>(y);
+			float *pI2x = I2x.ptr<float>(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<float>(y); 
+					pG_cu	= g_c.ptr<float>(y + 1); 
+					pG_cd	= g_c.ptr<float>(y - 1); 
+					break;
+				case PENALIZATION_PERONA_MALIK: 
+					pG_p	= g_p.ptr<float>(y); 
+					pG_pu	= g_p.ptr<float>(y + 1); 
+					pG_pd	= g_p.ptr<float>(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<float>(y,x) > maxDisp * scale) {fi*=1000; U.at<float>(y,x) = static_cast<float>(maxDisp * scale);} 
-					if (U.at<float>(y,x) < minDisp * scale) {fi*=1000; U.at<float>(y,x) = static_cast<float>(minDisp * scale);} 
+					if (pU[x] > maxDisp * scale) {fi *= 1000; pU[x] = static_cast<float>(maxDisp * scale);} 
+					if (pU[x] < minDisp * scale) {fi *= 1000; pU[x] = static_cast<float>(minDisp * scale);} 
 				}
 
-				int A = (int) (U.at<float>(y,x));
-				int neg = 0; if (U.at<float>(y,x) <= 0) neg = -1;
+				int A = static_cast<int>(pU[x]);
+				int neg = 0; if (pU[x] <= 0) neg = -1;
 
-				if (x + A >= u.cols)
-					u.at<float>(y, x) = U.at<float>(y, u.cols - A - 1);
+				if (x + A > width)
+					pu[x] = pU[width - A];
 				else if (x + A + neg < 0)
-					u.at<float>(y, x) = U.at<float>(y, - A + 2);
+					pu[x] = pU[- A + 2];
 				else { 
-					u.at<float>(y, x) = A + (I2x.at<float>(y, x + A + neg) * (I1.at<float>(y, x) - I2.at<float>(y, x + A))
-										  + fi * (gr * U.at<float>(y, x + 1) + gl * U.at<float>(y, x - 1) + gu * U.at<float>(y + 1, x) + gd * U.at<float>(y - 1, x) - gc * A)) 
-										  / (I2x.at<float>(y, x + A + neg) * I2x.at<float>(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<float>(0, x) = u.at<float>(1, x);
-			u.at<float>(u.rows - 1, x) = u.at<float>(u.rows - 2, x);
-		}//x
-		for (y = 0; y < u.rows; y++) {
-			u.at<float>(y, 0) = u.at<float>(y, 1);
-			u.at<float>(y, u.cols - 1) = u.at<float>(y, u.cols - 2);
+			u.at<float>(height, x) = u.at<float>(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<double>(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);

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 <left_image> <right_image> [--algorithm=bm|sgbm|hh|var] [--blocksize=<block_size>]\n"
-           "[--max-disparity=<max_disparity>] [-i <intrinsic_filename>] [-e <extrinsic_filename>]\n"
+           "[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i <intrinsic_filename>] [-e <extrinsic_filename>]\n"
            "[--no-display] [-o <disparity_image>] [-p <point_cloud_file>]\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;