开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
stereoMatching_algo.cpp
资源名称：estereo2.zip　[点击查看]
上传用户：fengshi120
上传日期：2014-07-17
资源大小：6155k
文件大小：15k
源码类别：

3D图形编程

开发平台：

C/C++

stereoMatching_algo.cpp：源码内容
  1. /*************************************************************************** 
  2. *
  3. * Copyright 2000 by David Demirdjian.   All rights reserved. 
  4. *  
  5. * Developed  by David Demirdjian
  6. *  
  7. * Permission to use, copy, or modify this software and  its documentation 
  8. * for  educational  and  research purposes only and without fee  is hereby 
  9. * granted, provided  that this copyright notice and the original authors's 
  10. * names appear  on all copies and supporting documentation.  If individual 
  11. * files are  separated from  this  distribution directory  structure, this 
  12. * copyright notice must be included.  For any other uses of this software, 
  13. * in original or  modified form, including but not limited to distribution 
  14. * in whole or in  part, specific  prior permission  must be  obtained from 
  15. * MIT.  These programs shall not  be  used, rewritten, or  adapted as  the 
  16. * basis  of  a  commercial  software  or  hardware product  without  first 
  17. * obtaining appropriate  licenses from David Demirdjian.  The author makes 
  18. * no representations about the suitability of this software for any purpose.  
  19. * It is provided "as is" without express or implied warranty. 
  20. *  
  21. **************************************************************************/
  22. #include "stdafx.h"
  23. #include "stereoMatching.h"
  24. #include "processingMMX.h"
  26. // ************************************************************
  27. // ************************************************************
  28. // *** Main algorithm for stereo computation
  29. // ************************************************************
  30. // ************************************************************
  32. typedef unsigned char uchar;
  33. typedef unsigned short ushort;
  35. #ifdef _DEBUG
  36. #define new DEBUG_NEW
  37. #undef THIS_FILE
  38. static char THIS_FILE[] = __FILE__;
  39. #endif
  42. #define _PROCESS_AND_COMPARE_SSE2_ 
  43. ImgSubandAdd_sse2(imr8_bw , imtop8_bw +(i+k)*width , iml8_bw +(i+k) ,ptBuff , subImageSize); 
  44. sum_Row_mmx(ptBuff , ptBuff16+2*backStep, subImageSize, maskSizeX); 
  45. avg_Col_mmx(ptBuff16+backStep , ptBuff-backStep,subImageSize, width, maskSizeY); 
  46. copyMMX(ptBuff16, ptBuff16+subImageSize, 2*backStep*sizeof(short));  
  47. findMinimumCorrelation_mmx(ptBuff-backStep, i+k, imDepth8u+offset-backStep, 
  48. corrScore+offset-backStep,  corrScoreSec+offset-backStep,
  49. subImageSize);
  51. void StereoMatching::estimateStereo(const uchar* iml8_bw, 
  52. const uchar* imr8_bw,
  53. const uchar* imtop8_bw,
  54. int width, int height, int maskSizeX, int maskSizeY, 
  55. char minDiff, int nbDepth, int nbPartitions,
  56. StereoBuffer* sb, StereoImage* sdata)
  57. {
  58. // assure good partitioning (.....)
  59. while (height/nbPartitions - 2*(maskSizeY/2) <=0) {
  60. nbPartitions /= 2;
  61. }
  63. int i,j,k;
  64. int subImageSize=width*height/nbPartitions;
  65. int backStep = (maskSizeY/2)*width;
  67. uchar* ptBuff;
  68. ushort* ptBuff16;
  69. int siz;
  71. // ----------- process the first sub-images -----------
  72. for (i=0; i<nbDepth; i+=8) {
  73. ptBuff = sb->buff[i];
  74. ImgSubandAdd2(imr8_bw , imtop8_bw +i*width , iml8_bw +i , ptBuff , subImageSize, sb->buffStep, width);
  76. for (k=0; k<8; ++k) 
  77. {
  78. ptBuff = sb->buff[i+k];
  79. ptBuff16 = sb->buff16[i+k];
  80. siz = __max(0, subImageSize - 2*backStep);
  82. sum_Row_mmx(ptBuff , ptBuff16 , subImageSize,maskSizeX);
  83. avg_Col_mmx(ptBuff16+backStep, ptBuff+backStep, siz, width, maskSizeY);
  84. copyMMX(ptBuff16,ptBuff16+subImageSize-2*backStep,2*backStep*sizeof(short));
  86. if (i==0 && k==0) 
  87. initMinimumCorrelation(ptBuff, 0, sdata->imDepth8u, sb->corrScore, sb->corrScoreSec, subImageSize);
  88. else
  89. findMinimumCorrelation_mmx(ptBuff+backStep, i+k, sdata->imDepth8u+backStep, 
  90. sb->corrScore+backStep, sb->corrScoreSec+backStep,
  91. subImageSize-backStep);
  92. }
  93. }
  95. // check that there are enough diff. between best and second disp
  96. compareBestAndSecond(sb->corrScore+backStep, sb->corrScoreSec+backStep, minDiff, 
  97.  sdata->UNDEFINED_DEPTH, sdata->imDepth8u+backStep, subImageSize-backStep);
  99. int offset=subImageSize;
  100. // ----------- process the other ones -----------
  101. for (j=1; j<nbPartitions; ++j, offset+=subImageSize) 
  102. {
  104. // process depth i=0
  105. i=0;
  106. ptBuff = sb->buff[0]+offset;
  107. ptBuff16 = sb->buff16[0];
  109. // estimate absolute differences between the 3 stereo images
  110. ImgSubandAdd2(imr8_bw +offset, imtop8_bw + i*width +offset,  iml8_bw + i +offset,
  111.    ptBuff, subImageSize, sb->buffStep, width);
  113. // ptBuff16 data from [0] to [backStep] come from previous iter
  114. // sum_Row_mmx fills ptBuff16 from [2*backStep] to [2*backStep+subImageSize]
  115. sum_Row_mmx(ptBuff , ptBuff16+2*backStep, subImageSize, maskSizeX);
  116. // avg_Col_mmx process data in ptBuff16 from [0] to [subImageSize]
  117. avg_Col_mmx(ptBuff16+backStep , ptBuff-backStep,subImageSize, width, maskSizeY);
  119. // then copy ptBuff16 data [subImageSize] to [2*backStep+subImageSize]
  120. // to [0] to [2*backStep]
  121. copyMMX(ptBuff16, ptBuff16+subImageSize, 2*backStep*sizeof(short));
  123. // find minimum SAD score
  124. initMinimumCorrelation(ptBuff-backStep, 0, sdata->imDepth8u+offset-backStep, 
  125.    sb->corrScore+offset-backStep, sb->corrScoreSec+offset-backStep, subImageSize);
  127. // do stuff 7 times
  128. for (k=1; k<=7; ++k) {
  129. ptBuff+=sb->buffStep; ptBuff16+=sb->buffStep;
  130. sb->filterSAD(maskSizeX, maskSizeY, ptBuff, ptBuff16, 
  131.   backStep, subImageSize, offset);
  132. findMinimumCorrelation_mmx(ptBuff-backStep, i+k, sdata->imDepth8u+offset-backStep, 
  133.  sb->corrScore+offset-backStep, 
  134. sb->corrScoreSec+offset-backStep,
  135. subImageSize);
  136. }
  138. for (i=8; i<nbDepth; i+=8) {
  139. ptBuff = sb->buff[i]+offset;
  140. ptBuff16 = sb->buff16[i];
  142. // estimate absolute differences between the 3 stereo images
  143. ImgSubandAdd2(imr8_bw +offset, imtop8_bw + i*width +offset,  iml8_bw + i +offset,
  144. ptBuff, subImageSize, sb->buffStep, width);
  146. // process and comparison 
  147. for (k=0; k<=7; ++k) {
  148. sb->filterSAD(maskSizeX, maskSizeY, ptBuff, ptBuff16, backStep, 
  149.   subImageSize, offset);
  150. findMinimumCorrelation_mmx(ptBuff-backStep, i+k, sdata->imDepth8u+offset-backStep, 
  151.  sb->corrScore+offset-backStep, 
  152. sb->corrScoreSec+offset-backStep,
  153. subImageSize);
  154. ptBuff+=sb->buffStep; ptBuff16+=sb->buffStep;
  155. }
  156. }
  158. // check that there are enough diff. between best and second disp
  159. compareBestAndSecond(sb->corrScore+offset-backStep, sb->corrScoreSec+offset-backStep, minDiff, 
  160.   sdata->UNDEFINED_DEPTH, sdata->imDepth8u+offset-backStep, subImageSize);
  161. }
  163. }
  166. void StereoMatching::estimateStereo_sse2(const uchar* iml8_bw, 
  167. const uchar* imr8_bw,
  168. const uchar* imtop8_bw,
  169. int width, int height, int maskSizeX, int maskSizeY, 
  170. char minDiff, int nbDepth, int nbPartitions,
  171. StereoBuffer* sb, StereoImage* sdata)
  172. {
  173. // assure good partitioning (.....)
  174. while (height/nbPartitions - 2*(maskSizeY/2) <=0)
  175. {
  176. nbPartitions /= 2;
  177. }
  179. int i,j;
  180. int subImageSize=width*height/nbPartitions;
  182. int backStep = (maskSizeY/2)*width;
  184. int buffStep16 = sb->buffStep;
  185. uchar* ptBuff;
  186. ushort* ptBuff16;
  187. int siz;
  189. // ----------- process the first sub-images -----------
  190. for (i=0; i<nbDepth; i++) 
  191. {
  192. ptBuff = sb->buff[i];
  193. ptBuff16 = sb->buff16[i];
  194. ImgSubandAdd_sse2(imr8_bw , imtop8_bw +i*width , iml8_bw +i ,ptBuff , subImageSize);
  195. siz = __max(0, subImageSize - 2*backStep);
  197. sum_Row_mmx(ptBuff , ptBuff16 , subImageSize,maskSizeX);
  198. avg_Col_sse2(ptBuff16+backStep, ptBuff+backStep, siz, width, maskSizeY);
  199. copySSE(ptBuff16,ptBuff16+subImageSize-2*backStep,2*backStep*sizeof(short));
  201. if (i==0) 
  202. initMinimumCorrelation(ptBuff, 0, sdata->imDepth8u, sb->corrScore, sb->corrScoreSec, subImageSize);
  203. else
  204. findMinimumCorrelation_mmx(ptBuff+backStep, i, sdata->imDepth8u+backStep, 
  205. sb->corrScore+backStep, sb->corrScoreSec+backStep,
  206. subImageSize-backStep);
  207. }
  209. // check that there are enough diff. between best and second disp
  210. compareBestAndSecond(sb->corrScore+backStep, sb->corrScoreSec+backStep, minDiff, 
  211.  sdata->UNDEFINED_DEPTH, sdata->imDepth8u+backStep, subImageSize-backStep);
  213. int offset=subImageSize;
  214. // ----------- process the other ones -----------
  215. for (j=1; j<nbPartitions; ++j, offset+=subImageSize) 
  216. {
  217. for (i=0; i<nbDepth; i++) 
  218. {
  219. ptBuff = sb->buff[i]+offset;
  220. ptBuff16 = sb->buff16[i];
  222. // process and comparison 
  223. ImgSubandAdd_sse2(imr8_bw+offset , imtop8_bw+offset +i*width , iml8_bw+offset +i ,ptBuff , subImageSize); 
  225. // filtering
  226. sum_Row_5_mmx(ptBuff , ptBuff16+2*backStep, subImageSize); //, maskSizeX); 
  227. avg_Col_sse2(ptBuff16+backStep , ptBuff-backStep, subImageSize, width, maskSizeY); 
  228. copySSE(ptBuff16, ptBuff16+subImageSize, 2*backStep*sizeof(short));  
  230. if (i==0)
  231. initMinimumCorrelation(ptBuff-backStep, 0, sdata->imDepth8u+offset-backStep, sb->corrScore+offset-backStep, 
  232. sb->corrScoreSec+offset-backStep, subImageSize);
  233. else
  234. findMinimumCorrelation_mmx(ptBuff-backStep, i, sdata->imDepth8u+offset-backStep, 
  235. sb->corrScore+offset-backStep,  sb->corrScoreSec+offset-backStep,  subImageSize);
  236. }
  238. // check that there are enough diff. between best and second disp
  239. compareBestAndSecond(sb->corrScore+offset-backStep, sb->corrScoreSec+offset-backStep, minDiff, 
  240.   sdata->UNDEFINED_DEPTH, sdata->imDepth8u+offset-backStep, subImageSize);
  241. }
  243. }
  246. void StereoMatching::estimateStereo_Horiz(const uchar* iml8_bw, const uchar* imr8_bw,
  247. int width, int height, int maskSizeX, int maskSizeY, 
  248. char minDiff, int nbDepth, int nbPartitions,
  249. StereoBuffer* sb, StereoImage* sdata)
  250. {
  251. int i,k;
  252. int imageSize=width*height;
  253. int backStep = (maskSizeY/2)*width;
  254. uchar* ptBuff;
  255. ushort* ptBuff16;
  256. int siz = 8*(__max(0, imageSize - 2*backStep)/8);
  257. for (i=0; i<nbDepth; i+=8) {
  258. ptBuff = sb->buff[i];
  259. // compute SAD for (imr8_bw, iml8_bw+i), (imr8_bw, iml8_bw+i+1), ....
  260. // .. (imr8_bw, iml8_bw+i+7) 
  261. ImgSubandAdd_Horiz(imr8_bw, iml8_bw+i, ptBuff, imageSize, sb->buffStep, width);
  263. for (k=0; k<8; ++k) {
  264. ptBuff  = sb->buff[i+k];
  265. ptBuff16 = sb->buff16[i+k];
  266. // apply maskSizeX x maskSizeY average mask
  267. //sum_Row(ptBuff , ptBuff16,         imageSize,maskSizeX);
  268. //avg_Col(ptBuff16 +backStep, ptBuff +backStep, siz,  width, maskSizeY);
  269. sum_Row_mmx(ptBuff , ptBuff16,         imageSize,maskSizeX);
  270. avg_Col_mmx(ptBuff16 +backStep, ptBuff +backStep, siz,  width, maskSizeY);
  271. // translate data
  272. copyMMX(ptBuff16,ptBuff16+siz,2*backStep*sizeof(short));
  274. if (i==0 && k==0) 
  275. initMinimumCorrelation(ptBuff, 0,  sdata->imDepth8u, sb->corrScore,
  276. sb->corrScoreSec, imageSize);
  277. else
  278. /*findMinimumCorrelation(ptBuff+backStep, i+k, 
  279. sdata->imDepth8u+backStep, 
  280. sb->corrScore+backStep, sb->corrScoreSec+backStep,
  281. siz);*/
  282. findMinimumCorrelation_mmx(ptBuff+backStep, i+k, 
  283. sdata->imDepth8u+backStep, 
  284. sb->corrScore+backStep, sb->corrScoreSec+backStep,
  285. siz);
  286. }
  287. }
  288. // check that there are enough diff. between best and second disp
  289. compareBestAndSecond(sb->corrScore+backStep, sb->corrScoreSec+backStep, minDiff, 
  290.  sdata->UNDEFINED_DEPTH, sdata->imDepth8u+backStep, siz);
  291. }
  294. void StereoMatching::estimateStereo_Horiz_mmx(const uchar* iml8_bw, const uchar* imr8_bw,
  295.   int width, int height, int maskSizeX, int maskSizeY, 
  296.   char minDiff, int nbDepth, int nbPartitions,
  297. StereoBuffer* sb, StereoImage* sdata)
  298. {
  299. // assure good partitioning (.....)
  300. while (height/nbPartitions - 2*(maskSizeY/2) <=0)
  301. {
  302. nbPartitions /= 2;
  303. }
  305. int i,j,k;
  306. int subImageSize=width*height/nbPartitions;
  307. int backStep = (maskSizeY/2)*width;
  308. uchar* ptBuff;
  309. ushort* ptBuff16;
  310. int siz = __max(0, subImageSize - 2*backStep);
  312. // ----------- process the first sub-images -----------
  313. for (i=0; i<nbDepth; i+=8) {
  314. ptBuff = sb->buff[i];
  315. // compute SAD for (imr8_bw, iml8_bw+i), (imr8_bw, iml8_bw+i+1), ....
  316. // .. (imr8_bw, iml8_bw+i+7) 
  317. ImgSubandAdd_Horiz(imr8_bw, iml8_bw+i, ptBuff, subImageSize, sb->buffStep, width);
  319. for (k=0; k<8; ++k) {
  320. ptBuff  = sb->buff[i+k];
  321. ptBuff16 = sb->buff16[i+k];
  322. // apply maskSizeX x maskSizeY avergae mask
  323. sum_Row_mmx(ptBuff , ptBuff16,         subImageSize,maskSizeX);
  324. avg_Col_mmx(ptBuff16 +backStep, ptBuff +backStep, siz,  width, maskSizeY);
  325. // translate data
  326. copyMMX(ptBuff16,ptBuff16+subImageSize-2*backStep,2*backStep*sizeof(short));
  328. if (i==0 && k==0) 
  329. initMinimumCorrelation(ptBuff, 0,  sdata->imDepth8u, sb->corrScore,
  330. sb->corrScoreSec, subImageSize);
  331. else
  332. findMinimumCorrelation_mmx(ptBuff+backStep, i+k, 
  333. sdata->imDepth8u+backStep, 
  334. sb->corrScore+backStep, sb->corrScoreSec+backStep,
  335. subImageSize-backStep);
  336. }
  337. }
  338. // check that there are enough diff. between best and second disp
  339. compareBestAndSecond(sb->corrScore+backStep, sb->corrScoreSec+backStep, minDiff, 
  340.  sdata->UNDEFINED_DEPTH, sdata->imDepth8u+backStep, subImageSize-backStep);
  342. int offset=subImageSize;
  343. // ----------- process the other ones -----------
  344. for (j=1; j<nbPartitions; ++j, offset+=subImageSize) {
  345. // process depth i=0
  346. i=0;
  347. ptBuff = sb->buff[0]+offset;
  348. // estimate absolute differences between the stereo images
  349. ImgSubandAdd_Horiz(imr8_bw +offset, iml8_bw +i +offset, ptBuff,
  350.    subImageSize, sb->buffStep, width);
  351. ptBuff16 = sb->buff16[0];
  352. sb->filterSAD(maskSizeX, maskSizeY, ptBuff, ptBuff16, 
  353.   backStep, subImageSize, offset);
  354. initMinimumCorrelation(ptBuff-backStep, 0, sdata->imDepth8u+offset-backStep, 
  355. sb->corrScore+offset-backStep,
  356. sb->corrScoreSec+offset-backStep, subImageSize);
  358. // process and comparison 
  359. for (int k=1; k<=7; ++k) {
  360. ptBuff  +=sb->buffStep; 
  361. ptBuff16+=sb->buffStep;
  362.     sb->filterSAD(maskSizeX, maskSizeY, ptBuff, ptBuff16, 
  363.   backStep, subImageSize, offset);
  364. findMinimumCorrelation_mmx(ptBuff-backStep, i+k,   sdata->imDepth8u+offset-backStep, 
  365.    sb->corrScore+offset-backStep,  sb->corrScoreSec+offset-backStep,
  366.    subImageSize);
  367. }
  369. for (i=8; i<nbDepth; i+=8) {
  370. ptBuff = sb->buff[i]+offset;
  371. // estimate absolute differences between the 3 stereo images
  372. ImgSubandAdd_Horiz(imr8_bw +offset, iml8_bw + i +offset, ptBuff,
  373. subImageSize, sb->buffStep, width);
  374. ptBuff16 = sb->buff16[i];
  375. // process and comparison 
  376. for (int k=0; k<=7; ++k) {
  377. ptBuff  +=sb->buffStep; 
  378. ptBuff16+=sb->buffStep;
  379. sb->filterSAD(maskSizeX, maskSizeY, ptBuff, ptBuff16, 
  380.  backStep, subImageSize, offset);
  381. findMinimumCorrelation_mmx(ptBuff-backStep, i+k, sdata->imDepth8u+offset-backStep, 
  382.    sb->corrScore+offset-backStep, 
  383.    sb->corrScoreSec+offset-backStep,
  384.    subImageSize);
  385. }
  386. }
  388. // check that there are enough diff. between best and second disp
  389. compareBestAndSecond(sb->corrScore+offset-backStep, 
  390.  sb->corrScoreSec+offset-backStep, minDiff, 
  391.   sdata->UNDEFINED_DEPTH, 
  392.   sdata->imDepth8u+offset-backStep, subImageSize);
  394. }
  396. }