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

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

3D图形编程

开发平台：

C/C++

misc.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. // *** List of functions for image processing
  29. // ************************************************************
  30. // ************************************************************
  32. #ifdef _DEBUG
  33. #define new DEBUG_NEW
  34. #undef THIS_FILE
  35. static char THIS_FILE[] = __FILE__;
  36. #endif
  39. // ---------------------------------------------------------------
  40. // check if the disparities correspond to 'acceptable' values of
  41. void checkDisparityValidity(uchar* disp, uchar* buff, int buffStep, 
  42. uchar* bestScore, uchar tol, 
  43. uchar undefined_val, uchar nbDepth, int imageSize)
  44. {
  45. uchar* ptDisp = disp, *ptBuff=buff, *ptBestScore = bestScore;
  46. for (int i=0; i<imageSize; ++i,++ptDisp,++ptBuff,++ptBestScore)
  47. {
  48. uchar d=*ptDisp;
  49. if (d!=undefined_val && d>=0 && d<nbDepth ) 
  50. {
  51. uchar* ptBuffToCheck=ptBuff+d*buffStep; // points to the score associated with d
  52. if (*ptBuffToCheck >= *ptBestScore + tol)  *ptDisp=0;
  53. }
  54. }
  55. }
  58. // ---------------------------------------------------------------
  59. // check if the disparities correspond to 'acceptable' values of
  60. void checkDisparityValidityAndSearchAround(uchar* disp, uchar* buff, int buffStep, 
  61. uchar* bestScore, uchar tol, 
  62. uchar undefined_val, uchar nbDepth, int imageSize)
  63. {
  64. float score_prevDisp, score_nextDisp, minScore;
  65. bool test;
  66. uchar* ptDisp = disp, *ptBuff=buff, *ptBestScore = bestScore;
  67. for (int i=0; i<imageSize; ++i,++ptDisp,++ptBuff,++ptBestScore)
  68. {
  69. uchar d=*ptDisp;
  70. if (d!=undefined_val && d>0 && d<nbDepth-1 ) {
  71. uchar* ptBuffToCheck=ptBuff+d*buffStep; // points to the score associated with d
  73. int thresh = *ptBestScore + tol;
  74. if (*ptBuffToCheck >= thresh) 
  75. {
  76. // pixel not valid
  77. score_prevDisp = *(ptBuffToCheck-buffStep) ;
  78. score_nextDisp = *(ptBuffToCheck+buffStep) ;
  79. test = (score_prevDisp < score_nextDisp);
  80. minScore = (test)?score_prevDisp:score_nextDisp;
  81. if (minScore > thresh) 
  82. *ptDisp=0;
  83. else
  84. {
  85. if (test) 
  86. --(*ptDisp); 
  87. else 
  88. ++(*ptDisp);
  89. }
  90. }
  91. }
  92. }
  93. }
  94. #define _ABS_DIFF_TRI_320_240(width,sizeImage,Z) __asm 
  95. { 
  96. __asm movq mm4,mm1 /* mm4=mm1 */ 
  97. __asm por mm3,mm7 /* here mm2=new src2 mm3=new src3 */ 
  99. __asm movq mm7, mm0 
  100. __asm psubusb mm4,mm2 /* mm4 = src1 - src2 */ 
  102. __asm psubusb mm2,mm1 /* mm2 = src2 - src1 */ 
  103. __asm psllq mm7,Z
  105. __asm movq mm5,mm1 /* mm5=src1 */ 
  106. __asm por mm4,mm2 /* mm2=|src1-src2| */ 
  108. __asm movq mm2,[ebx + width] /* mm2= src2 + 'width' = new src2*/ 
  109. __asm psubusb mm5,mm3 /* mm5=src1-src3*/ 
  111. __asm movq mm6,mm3 /* mm6=src3*/ 
  112.     __asm   psubusb mm6,mm1   /* mm3=src3-src1*/ 
  114. __asm por mm6,mm5 /* mm6=|src1-src3|*/ 
  115. __asm paddusb mm4,mm6 /* mm4 = |src1-src2|+|src1-src3|*/ 
  117. __asm movq    [edi+sizeImage], mm4  /* here mm1=src1*/  
  118. __asm psrlq mm3, 8 /* mm3 = src3 + '1' ... with [x00000000] at the end*/
  119. }
  122. //  ImgSubandAdd2: D = saturation0(|S1 - S2| + |S1 - S3|)
  123. // process 8 disparities at a time
  124. //
  125. // Src1: right
  126. // Src2: top
  127. // Src3: left
  128. //
  129. // TODO? divide the result by 2 (shift)
  130. int ImgSubandAdd2_320_240(const unsigned char *Src1, const unsigned char *Src2, 
  131.  const unsigned char *Src3, 
  132.  unsigned char* Dest1, int l)
  133. {
  135. if (l < 8) return 0;              // image size must be at least 8 bytes 
  137.   __asm 
  138.   {
  139.         mov eax, Src1     
  140.         mov ebx, Src2
  141. mov edx, Src3
  142.         mov edi, Dest1   
  144.         mov ecx, l   
  145.         shr ecx, 3
  147. movq mm0,[edx] // mm0=src3
  148. movq mm0,[edx] // mm0=src3
  149. align 16
  150. inner_loop:
  151. // mov esi, width
  152. movq mm1,[eax] // mm1=src1
  153. movq mm3,mm0 // mm3=src3
  155. movq mm2,[ebx] // mm2=src2
  156.         add eax,8         
  157.  
  158. // -- 1 --------- in : mm1,mm2,mm3     out: mm4=SAD  mm2=new mm2 --
  159. movq mm4,mm1 // mm4=mm1
  160. psubusb mm4,mm2 // mm4 = src1 - src2
  162. movq mm0,[edx+8]
  163. psubusb mm2,mm1 // mm2 = src2 - src1
  164.         
  165. movq mm5,mm1 // mm5=src1
  166. por mm4,mm2 // mm2=|src1-src2|
  168. movq mm2,[ebx+320] // mm2= src2 + 'width' = new src2
  169. psubusb mm5,mm3 // mm5=src1-src3
  171. movq mm6,mm3 // mm6=src3
  172.         psubusb mm6,mm1   // mm3=src3-src1
  174. movq mm7, mm0
  175. psrlq mm3, 8 // mm3 = src3 + '1' ... with [x00000000] at the end
  177. por mm6,mm5 // mm6=|src1-src3|
  178. paddusb mm4,mm6 // mm4 = |src1-src2|+|src1-src3|
  180.         movq    [edi], mm4  
  181. psllq mm7, 56 // here mm1=src1 mm2=NEW src2 mm3=begin of NEWsrc3    mm7=end of NEWsrc3
  182. // -------------------------------------------------------------
  185. // - 2 ----------------
  186. _ABS_DIFF_TRI_320_240(640,320*240,48)
  188.   // - 3 ----------------
  189. _ABS_DIFF_TRI_320_240(960,2*320*240,40)
  191.   // - 4 ----------------
  192. _ABS_DIFF_TRI_320_240(1280,3*320*240,32)
  194. // - 5 ----------------
  195. _ABS_DIFF_TRI_320_240(1600,4*320*240,24)
  197. // - 6 ----------------
  198. _ABS_DIFF_TRI_320_240(1920,5*320*240,16)
  200. // - 7 ----------------
  201. _ABS_DIFF_TRI_320_240(2240,6*320*240,8)
  204. // - 8 ----------------
  205. movq mm4,mm1 // mm4=mm1
  206. por mm3,mm7 // here mm2=new src2 mm3=new src3
  208. psubusb mm4,mm2 // mm4 = src1 - src2
  209. psubusb mm2,mm1 // mm2 = src2 - src1
  210.         
  211. movq mm5,mm1 // mm5=src1
  212. por mm4,mm2 // mm2=|src1-src2|
  214. psubusb mm5,mm3 // mm5=src1-src3
  215.         psubusb mm3,mm1   // mm3=src3-src1
  217. por mm3,mm5 // mm6=|src1-src3|
  218. paddusb mm4,mm3 // mm4 = |src1-src2|+|src1-src3|
  220.         movq    [edi+2560], mm4  // here mm1=src1
  221. // -------------------------------------------------------------
  222.   // 
  223.         add ebx,8
  224.         add edx,8     
  225.         add edi,8
  226.         dec ecx      
  227.         jnz inner_loop    
  228.         emms   
  229.   }
  231.   return 1;
  232. }
  235. // macro: in: mm1,mm2
  236. #define _ABS_DIFF_HORIZ_16(Z) __asm 
  237. { 
  238. __asm  movq mm7, mm0 
  239. __asm  add edi, imageSize 
  240. __asm  movq mm5,mm1 /* mm5=src1 */ 
  241. __asm  psllq mm7, Z 
  242. __asm  psubusb mm5,mm3 /* mm5=src1-src3 */ 
  243. __asm  movq mm6,mm3 /* mm6=src3 */ 
  244.     __asm  psubusb mm6,mm1   /* mm3=src3-src1 */ 
  245. __asm  por mm6,mm5 /* mm6=|src1-src3| */ 
  246. __asm  psrlq mm3, 8 /* mm3 = src3 + '1' ... with [x00000000] at the end */ 
  247. __asm  por mm3,mm7 /* here mm3=new src3 */ 
  249. __asm  movq mm7, mm6 /* copy results to mm7 as well */ 
  250. __asm  pxor mm5, mm5  
  251. __asm  punpcklbw mm6, mm5 /* unpack mm6 */ 
  253.     __asm  movq    [edi], mm6  /* here mm1=src1 */ 
  254. __asm  punpckhbw mm7, mm5 /* unpack mm6 */ 
  255.     __asm  movq    [edi+8], mm7  /* here mm1=src1 */ 
  256. }
  258. //  ImgSubandAdd2: D = saturation0(|S1 - S2| + |S1 - S3|)
  259. // process 8 disparities at a time
  260. //
  261. // Src1: right
  262. // Src2: top
  263. // Src3: left
  264. //
  265. // TODO? divide the result by 2 (shift)
  266. int ImgSubandAdd2_Horiz_16(const unsigned char *rightIm, const unsigned char *leftIm, 
  267.    unsigned short* Dest, int l, int imageSize, int width)
  268. {
  270. if (l < 8) return 0;              // image size must be at least 8 bytes 
  271. const int back_step2 = 7*imageSize;
  272.   __asm 
  273.   {
  274.         mov eax, rightIm     
  275.   mov edx, leftIm
  276.         mov edi, Dest 
  278.         mov ecx, l   
  279.         shr ecx, 3
  281. movq mm0,[edx] // mm0=src3
  282. movq mm0,[edx] // mm0=src3
  283. align 16
  284. inner_loop:
  286. movq mm1,[eax] // mm1=src1
  287. movq mm3,mm0 // mm3=src3
  289. // -- 1 --------- in : mm1,mm2,mm3     out: mm4=SAD  mm2=new mm2 --
  290. movq mm0,[edx+8]
  291.         add eax,8         
  292.    
  293. movq mm5,mm1 // mm5=src1
  294. psubusb mm5,mm3 // mm5=src1-src3
  296. movq mm6,mm3 // mm6=src3
  297.         psubusb mm6,mm1   // mm3=src3-src1
  299. movq mm7, mm0
  300. psrlq mm3, 8 // mm3 = src3 + '1' ... with [x00000000] at the end
  302. por mm6,mm5 // mm6=|src1-src3|
  303. psllq mm7, 56 // here mm1=src1 mm3=begin of NEWsrc3    mm7=end of NEWsrc3
  304. por mm3,mm7 // here mm3=new src3
  306. // ------ new ---------
  307. movq mm7, mm6 // copy results to mm7 as well
  308. pxor mm5, mm5
  309. punpcklbw mm6, mm5 // unpack mm6
  311. movq    [edi], mm6  /* here mm1=src1 */ 
  312. punpckhbw mm7, mm5 // unpack mm6
  313. movq    [edi+8], mm7  /* here mm1=src1 */ 
  315. // - 2 ----------------
  316. _ABS_DIFF_HORIZ_16(48)
  317. _ABS_DIFF_HORIZ_16(40)
  318. _ABS_DIFF_HORIZ_16(32)
  319. _ABS_DIFF_HORIZ_16(24)
  320. _ABS_DIFF_HORIZ_16(16)
  321. _ABS_DIFF_HORIZ_16(8)
  323. // - 8 ----------------
  324. movq mm5,mm1 // mm5=src1
  325. add edi, imageSize
  327. psubusb mm5,mm3 // mm5=src1-src3
  328.         psubusb mm3,mm1   // mm3=src3-src1
  330. por mm3,mm5 // mm6=|src1-src3|
  331.         movq    [edi], mm3  
  333. // ------ new ---------
  334. movq mm7, mm3 // copy results to mm7 as well
  335. pxor mm5, mm5
  336. punpcklbw mm3, mm5 // unpack mm6
  338. movq    [edi], mm3  /* here mm1=src1 */ 
  339. punpckhbw mm7, mm5 // unpack mm6
  340. movq    [edi+8], mm7  /* here mm1=src1 */ 
  342. // -------------------------------------------------------------
  343.   // 
  344.         add edx,8     
  345. sub edi, back_step2
  346.         add edi,16
  347.         dec ecx      
  348.         jnz inner_loop    
  349.         emms   
  350.   }
  352.   return 1;
  353. }