modules/contrib/src/stereovar.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   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.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  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.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  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.
  26 //
  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.
  29 //
  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.
  40 //
  41 //M*/
  42
  43 /*
  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
  47
  48  This code is written by Sergey G. Kosov for "Visir PX" application as part of Project X (www.project-10.de)
  49  */
  50
  51 #include "precomp.hpp"
  52 #include <limits.h>
  53
  54 namespace cv
  55 {
  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)
  57 {
  58 }
  59
  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
  62 }
  63
  64 StereoVar::~StereoVar()
  65 {
  66 }
  67
  68 static Mat diffX(Mat &src)
  69 {
  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);
  75         int x = 0;
  76 #if CV_SSE2
  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);
  86         }
  87 #endif
  88         for( ; x < cols; x++) pDst[x] = pSrc[x+1] - pSrc[x];
  89         pDst[cols] = 0.f;
  90     }
  91     return dst;
  92 }
  93
  94 static Mat getGradient(Mat &src)
  95 {
  96     register int x, y;
  97     Mat dst(src.size(), src.type());
  98     dst.setTo(0);
  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]);
 105     }
 106     return dst;
 107 }
 108
 109 static Mat getG_c(Mat &src, float l)
 110 {
 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]);
 117     }
 118     return dst;
 119 }
 120
 121 static Mat getG_p(Mat &src, float l)
 122 {
 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]);
 129     }
 130     return dst;
 131 }
 132
 133 void StereoVar::VariationalSolver(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level)
 134 {
 135     register int n, x, y;
 136     float gl = 1, gr = 1, gu = 1, gd = 1, gc = 4;
 137     Mat g_c, g_p;
 138     Mat U;
 139     u.copyTo(U);
 140
 141     int     N = nIt;
 142     float   l = lambda;
 143     float   Fi = fi;
 144
 145
 146     if (flags & USE_SMART_ID) {
 147         double scale = pow(pyrScale, (double) level) * (1 + pyrScale);
 148         N = (int) (N / scale);
 149     }
 150
 151     double scale = pow(pyrScale, (double) level);
 152     Fi /= (float) scale;
 153     l *= (float) scale;
 154
 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;
 163             }
 164             gradient.release();
 165         }
 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);
 181                     break;
 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);
 186                     break;
 187             }
 188             for (x = 1; x < width; x++) {
 189                 switch (penalization) {
 190                     case PENALIZATION_CHARBONNIER:
 191                         gc = pG_c[x];
 192                         gl = gc + pG_c[x - 1];
 193                         gr = gc + pG_c[x + 1];
 194                         gu = gc + pG_cu[x];
 195                         gd = gc + pG_cd[x];
 196                         gc = gl + gr + gu + gd;
 197                         break;
 198                     case PENALIZATION_PERONA_MALIK:
 199                         gc = pG_p[x];
 200                         gl = gc + pG_p[x - 1];
 201                         gr = gc + pG_p[x + 1];
 202                         gu = gc + pG_pu[x];
 203                         gd = gc + pG_pd[x];
 204                         gc = gl + gr + gu + gd;
 205                         break;
 206                 }
 207
 208                 float _fi = Fi;
 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);}
 212                 }
 213
 214                 int A = static_cast<int>(pU[x]);
 215                 int neg = 0; if (pU[x] <= 0) neg = -1;
 216
 217                 if (x + A > width)
 218                     pu[x] = pU[width - A];
 219                 else if (x + A + neg < 0)
 220                     pu[x] = pU[- A + 2];
 221                 else {
 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) ;
 225                 }
 226             }// x
 227             pu[0] = pu[1];
 228             pu[width] = pu[width - 1];
 229         }// y
 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);
 233         }
 234         u.copyTo(U);
 235         if (!g_c.empty()) g_c.release();
 236         if (!g_p.empty()) g_p.release();
 237     }//n
 238 }
 239
 240 void StereoVar::VCycle_MyFAS(Mat &I1, Mat &I2, Mat &I2x, Mat &_u, int level)
 241 {
 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;
 245
 246     //PRE relaxation
 247     VariationalSolver(I1, I2, I2x, _u, level);
 248
 249     if (level >= levels - 1) return;
 250     level ++;
 251
 252     //scaling DOWN
 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);
 257     I2x_h = diffX(I2_h);
 258
 259     //Next level
 260     U_h = u_h.clone();
 261     VCycle_MyFAS(I1_h, I2_h, I2x_h, U_h, level);
 262
 263     subtract(U_h, u_h, U_h);
 264     U_h.convertTo(U_h, U_h.type(), 1.0 / pyrScale);
 265
 266     //scaling UP
 267     resize(U_h, U, imgSize);
 268
 269     //correcting the solution
 270     add(_u, U, _u);
 271
 272     //POST relaxation
 273     VariationalSolver(I1, I2, I2x, _u, level - 1);
 274
 275     if (flags & USE_MEDIAN_FILTERING) medianBlur(_u, _u, 3);
 276
 277     I1_h.release();
 278     I2_h.release();
 279     I2x_h.release();
 280     u_h.release();
 281     U.release();
 282     U_h.release();
 283 }
 284
 285 void StereoVar::FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level)
 286 {
 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;
 290
 291     //scaling DOWN
 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);
 296     I2x_h = diffX(I2_h);
 297
 298     switch (cycle) {
 299         case CYCLE_O:
 300             VariationalSolver(I1_h, I2_h, I2x_h, u_h, level);
 301             break;
 302         case CYCLE_V:
 303             VCycle_MyFAS(I1_h, I2_h, I2x_h, u_h, level);
 304             break;
 305     }
 306
 307     u_h.convertTo(u_h, u_h.type(), 1.0 / scale);
 308
 309     //scaling UP
 310     resize(u_h, u, u.size(), 0, 0, INTER_CUBIC);
 311
 312     I1_h.release();
 313     I2_h.release();
 314     I2x_h.release();
 315     u_h.release();
 316
 317     level--;
 318     if ((flags & USE_AUTO_PARAMS) && (level < levels / 3)) {
 319         penalization = PENALIZATION_PERONA_MALIK;
 320         fi *= 100;
 321         flags -= USE_AUTO_PARAMS;
 322         autoParams();
 323     }
 324     if (flags & USE_MEDIAN_FILTERING) medianBlur(u, u, 3);
 325     if (level >= 0) FMG(I1, I2, I2x, u, level);
 326 }
 327
 328 void StereoVar::autoParams()
 329 {
 330     int maxD = MAX(labs(maxDisp), labs(minDisp));
 331
 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;
 336
 337     if (maxD) {
 338         levels = 0;
 339         while ( pow(pyrScale, levels) * maxD > 1.5) levels ++;
 340         levels++;
 341     }
 342
 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;
 347     }
 348 }
 349
 350 void StereoVar::operator ()( const Mat& left, const Mat& right, Mat& disp )
 351 {
 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;}
 357
 358     Mat u;
 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);
 362     } else {
 363         u.create(imgSize, CV_32FC1);
 364         u.setTo(0);
 365     }
 366
 367     // Preprocessing
 368     Mat leftgray, rightgray;
 369     if (left.type() != CV_8UC1) {
 370         cvtColor(left, leftgray, CV_BGR2GRAY);
 371         cvtColor(right, rightgray, CV_BGR2GRAY);
 372     } else {
 373         left.copyTo(leftgray);
 374         right.copyTo(rightgray);
 375     }
 376     if (flags & USE_EQUALIZE_HIST) {
 377         equalizeHist(leftgray, leftgray);
 378         equalizeHist(rightgray, rightgray);
 379     }
 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);
 383     }
 384
 385     if (flags & USE_AUTO_PARAMS) {
 386         penalization = PENALIZATION_TICHONOV;
 387         autoParams();
 388     }
 389
 390     Mat I1, I2;
 391     leftgray.convertTo(I1, CV_32FC1);
 392     rightgray.convertTo(I2, CV_32FC1);
 393     leftgray.release();
 394     rightgray.release();
 395
 396     Mat I2x = diffX(I2);
 397
 398     FMG(I1, I2, I2x, u, levels - 1);
 399
 400     I1.release();
 401     I2.release();
 402     I2x.release();
 403
 404
 405     disp.create( left.size(), CV_8UC1 );
 406     u = abs(u);
 407     u.convertTo(disp, disp.type(), 256 / MaxD, 0);
 408
 409     u.release();
 410 }
 411 } // namespace