16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cvaux.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn The code below is some modification of Stan Birchfield's algorithm described in: 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Depth Discontinuities by Pixel-to-Pixel Stereo 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Stan Birchfield and Carlo Tomasi 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn International Journal of Computer Vision, 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35(3): 269-293, December 1999. 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn This implementation uses different cost function that results in 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn O(pixPerRow*maxDisparity) complexity of dynamic programming stage versus 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn O(pixPerRow*log(pixPerRow)*maxDisparity) in the above paper. 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Find stereo correspondence by dynamic programming algorithm * 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DP_STEP_LEFT 0 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DP_STEP_UP 1 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DP_STEP_DIAG 2 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_BIRCH_DIFF_LUM 5 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_MAX_DP_SUM_VAL (INT_MAX/4) 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef struct _CvDPCell 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar step; //local-optimal step 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sum; //current sum 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}_CvDPCell; 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef struct _CvRightImData 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar min_val, max_val; 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} _CvRightImData; 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define CV_IMAX3(a,b,c) ((temp3 = (a) >= (b) ? (a) : (b)),(temp3 >= (c) ? temp3 : (c))) 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define CV_IMIN3(a,b,c) ((temp3 = (a) <= (b) ? (a) : (b)),(temp3 <= (c) ? temp3 : (c))) 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvFindStereoCorrespondenceByBirchfieldDP( uchar* src1, uchar* src2, 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* disparities, 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size, int widthStep, 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxDisparity, 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _param1, float _param2, 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _param3, float _param4, 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _param5 ) 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, i, j, temp3; 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int d, s; 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dispH = maxDisparity + 3; 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *dispdata; 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imgW = size.width; 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imgH = size.height; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar val, prevval, prev, curr; 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int min_val; 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dest = disparities; 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int param1 = cvRound(_param1); 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int param2 = cvRound(_param2); 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int param3 = cvRound(_param3); 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int param4 = cvRound(_param4); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int param5 = cvRound(_param5); 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #define CELL(d,x) cells[(d)+(x)*dispH] 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dsi = (uchar*)cvAlloc(sizeof(uchar)*imgW*dispH); 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* edges = (uchar*)cvAlloc(sizeof(uchar)*imgW*imgH); 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _CvDPCell* cells = (_CvDPCell*)cvAlloc(sizeof(_CvDPCell)*imgW*MAX(dispH,(imgH+1)/2)); 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _CvRightImData* rData = (_CvRightImData*)cvAlloc(sizeof(_CvRightImData)*imgW); 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* reliabilities = (int*)cells; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < imgH; y++ ) 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* srcdata1 = src1 + widthStep * y; 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* srcdata2 = src2 + widthStep * y; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //init rData 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevval = prev = srcdata2[0]; 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 1; j < imgW; j++ ) 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn curr = srcdata2[j]; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn val = (uchar)((curr + prev)>>1); 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rData[j-1].max_val = (uchar)CV_IMAX3( val, prevval, prev ); 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rData[j-1].min_val = (uchar)CV_IMIN3( val, prevval, prev ); 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevval = val; 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev = curr; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rData[j-1] = rData[j-2];//last elem 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // fill dissimularity space image 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i <= maxDisparity + 1; i++ ) 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dsi += imgW; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rData--; 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = i - 1; j < imgW - 1; j++ ) 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (t = srcdata1[j] - rData[j+1].max_val) >= 0 ) 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dsi[j] = (uchar)t; 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( (t = rData[j+1].min_val - srcdata1[j]) >= 0 ) 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dsi[j] = (uchar)t; 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dsi[j] = 0; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dsi -= (maxDisparity+1)*imgW; 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rData += maxDisparity+1; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //intensity gradients image construction 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //left row 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[y*imgW] = edges[y*imgW+1] = edges[y*imgW+2] = 2; 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[y*imgW+imgW-1] = edges[y*imgW+imgW-2] = edges[y*imgW+imgW-3] = 1; 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 3; j < imgW-4; j++ ) 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[y*imgW+j] = 0; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ( CV_IMAX3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) - 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_IMIN3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) ) >= ICV_BIRCH_DIFF_LUM ) 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[y*imgW+j] |= 1; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ( CV_IMAX3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) - 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_IMIN3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) ) >= ICV_BIRCH_DIFF_LUM ) 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[y*imgW+j] |= 2; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //find correspondence using dynamical programming 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //init DP table 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < imgW; x++ ) 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CELL(0,x).sum = CELL(dispH-1,x).sum = ICV_MAX_DP_SUM_VAL; 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CELL(0,x).step = CELL(dispH-1,x).step = ICV_DP_STEP_LEFT; 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 2; d < dispH; d++ ) 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CELL(d,d-2).sum = ICV_MAX_DP_SUM_VAL; 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CELL(d,d-2).step = ICV_DP_STEP_UP; 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CELL(1,0).sum = 0; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CELL(1,0).step = ICV_DP_STEP_LEFT; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 1; x < imgW; x++ ) 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int d = MIN( x + 1, maxDisparity + 1); 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* _edges = edges + y*imgW + x; 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int e0 = _edges[0] & 1; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _CvDPCell* _cell = cells + x*dispH; 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int s = dsi[d*imgW+x]; 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sum[3]; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //check left step 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum[0] = _cell[d-dispH].sum - param2; 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //check up step 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _cell[d+1].step != ICV_DP_STEP_DIAG && e0 ) 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum[1] = _cell[d+1].sum + param1; 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) ) 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t; 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum[2] = _cell[d-1-dispH].sum + param1; 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = sum[1] < sum[0]; 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //choose local-optimal pass 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( sum[t] <= sum[2] ) 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].step = (uchar)t; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].sum = sum[t] + s; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].step = ICV_DP_STEP_DIAG; 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].sum = sum[2] + s; 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( sum[0] <= sum[1] ) 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].step = ICV_DP_STEP_LEFT; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].sum = sum[0] + s; 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].step = ICV_DP_STEP_UP; 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].sum = sum[1] + s; 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) ) 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum[2] = _cell[d-1-dispH].sum + param1; 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( sum[0] <= sum[2] ) 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].step = ICV_DP_STEP_LEFT; 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].sum = sum[0] + s; 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].step = ICV_DP_STEP_DIAG; 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].sum = sum[2] + s; 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].step = ICV_DP_STEP_LEFT; 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cell[d].sum = sum[0] + s; 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( --d ); 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }// for x 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //extract optimal way and fill disparity image 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dispdata = dest + widthStep * y; 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //find min_val 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_val = ICV_MAX_DP_SUM_VAL; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i <= maxDisparity + 1; i++ ) 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( min_val > CELL(i,imgW-1).sum ) 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = i; 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_val = CELL(i,imgW-1).sum; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //track optimal pass 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = imgW - 1; x > 0; x-- ) 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dispdata[x] = (uchar)(d - 1); 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( CELL(d,x).step == ICV_DP_STEP_UP ) d++; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if ( CELL(d,x).step == ICV_DP_STEP_DIAG ) 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = x; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( CELL(d,x).step == ICV_DP_STEP_DIAG ) 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d--; 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x--; 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = x; i < s; i++ ) 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dispdata[i] = (uchar)(d-1); 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }//for x 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }// for y 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //Postprocessing the Disparity Map 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //remove obvious errors in the disparity map 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < imgW; x++ ) 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1; y < imgH - 1; y++ ) 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dest[(y-1)*widthStep+x] == dest[(y+1)*widthStep+x] ) 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dest[y*widthStep+x] = dest[(y-1)*widthStep+x]; 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //compute intensity Y-gradients 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < imgW; x++ ) 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1; y < imgH - 1; y++ ) 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ( CV_IMAX3( src1[(y-1)*widthStep+x], src1[y*widthStep+x], 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src1[(y+1)*widthStep+x] ) - 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_IMIN3( src1[(y-1)*widthStep+x], src1[y*widthStep+x], 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src1[(y+1)*widthStep+x] ) ) >= ICV_BIRCH_DIFF_LUM ) 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[y*imgW+x] |= 4; 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[(y+1)*imgW+x] |= 4; 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[(y-1)*imgW+x] |= 4; 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y++; 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //remove along any particular row, every gradient 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //for which two adjacent columns do not agree. 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < imgH; y++ ) 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev = edges[y*imgW]; 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 1; x < imgW - 1; x++ ) 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn curr = edges[y*imgW+x]; 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (curr & 4) && 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( !( prev & 4 ) || 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !( edges[y*imgW+x+1] & 4 ) ) ) 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn edges[y*imgW+x] -= 4; 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev = curr; 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // define reliability 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < imgW; x++ ) 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1; y < imgH; y++ ) 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = y - 1; 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; y < imgH && dest[y*widthStep+x] == dest[(y-1)*widthStep+x]; y++ ) 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = y - i; 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; i < y; i++ ) 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reliabilities[i*imgW+x] = s; 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //Y - propagate reliable regions 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < imgW; x++ ) 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < imgH; y++ ) 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = dest[y*widthStep+x]; 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 4) && 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d > 0 )//highly || moderately 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[y*widthStep+x] = (uchar)d; 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //up propagation 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = y - 1; i >= 0; i-- ) 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ( edges[i*imgW+x] & 4 ) || 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( dest[i*widthStep+x] < d && 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reliabilities[i*imgW+x] >= param3 ) || 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( reliabilities[y*imgW+x] < param5 && 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dest[i*widthStep+x] - 1 == d ) ) break; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[i*widthStep+x] = (uchar)d; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //down propagation 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = y + 1; i < imgH; i++ ) 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ( edges[i*imgW+x] & 4 ) || 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( dest[i*widthStep+x] < d && 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reliabilities[i*imgW+x] >= param3 ) || 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( reliabilities[y*imgW+x] < param5 && 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dest[i*widthStep+x] - 1 == d ) ) break; 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[i*widthStep+x] = (uchar)d; 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = i - 1; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[y*widthStep+x] = (uchar)d; 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // define reliability along X 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < imgH; y++ ) 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 1; x < imgW; x++ ) 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = x - 1; 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; x < imgW && dest[y*widthStep+x] == dest[y*widthStep+x-1]; x++ ); 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = x - i; 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; i < x; i++ ) 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reliabilities[y*imgW+i] = s; 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //X - propagate reliable regions 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < imgH; y++ ) 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < imgW; x++ ) 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = dest[y*widthStep+x]; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 1) && 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d > 0 )//highly || moderately 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[y*widthStep+x] = (uchar)d; 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //up propagation 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = x - 1; i >= 0; i-- ) 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (edges[y*imgW+i] & 1) || 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( dest[y*widthStep+i] < d && 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reliabilities[y*imgW+i] >= param3 ) || 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( reliabilities[y*imgW+x] < param5 && 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dest[y*widthStep+i] - 1 == d ) ) break; 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[y*widthStep+i] = (uchar)d; 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //down propagation 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = x + 1; i < imgW; i++ ) 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (edges[y*imgW+i] & 1) || 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( dest[y*widthStep+i] < d && 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reliabilities[y*imgW+i] >= param3 ) || 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( reliabilities[y*imgW+x] < param5 && 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dest[y*widthStep+i] - 1 == d ) ) break; 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[y*widthStep+i] = (uchar)d; 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = i - 1; 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn disparities[y*widthStep+x] = (uchar)d; 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //release resources 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &dsi ); 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &edges ); 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &cells ); 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &rData ); 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////// 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvFindStereoCorrespondence 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: find stereo correspondence on stereo-pair 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// leftImage - left image of stereo-pair (format 8uC1). 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// rightImage - right image of stereo-pair (format 8uC1). 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// mode -mode of correspondance retrieval (now CV_RETR_DP_BIRCHFIELD only) 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// dispImage - destination disparity image 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// maxDisparity - maximal disparity 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// param1, param2, param3, param4, param5 - parameters of algorithm 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Images must be rectified. 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// All images must have format 8uC1. 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvFindStereoCorrespondence( 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvArr* leftImage, const CvArr* rightImage, 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int mode, 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvArr* depthImage, 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxDisparity, 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double param1, double param2, double param3, 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double param4, double param5 ) 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvFindStereoCorrespondence" ); 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *src1, *src2; 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *dst; 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat src1_stub, src2_stub, dst_stub; 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int coi; 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( src1 = cvGetMat( leftImage, &src1_stub, &coi )); 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" ); 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( src2 = cvGetMat( rightImage, &src2_stub, &coi )); 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" ); 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( dst = cvGetMat( depthImage, &dst_stub, &coi )); 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" ); 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // check args 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( src1->type ) != CV_8UC1 || 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE( src2->type ) != CV_8UC1 || 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE( dst->type ) != CV_8UC1) CV_ERROR(CV_StsUnsupportedFormat, 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "All images must be single-channel and have 8u" ); 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( src1, src2 ) || !CV_ARE_SIZES_EQ( src1, dst ) ) 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxDisparity <= 0 || maxDisparity >= src1->width || maxDisparity > 255 ) 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsOutOfRange, 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "parameter /maxDisparity/ is out of range"); 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mode == CV_DISPARITY_BIRCHFIELD ) 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( param1 == CV_UNDEF_SC_PARAM ) param1 = CV_IDP_BIRCHFIELD_PARAM1; 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( param2 == CV_UNDEF_SC_PARAM ) param2 = CV_IDP_BIRCHFIELD_PARAM2; 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( param3 == CV_UNDEF_SC_PARAM ) param3 = CV_IDP_BIRCHFIELD_PARAM3; 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( param4 == CV_UNDEF_SC_PARAM ) param4 = CV_IDP_BIRCHFIELD_PARAM4; 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( param5 == CV_UNDEF_SC_PARAM ) param5 = CV_IDP_BIRCHFIELD_PARAM5; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( icvFindStereoCorrespondenceByBirchfieldDP( src1->data.ptr, 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src2->data.ptr, dst->data.ptr, 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetMatSize( src1 ), src1->step, 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxDisparity, (float)param1, (float)param2, (float)param3, 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (float)param4, (float)param5 ) ); 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Unsupported mode of function" ); 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 555