3D图形编程

开发平台：
C/C++

stereoMatching.cpp：源码内容
							/*************************************************************************** 
*
* Copyright 2000 by David Demirdjian.   All rights reserved. 
*  
* Developed  by David Demirdjian
*  
* 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 David Demirdjian.  The author makes 
* no representations about the suitability of this software for any purpose.  
* It is provided "as is" without express or implied warranty. 
*  
**************************************************************************/
#include "stdafx.h"
#include "stereoMatching.h"
#include "processingMMX.h"
typedef unsigned char uchar;
typedef unsigned short ushort;
#define ALLOC_ALIGN_MEMORY(X, X_origin, type, size) (X_origin)=((type*)malloc((size)*sizeof(type)+127)); (X) = (type*)((((unsigned int)(X_origin))+127) & (~127));
#define DELETENULL(object) if (object) {delete object;object = NULL;}
#define FREENULL(object) if (object) {free(object); object = NULL;}
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
// ******************************************************************
// ******************************************************************
StereoMatching::StereoMatching()
{
	maxNbDepth = 48;
	numScales = 5;
	scale = 0;
	leftImageXOffset = 0;
	leftImageYOffset = 0;
	width = 320; 
	height = 240;
	scaleProcessing = MONOSCALE;
	sbuffer = NULL;
	inputImages = NULL;
	scorePrev_origin = scoreInit_origin = scoreNext_origin = NULL;
}
StereoMatching::~StereoMatching()
{
	destroyContext();
}
void StereoMatching::initializeContext()
{
	destroyContext();
	sbuffer = new StereoBuffer[numScales];
	inputImages = new InputImages[numScales];
	for (int i=0, fact=1; i<numScales; ++i)
	{
		sbuffer[i].setSize(width/fact, height/fact, maxNbDepth/fact);
		sbuffer[i].alloc();
		inputImages[i].alloc(width/fact, height/fact, maxNbDepth/fact+21);
		fact *= 2;
	}
	// alloc for sub-pixel 
	ALLOC_ALIGN_MEMORY(scorePrev, scorePrev_origin, float, width*height);
	ALLOC_ALIGN_MEMORY(scoreInit, scoreInit_origin, float, width*height);
	ALLOC_ALIGN_MEMORY(scoreNext, scoreNext_origin, float, width*height);
}
void StereoMatching::destroyContext()
{
	if (sbuffer)
	{
		for (int i=0; i<numScales; ++i)
			sbuffer[i].deAlloc();
		delete[] sbuffer;
		sbuffer = NULL;
	}
	if (inputImages)
	{
		for (int i=0; i<numScales; ++i)
			inputImages[i].deAlloc();
		delete[] inputImages;
		inputImages = NULL;
	}
	FREENULL(scorePrev_origin);
	FREENULL(scoreInit_origin);
	FREENULL(scoreNext_origin);	
}
void StereoMatching::setHoropter (const int horopt)
{
	horopter = horopt;
}
// set numScales
void StereoMatching::setNumScales (const int numScales)
{
	this->numScales = numScales;
}
int StereoMatching::getNumScales() const
{
	return numScales;
}
// set the scale to process
void StereoMatching::setScale (const int scale)
{
	this->scale = scale;
}
int StereoMatching::getScale() const
{
	return scale;
}
void StereoMatching::setLeftImageXOffset(int x)
{
	leftImageXOffset = x;
}
void StereoMatching::setLeftImageYOffset(int y)
{
	leftImageYOffset = y;
}
int StereoMatching::getLeftImageXOffset() const
{
	return leftImageXOffset;
}
int StereoMatching::getLeftImageYOffset() const
{
	return leftImageYOffset;
}
// for monoscale (standard) or multiscale
void StereoMatching::setScaleProcessing(tScaleProcessing sprocessing)
{
	scaleProcessing = sprocessing;
}
StereoMatching::tScaleProcessing StereoMatching::getScaleProcessing(void) const
{
	return scaleProcessing;
}
void StereoMatching::setNumDepth (const int nbD)
{
	if (nbD % 8 == 0) 
		nbDepth= nbD;
	else 
		nbDepth= 8*(1 +nbD/8);
	// change the max. num of depth (for memory allocation)
	if (nbDepth>maxNbDepth) {
		destroyContext();
		maxNbDepth = nbDepth + 32;
		initializeContext();
	}
}
void StereoMatching::setInputImageSize(int w, int h)
{
	if (width!=w && height!=h) {
		destroyContext();
		width=w; 
		height=h;
		initializeContext();
	}
}
void StereoMatching::setAcceptDisparityThreshold(const float newVal)
{
	peakValidationThreshold = newVal;
}
int StereoMatching::getWidth() const
{
	return width;
}
int StereoMatching::getHeight() const
{
	return height;
}
int StereoMatching::getHoropter() const
{
	return horopter;
}
int StereoMatching::getNumDepth() const
{
	return nbDepth;
}
float StereoMatching::getAcceptDisparityThreshold() const
{
	return peakValidationThreshold;
}
void StereoMatching::setCorrelationWindowSize(const int hmaskX, const int hmaskY)
{
	maskSizeX=hmaskX;
	maskSizeY=hmaskY;
}
int StereoMatching::getCorrelationWindowSizeX() const
{
	return maskSizeX;
}
int StereoMatching::getCorrelationWindowSizeY() const
{
	return maskSizeY;
}
// compute normalize/translated images
void StereoMatching::setInputImages(int sc, int  xOffset, int  yOffset, const uchar* iml8_bw,  const uchar* imr8_bw)
{
	InputImages* inImage = inputImages+sc;
	inImage->numImages = 2;
	int f = (int)pow(2.0f, sc);
	int w = width/f, h = height/f;
	if (sc>0) {		
		// shrink images
		shrinkImages(inImage->subIm_l, iml8_bw, width, height, f);
		shrinkImages(inImage->subIm_r, imr8_bw, width, height, f);
		// intensity normalization
		normalizeImages(inImage->subIm_l, inImage->subIm_r, w*h);
	} else {
		// intensity normalization
		normalizeImages(iml8_bw, imr8_bw, inImage->subIm_l, inImage->subIm_r, w*h);
	}
	// translate left image (horopter)
	translateImage((horopter+leftImageXOffset+leftImageYOffset*width)/f, inImage->subIm_l, w*h);
}
// compute normalize/translated images
void StereoMatching::setInputImages(int sc, int  xOffset, int  yOffset, const uchar* iml8_bw,  const uchar* imr8_bw,
									const uchar* imt8_bw)
{
	InputImages* inImage = inputImages+sc;
	inImage->numImages = 3;
	int f = (int)pow(2.0f, sc);
	int w = width/f, h = height/f;
	if (sc>0) {		
		// shrink images
		shrinkImages(inImage->subIm_l, iml8_bw, width, height, f);
		shrinkImages(inImage->subIm_r, imr8_bw, width, height, f);
		shrinkImages(inImage->subIm_t, imt8_bw, width, height, f);
		// intensity normalization
		normalizeImages(inImage->subIm_l, inImage->subIm_r, inImage->subIm_t, w*h);
	} else {
		// intensity normalization
		normalizeImages(iml8_bw, imr8_bw, imt8_bw,
						inImage->subIm_l, inImage->subIm_r, inImage->subIm_t, w*h);
	}
	// translate left image (horopter)
	translateImage(horopter/f, inImage->subIm_l, w*h);
}
// --------- stereo ---------------------------------------------
void StereoMatching::doStereo_fixedScale(StereoImage* simage, int sc, const InputImages* inImage)
{
	int f = (int)pow(2.0f, sc);
// SAD estimation, etc...
	if (inImage->numImages == 2) {
		int numdepths = nbDepth/f;
		numdepths = ((numdepths-1)/8+1)*8;
		estimateStereo_Horiz(inImage->subIm_l, inImage->subIm_r, width/f,height/f,	
							 maskSizeX,maskSizeY,  
							(uchar)peakValidationThreshold,	numdepths, 8,
							 sbuffer+sc, simage);
	} else if (inImage->numImages == 3)
		estimateStereo(inImage->subIm_l, inImage->subIm_r, inImage->subIm_t, 
							width/f,height/f,	 maskSizeX,maskSizeY,  
							(uchar)peakValidationThreshold,	nbDepth/f, 8,
							 sbuffer+sc, simage);
	simage->setUndefinedBorders(maskSizeX/2,maskSizeX/2);
}
void StereoMatching::doStereo(StereoImage* simage, 
							  const uchar* iml8_bw, const uchar* imr8_bw, const uchar* imt8_bw)
{
	// (re)alloc (output) simage if it does not fit
	int f = (int)pow(2.0f, scale);
	if(	simage->width != width/f || simage->height != height/f)
		simage->alloc(width/f,height/f);
	// pre-process (input) images (inputImages)
	if (!imt8_bw) setInputImages(scale, leftImageXOffset, leftImageYOffset, iml8_bw,imr8_bw);
	else setInputImages(scale, leftImageXOffset, leftImageYOffset, iml8_bw,imr8_bw,imt8_bw);
	switch(scaleProcessing) {
		case MONOSCALE:
			if (scale==0) {		
				doStereo_fixedScale(simage, scale, inputImages);
			} else {
				doStereo_fixedScale(simage, scale, inputImages+scale);
			}
			// sub-pixel estimation
			getList_subPixel(simage, scale);
		break;
		case MULTISCALE: {
		// compute image at LOWER scale
			// alloc lowres stereo image
			StereoImage* lowscaleSImage = new StereoImage();
			lowscaleSImage->alloc(simage->width/2,simage->height/2);
			int lowScale = scale+1;
			setInputImages(lowScale, leftImageXOffset, leftImageYOffset, iml8_bw,imr8_bw);
			doStereo_fixedScale(lowscaleSImage, lowScale, inputImages+lowScale);
			// sub-pixel estimation
			getList_subPixel(lowscaleSImage, lowScale);
			StereoImage* highscaleSImage = new StereoImage();
			highscaleSImage->alloc(simage->width,simage->height);
			setInputImages(scale, leftImageXOffset, leftImageYOffset, iml8_bw,imr8_bw);
			doStereo_fixedScale(highscaleSImage, scale, inputImages+scale);
			getList_subPixel(highscaleSImage, scale);
			
			// compute image at HIGHER scale 
			doStereo_multiscale(simage, highscaleSImage, lowscaleSImage, 0);
			delete lowscaleSImage;
			delete highscaleSImage;
		}
		break;
	}
	subPixelPerformed = true;
}
// -------- multiscale algorithm
// assumes simageHigh and simageLow are 2 stereo images estimated at some consecutive scales
void StereoMatching::doStereo_multiscale(StereoImage* simage,
										const StereoImage* simageHigh, const StereoImage* simageLow, 
										const unsigned char acceptNew)
{
	// copy 8u/32f depth image
	unsigned char* ptDepth8u_new = simage->imDepth8u;
	unsigned char* ptDepth8u_lowres = simageLow->imDepth8u;
	for (int i=0; i<simageLow->height; ++i) {
		for (int j=0; j<simageLow->width; ++j) {
			*ptDepth8u_new = 2* *ptDepth8u_lowres;
			*(ptDepth8u_new+1) = 2* *ptDepth8u_lowres;
			*(ptDepth8u_new+simage->width) = 2* *ptDepth8u_lowres;
			*(ptDepth8u_new+simage->width+1) = 2* *ptDepth8u_lowres;
			ptDepth8u_lowres++;
			ptDepth8u_new+=2;
		}
		ptDepth8u_new+=simage->width;
	}
	short* x_low = simageLow->x, *y_low = simageLow->y;
	float* d_low = simageLow->depth_float_list;
	short* x_high = simage->x, *y_high = simage->y;
	float* d_high = simage->depth_float_list;
	simage->m_nGood3DPoints = 4*simageLow->m_nGood3DPoints;
	for (int i=0; i<simageLow->m_nGood3DPoints; ++i,++x_low,++y_low,++d_low) {
		float NEWD = 2* *d_low;
		*x_high = 2* *x_low;
		*y_high = 2* *y_low;
		*d_high = NEWD;
		++x_high; ++y_high; ++d_high;
		*x_high = 2* *x_low;
		*y_high = 2* *y_low+1;
		*d_high = NEWD;
		++x_high; ++y_high; ++d_high;
		*x_high = 2* *x_low+1;
		*y_high = 2* *y_low;
		*d_high = NEWD;
		++x_high; ++y_high; ++d_high;
		*x_high = 2* *x_low+1;
		*y_high = 2* *y_low+1;
		*d_high = NEWD;
		++x_high; ++y_high; ++d_high;
	}
	simage->generateDepth32f();
	float* ptDepth32f_new = simage->imDepth32f;
	float* ptDepth32f_highres = simageHigh->imDepth32f;
	for (int i=0; i<simageHigh->height; ++i) {
		for (int j=0; j<simageHigh->width; ++j) {
			if (*ptDepth32f_highres != 0) *ptDepth32f_new = *ptDepth32f_highres;
			ptDepth32f_highres++;
			ptDepth32f_new++;
		}
	}
	simage->generateDepth8uFromDepth32f();
}
// --------- stereo + filling -----------------------------------	
void StereoMatching::doStereo_grow(StereoImage* sdata, int mode, const uchar acceptNew, int nbIteration)
{
	int backStep = (maskSizeY/2)*width;
	// save original sdata->imDepth8u in imDepth_ref
	memcpy(sbuffer[0].imDepth_ref, sdata->imDepth8u, width*height);
	for (int i=0; i<nbIteration; ++i) {
		// count non undefined pixels
		binarize(sdata->imDepth8u, sbuffer[0].count_non_null_pixels, sdata->UNDEFINED_DEPTH, width*height);
		sum_5x5_mmx(sbuffer[0].count_non_null_pixels, sbuffer[0].count_non_null_pixels,	width*height, width, sbuffer[0].buffTemp);
		// count sum of depths
		set_undefined_to_zero(sdata->imDepth8u, sdata->imDepth8u, sdata->UNDEFINED_DEPTH, width*height);
		sum_5x5_mmx(sdata->imDepth8u, sbuffer[0].Depth16, width*height,	width, sbuffer[0].buffTemp);
		divide(sbuffer[0].Depth16 + 2*width, sbuffer[0].count_non_null_pixels + 2*width, sdata->imDepth8u + 2*width, width*(height-4-(maskSizeY/2)));
	
		if (mode==1)
			checkDisparityValidity(	sdata->imDepth8u+ 2*width, 	sbuffer[0].buff[0]+ 2*width, width*height,
										sbuffer[0].corrScore+ 2*width, 	acceptNew, 
										sdata->UNDEFINED_DEPTH, nbDepth, width*(height-(maskSizeY/2)));
		else 
			checkDisparityValidityAndSearchAround(	sdata->imDepth8u+backStep,	sbuffer[0].buff[0]+backStep, width*height, 
													sbuffer[0].corrScore+backStep, acceptNew, 
													sdata->UNDEFINED_DEPTH, nbDepth, width*height-2*backStep);
				
		overrideImage(sdata->imDepth8u+ 2*width, sbuffer[0].imDepth_ref+ 2*width, sdata->UNDEFINED_DEPTH, width*(height-4-(maskSizeY/2)));
		
		// set to UNDEFINED, pixels of sdata->imDepth8u which are surrounded by 0 defined points
		set_undefined_to_zero(sbuffer[0].count_non_null_pixels+ 2*width, sdata->imDepth8u+ 2*width, sdata->UNDEFINED_DEPTH, 
								width*(height-4) - backStep);
		sdata->setUndefinedBorders(maskSizeX/2,maskSizeX/2);					
	}
}
// ------ functions to estimate validity of points (x,y,d) -------
bool StereoMatching::checkValidity(short x, short y, uchar d, const char tol) const
{
	if (x>=maskSizeY/2 && x<width-maskSizeY/2 && y>=maskSizeY/2 && y<height-maskSizeY/2 && d>=0 && d<nbDepth)
	{
		int idx = x + y*width;
		//uchar actual_disp = *(imDepth8u + idx);
		//if (actual_disp < nbDepth && actual_disp >= 0 && actual_disp != UNDEFINED_DEPTH) 
		//	return  (*(buff[d]+idx) < *(buff[actual_disp]+idx) + tol);
		//else 
		//	return true;
		return (*(sbuffer[0].buff[d]+idx) < *(sbuffer[0].corrScore+idx) + tol);
	}
	else return false;
}
short StereoMatching::checkValidity_error(short x, short y, uchar d, const char tol) const
{
	if (x>=maskSizeY/2 && x<width-maskSizeY/2 && y>=maskSizeY/2 && y<height-maskSizeY/2 && d>=0 && d<nbDepth)
	{
		int idx = x + y*width;
		return abs(*(sbuffer[0].buff[d]+idx) - *(sbuffer[0].corrScore+idx));
	}
	else return -1;
}

						