www.pudn.com > Estereo.rar > subPixel.cpp

/***************************************************************************  
* 
* Copyright 2004 by the Massachusetts Institute  of Technology.   All    
* rights reserved.  
*   
* Developed  by David Demirdjian 
* at the Computer Sciences and Artificial Intelligence Laboratory,  
* MIT, Cambridge, Massachusetts.  
*   
* Permission to use, copy, or modify this software and  its documentation  
* for  educational  and  research purposes only and without fee  is hereby  
* granted, provided  that this copyright notice and the original authors's  
* names appear  on all copies and supporting documentation.  If individual  
* files are  separated from  this  distribution directory  structure, this  
* copyright notice must be included.  For any other uses of this software,  
* in original or  modified form, including but not limited to distribution  
* in whole or in  part, specific  prior permission  must be  obtained from  
* MIT.  These programs shall not  be  used, rewritten, or  adapted as  the  
* basis  of  a  commercial  software  or  hardware product  without  first  
* obtaining appropriate licenses  from MIT.  MIT. makes no representations  
* about the suitability of this  software for any purpose.  It is provided  
* "as is" without express or implied warranty.  
*   
**************************************************************************/ 
#include "stereoMatching.h" 
#include "processingMMX.h" 
#include  
 
#define ALLOC_ALIGN_MEMORY(X, X_origin, type, size) (X_origin)=((type*)malloc((size)*sizeof(type)+63)); (X) = (type*)((((unsigned int)(X_origin))+63) & (~63)); 
#define _DDMIN(X,Y) ((X<=Y)?(X):(Y)) 
#define _DDMAX(X,Y) ((X>=Y)?(X):(Y)) 
 
int subpixelDisparity(float *scorePrev, float *scoreInit, float* scoreNext, 
					  float *Dest,  int l) 
{ 
	const float two_float = -2.0f; 
 
	if (l < 8) return 0;              // image size must be at least 8 bytes  
 
  __asm  
  {		 
	  		// make 4 copies of the constant 'radius' in xmm0 
		movss	xmm0, two_float 
		movss	xmm1, xmm0 
		unpcklps	xmm0, xmm1         
		movlhps		xmm0, xmm0 
 
        mov eax, scorePrev      
        mov ebx, scoreInit 
		mov ecx, scoreNext 
 
        mov edi, Dest     
        mov	edx, l    
        shr	edx, 2	 
	 
align 16 
inner_loop: 
		movaps	xmm1,[eax]	// xmm1=scorePrev 
 
		movaps	xmm3,[ecx]	// xmm3=scoreNext 
		movaps  xmm4, xmm1    // xmm4 = scorePrev 
 
		movaps	xmm2,[ebx]	// xmm2=scoreInit 
		subps   xmm4, xmm3	  // xmm4 = scorePrev - scoreNext 
							// xmm4 = delta = scorePrev - scoreNext 
 
		// scoreInit = -2*(scorePrev + scoreNext -2*scoreInit) 
		movaps  xmm5, [edi] 
		mulps   xmm2, xmm0		// nspsbMpy1(-2.0, scoreInit, nbPoints); 
 
		addps   xmm2, xmm1		// nspsbAdd2(scorePrev, scoreInit, nbPoints); 
		addps   xmm2, xmm3		// nspsbAdd2(scoreNext, scoreInit, nbPoints); 
		mulps   xmm2, xmm0		// warning : nspsbMpy1(-2.0, scoreInit, nbPoints); 
				 
		// delta = delta/scoreInit 
		divps  xmm4, xmm2 
 
		// delta += dispFloat 
		subps xmm5, xmm4 
		movaps  [edi], xmm5 
 
        add eax,16          
        add ebx,16     
		add ecx, 16 
        add edi,16		 
        dec edx       
        jnz inner_loop     
        emms   		 
  } 
	 
  return 1; 
} 
 
 
 
// does not check whether disp is in the good range 
// return 'delta', a sub-pixels depth LIST 
void StereoMatching::doSubPixel(const uchar* depth, const int* idx, float* delta, int nbPoints) 
{ 
	if (nbPoints > 0) 
	{ 
 
		int i; 
		float* ptPrev=scorePrev, *ptInit=scoreInit, *ptNext=scoreNext; 
		uchar disp; 
		float* ptDelta = delta; 
		 
		for (i=0; i0)  
	{ 
		int i, idx; 
		float* ptPrev=scorePrev, *ptInit=scoreInit, *ptNext=scoreNext; 
		uchar disp; 
		const short* ptx=x, *pty=y; 
		float* ptDelta = delta; 
		 
		for (i=0; i= (*ptPrev + *ptNext))  
			{ 
				*ptPrev = *ptNext = *ptInit+20; 
			} 
		} 
 
		subpixelDisparity(scorePrev, scoreInit, scoreNext, delta, nbPoints); 
	} 
}