1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
44 This is a modification of the variational stereo correspondence algorithm, described in:
45 S. Kosov, T. Thormaehlen, H.-P. Seidel "Accurate Real-Time Disparity Estimation with Variational Methods"
46 Proceedings of the 5th International Symposium on Visual Computing, Vegas, USA
48 This code is written by Sergey G. Kosov for "Visir PX" application as part of Project X (www.project-10.de)
51 #include "precomp.hpp"
56 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)
60 StereoVar::StereoVar(int _levels, double _pyrScale, int _nIt, int _minDisp, int _maxDisp, int _poly_n, double _poly_sigma, float _fi, float _lambda, int _penalization, int _cycle, int _flags) : levels(_levels), pyrScale(_pyrScale), nIt(_nIt), minDisp(_minDisp), maxDisp(_maxDisp), poly_n(_poly_n), poly_sigma(_poly_sigma), fi(_fi), lambda(_lambda), penalization(_penalization), cycle(_cycle), flags(_flags)
61 { // No Parameters check, since they are all public
64 StereoVar::~StereoVar()
68 static Mat diffX(Mat &src)
70 int cols = src.cols - 1;
71 Mat dst(src.size(), src.type());
72 for(int y = 0; y < src.rows; y++){
73 const float* pSrc = src.ptr<float>(y);
74 float* pDst = dst.ptr<float>(y);
77 for (x = 0; x <= cols - 8; x += 8) {
78 __m128 a0 = _mm_loadu_ps(pSrc + x);
79 __m128 b0 = _mm_loadu_ps(pSrc + x + 1);
80 __m128 a1 = _mm_loadu_ps(pSrc + x + 4);
81 __m128 b1 = _mm_loadu_ps(pSrc + x + 5);
82 b0 = _mm_sub_ps(b0, a0);
83 b1 = _mm_sub_ps(b1, a1);
84 _mm_storeu_ps(pDst + x, b0);
85 _mm_storeu_ps(pDst + x + 4, b1);
88 for( ; x < cols; x++) pDst[x] = pSrc[x+1] - pSrc[x];
94 static Mat getGradient(Mat &src)
97 Mat dst(src.size(), src.type());
99 for (y = 0; y < src.rows - 1; y++) {
100 float *pSrc = src.ptr<float>(y);
101 float *pSrcF = src.ptr<float>(y + 1);
102 float *pDst = dst.ptr<float>(y);
103 for (x = 0; x < src.cols - 1; x++)
104 pDst[x] = fabs(pSrc[x + 1] - pSrc[x]) + fabs(pSrcF[x] - pSrc[x]);
109 static Mat getG_c(Mat &src, float l)
111 Mat dst(src.size(), src.type());
112 for (register int y = 0; y < src.rows; y++) {
113 float *pSrc = src.ptr<float>(y);
114 float *pDst = dst.ptr<float>(y);
115 for (register int x = 0; x < src.cols; x++)
116 pDst[x] = 0.5f*l / sqrtf(l*l + pSrc[x]*pSrc[x]);
121 static Mat getG_p(Mat &src, float l)
123 Mat dst(src.size(), src.type());
124 for (register int y = 0; y < src.rows; y++) {
125 float *pSrc = src.ptr<float>(y);
126 float *pDst = dst.ptr<float>(y);
127 for (register int x = 0; x < src.cols; x++)
128 pDst[x] = 0.5f*l*l / (l*l + pSrc[x]*pSrc[x]);
133 void StereoVar::VariationalSolver(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level)
135 register int n, x, y;
136 float gl = 1, gr = 1, gu = 1, gd = 1, gc = 4;
146 if (flags & USE_SMART_ID) {
147 double scale = pow(pyrScale, (double) level) * (1 + pyrScale);
148 N = (int) (N / scale);
151 double scale = pow(pyrScale, (double) level);
155 int width = u.cols - 1;
156 int height = u.rows - 1;
157 for (n = 0; n < N; n++) {
158 if (penalization != PENALIZATION_TICHONOV) {
159 Mat gradient = getGradient(U);
160 switch (penalization) {
161 case PENALIZATION_CHARBONNIER: g_c = getG_c(gradient, l); break;
162 case PENALIZATION_PERONA_MALIK: g_p = getG_p(gradient, l); break;
166 for (y = 1 ; y < height; y++) {
167 float *pU = U.ptr<float>(y);
168 float *pUu = U.ptr<float>(y + 1);
169 float *pUd = U.ptr<float>(y - 1);
170 float *pu = u.ptr<float>(y);
171 float *pI1 = I1.ptr<float>(y);
172 float *pI2 = I2.ptr<float>(y);
173 float *pI2x = I2x.ptr<float>(y);
174 float *pG_c = NULL, *pG_cu = NULL, *pG_cd = NULL;
175 float *pG_p = NULL, *pG_pu = NULL, *pG_pd = NULL;
176 switch (penalization) {
177 case PENALIZATION_CHARBONNIER:
178 pG_c = g_c.ptr<float>(y);
179 pG_cu = g_c.ptr<float>(y + 1);
180 pG_cd = g_c.ptr<float>(y - 1);
182 case PENALIZATION_PERONA_MALIK:
183 pG_p = g_p.ptr<float>(y);
184 pG_pu = g_p.ptr<float>(y + 1);
185 pG_pd = g_p.ptr<float>(y - 1);
188 for (x = 1; x < width; x++) {
189 switch (penalization) {
190 case PENALIZATION_CHARBONNIER:
192 gl = gc + pG_c[x - 1];
193 gr = gc + pG_c[x + 1];
196 gc = gl + gr + gu + gd;
198 case PENALIZATION_PERONA_MALIK:
200 gl = gc + pG_p[x - 1];
201 gr = gc + pG_p[x + 1];
204 gc = gl + gr + gu + gd;
209 if (maxDisp > minDisp) {
210 if (pU[x] > maxDisp * scale) {_fi *= 1000; pU[x] = static_cast<float>(maxDisp * scale);}
211 if (pU[x] < minDisp * scale) {_fi *= 1000; pU[x] = static_cast<float>(minDisp * scale);}
214 int A = static_cast<int>(pU[x]);
215 int neg = 0; if (pU[x] <= 0) neg = -1;
218 pu[x] = pU[width - A];
219 else if (x + A + neg < 0)
222 pu[x] = A + (pI2x[x + A + neg] * (pI1[x] - pI2[x + A])
223 + _fi * (gr * pU[x + 1] + gl * pU[x - 1] + gu * pUu[x] + gd * pUd[x] - gc * A))
224 / (pI2x[x + A + neg] * pI2x[x + A + neg] + gc * _fi) ;
228 pu[width] = pu[width - 1];
230 for (x = 0; x <= width; x++) {
231 u.at<float>(0, x) = u.at<float>(1, x);
232 u.at<float>(height, x) = u.at<float>(height - 1, x);
235 if (!g_c.empty()) g_c.release();
236 if (!g_p.empty()) g_p.release();
240 void StereoVar::VCycle_MyFAS(Mat &I1, Mat &I2, Mat &I2x, Mat &_u, int level)
242 CvSize imgSize = _u.size();
243 CvSize frmSize = cvSize((int) (imgSize.width * pyrScale + 0.5), (int) (imgSize.height * pyrScale + 0.5));
244 Mat I1_h, I2_h, I2x_h, u_h, U, U_h;
247 VariationalSolver(I1, I2, I2x, _u, level);
249 if (level >= levels - 1) return;
253 resize(I1, I1_h, frmSize, 0, 0, INTER_AREA);
254 resize(I2, I2_h, frmSize, 0, 0, INTER_AREA);
255 resize(_u, u_h, frmSize, 0, 0, INTER_AREA);
256 u_h.convertTo(u_h, u_h.type(), pyrScale);
261 VCycle_MyFAS(I1_h, I2_h, I2x_h, U_h, level);
263 subtract(U_h, u_h, U_h);
264 U_h.convertTo(U_h, U_h.type(), 1.0 / pyrScale);
267 resize(U_h, U, imgSize);
269 //correcting the solution
273 VariationalSolver(I1, I2, I2x, _u, level - 1);
275 if (flags & USE_MEDIAN_FILTERING) medianBlur(_u, _u, 3);
285 void StereoVar::FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level)
287 double scale = pow(pyrScale, (double) level);
288 CvSize frmSize = cvSize((int) (u.cols * scale + 0.5), (int) (u.rows * scale + 0.5));
289 Mat I1_h, I2_h, I2x_h, u_h;
292 resize(I1, I1_h, frmSize, 0, 0, INTER_AREA);
293 resize(I2, I2_h, frmSize, 0, 0, INTER_AREA);
294 resize(u, u_h, frmSize, 0, 0, INTER_AREA);
295 u_h.convertTo(u_h, u_h.type(), scale);
300 VariationalSolver(I1_h, I2_h, I2x_h, u_h, level);
303 VCycle_MyFAS(I1_h, I2_h, I2x_h, u_h, level);
307 u_h.convertTo(u_h, u_h.type(), 1.0 / scale);
310 resize(u_h, u, u.size(), 0, 0, INTER_CUBIC);
318 if ((flags & USE_AUTO_PARAMS) && (level < levels / 3)) {
319 penalization = PENALIZATION_PERONA_MALIK;
321 flags -= USE_AUTO_PARAMS;
324 if (flags & USE_MEDIAN_FILTERING) medianBlur(u, u, 3);
325 if (level >= 0) FMG(I1, I2, I2x, u, level);
328 void StereoVar::autoParams()
330 int maxD = MAX(labs(maxDisp), labs(minDisp));
332 if (!maxD) pyrScale = 0.85;
333 else if (maxD < 8) pyrScale = 0.5;
334 else if (maxD < 64) pyrScale = 0.5 + static_cast<double>(maxD - 8) * 0.00625;
335 else pyrScale = 0.85;
339 while ( pow(pyrScale, levels) * maxD > 1.5) levels ++;
343 switch(penalization) {
344 case PENALIZATION_TICHONOV: cycle = CYCLE_V; break;
345 case PENALIZATION_CHARBONNIER: cycle = CYCLE_O; break;
346 case PENALIZATION_PERONA_MALIK: cycle = CYCLE_O; break;
350 void StereoVar::operator ()( const Mat& left, const Mat& right, Mat& disp )
352 CV_Assert(left.size() == right.size() && left.type() == right.type());
353 CvSize imgSize = left.size();
354 int MaxD = MAX(labs(minDisp), labs(maxDisp));
355 int SignD = 1; if (MIN(minDisp, maxDisp) < 0) SignD = -1;
356 if (minDisp >= maxDisp) {MaxD = 256; SignD = 1;}
359 if ((flags & USE_INITIAL_DISPARITY) && (!disp.empty())) {
360 CV_Assert(disp.size() == left.size() && disp.type() == CV_8UC1);
361 disp.convertTo(u, CV_32FC1, static_cast<double>(SignD * MaxD) / 256);
363 u.create(imgSize, CV_32FC1);
368 Mat leftgray, rightgray;
369 if (left.type() != CV_8UC1) {
370 cvtColor(left, leftgray, CV_BGR2GRAY);
371 cvtColor(right, rightgray, CV_BGR2GRAY);
373 left.copyTo(leftgray);
374 right.copyTo(rightgray);
376 if (flags & USE_EQUALIZE_HIST) {
377 equalizeHist(leftgray, leftgray);
378 equalizeHist(rightgray, rightgray);
380 if (poly_sigma > 0.0001) {
381 GaussianBlur(leftgray, leftgray, cvSize(poly_n, poly_n), poly_sigma);
382 GaussianBlur(rightgray, rightgray, cvSize(poly_n, poly_n), poly_sigma);
385 if (flags & USE_AUTO_PARAMS) {
386 penalization = PENALIZATION_TICHONOV;
391 leftgray.convertTo(I1, CV_32FC1);
392 rightgray.convertTo(I2, CV_32FC1);
398 FMG(I1, I2, I2x, u, levels - 1);
405 disp.create( left.size(), CV_8UC1 );
407 u.convertTo(disp, disp.type(), 256 / MaxD, 0);