3D图形编程

开发平台：
C/C++

reconst3D.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 "reconst3d.h"
#include "processingMMX.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
// ************************************************************
// ************************************************************
Reconst3D::Reconst3D()
{
	m_fFocalLength = 0.0f;
	m_fBaseLine = 0.0f;
	m_u0 = 0.0f;
	m_v0 = 0.0f;
}
Reconst3D::~Reconst3D()
{
}
void Reconst3D::setCameraParameters(float f, float b, float u, float v)
{
	m_fFocalLength = f;
	m_fBaseLine = b;
	m_u0 = u;
	m_v0 = v;
}
void Reconst3D::getCameraParameters(float& f, float& b, float& u, float& v) const
{
	f = m_fFocalLength;
	b = m_fBaseLine;
	u = m_u0;
	v = m_v0;
}
#ifdef _USEGDI
// 3D reconstruction
int Reconst3D::doReconstruction(ReconstPoints3D* reconstPts3d, const StereoImage* pStereoData,
								Bitmap *bitmap)
{
	if(m_fFocalLength == 0.0f)
		return CAMERA_PARAM_NOT_SET;
	if(!bitmap)
		return IMAGE_BUFFER_NULL;
	UINT uImageWidth = 0,uImageHeight = 0;
	if (bitmap) {
		uImageWidth = bitmap->GetWidth();
		uImageHeight = bitmap->GetHeight();
		if(	pStereoData->width != (int)uImageWidth || pStereoData->height > (int)uImageHeight)
			return WRONG_IMAGE_SIZE;
	}
	// verify valid pixel count. and get disparity range
	float fDisp;
	reconstPts3d->m_nGood3DPoints = 0;
	FLOAT* ptd_src = (FLOAT*)pStereoData->imDepth32f;
	for(UINT i=0;i<pStereoData->height*pStereoData->width;++i, ++ptd_src) {
		if (*ptd_src > 0 ) reconstPts3d->m_nGood3DPoints++;		
	}
	// alloc.
	reconstPts3d->FreeMemoryBuffers();
	reconstPts3d->alloc(reconstPts3d->getNumPoints());
	float fBOverD;
	DWORD iP = 0;
	Rect rect(0,0,uImageWidth,uImageHeight);
	BitmapData *pBitmapData = new BitmapData;
	if(bitmap->LockBits(&rect,ImageLockModeRead,PixelFormat32bppARGB,pBitmapData) == Ok)
	{
		ptd_src = (FLOAT*)pStereoData->imDepth32f;
		Color* pLine = (Color*)pBitmapData->Scan0;
		UINT uPixelsPerLine = pBitmapData->Stride / 4;
		for(UINT row=0;row<(UINT)pStereoData->height;row++)
		{
			for(UINT col=0;col<(UINT)pStereoData->width;col++)
			{
				fDisp = *ptd_src;
				if (fDisp> 0 ) {
					fBOverD = m_fBaseLine / fDisp;
					reconstPts3d->m_p3DPointsX[iP] = (float)(col - m_u0)*(fBOverD);
					reconstPts3d->m_p3DPointsY[iP] = (float)(row - m_v0)*(fBOverD);
					reconstPts3d->m_p3DPointsZ[iP] = fBOverD * m_fFocalLength;
					reconstPts3d->m_p3DPointsS[iP] = fBOverD;
					reconstPts3d->m_p3DPointsC[4*iP] = pLine[col].GetR();
					reconstPts3d->m_p3DPointsC[4*iP+1] = pLine[col].GetG();
					reconstPts3d->m_p3DPointsC[4*iP+2] = pLine[col].GetB();
					reconstPts3d->m_p3DPointsC[4*iP+3] = pLine[col].GetA();
					iP++;
				}
				ptd_src++;
			}
			pLine += uPixelsPerLine;
		}
		bitmap->UnlockBits(pBitmapData);
	}
	delete pBitmapData;
	return RECONST_OK;
}
#endif
// 3D reconstruction
int Reconst3D::doReconstruction(ReconstPoints3D* reconstPts3d, const StereoImage* pStereoData,
								float scale)
{
	if(m_fFocalLength == 0.0f)
		return CAMERA_PARAM_NOT_SET;
	// verify valid pixel count. and get disparity range
	//float fDisp;
	reconstPts3d->m_nGood3DPoints = 0;
	FLOAT* ptd_src = (FLOAT*)pStereoData->imDepth32f;
	for(UINT i=0;i<pStereoData->height*pStereoData->width;++i, ++ptd_src) {
		if (*ptd_src> 0 ) reconstPts3d->m_nGood3DPoints++;
		//reconstPts3d->m_nGood3DPoints+=(*ptd_src>0);
	}
	// alloc.
	reconstPts3d->FreeMemoryBuffers();
	reconstPts3d->alloc(reconstPts3d->getNumPoints());
	float fBOverD;
	ptd_src = (FLOAT*)pStereoData->imDepth32f;
//	DWORD iP = 0;
	float* X = reconstPts3d->m_p3DPointsX;
	float* Y = reconstPts3d->m_p3DPointsY;
	float* Z = reconstPts3d->m_p3DPointsZ;
	float* S = reconstPts3d->m_p3DPointsS;
	for(UINT row=0;row<(UINT)pStereoData->height;row++)
	{
		for(UINT col=0;col<(UINT)pStereoData->width;col++)
		{
			//fDisp = *ptd_src;
			if(*ptd_src > 0.0f) {						
				//fBOverD = m_fBaseLine / fDisp;
				fBOverD = m_fBaseLine / (*ptd_src);
				*X = (float)(scale*col - m_u0)*(fBOverD);
				*Y = (float)(scale*row - m_v0)*(fBOverD);
				*Z = fBOverD * m_fFocalLength;
				*S = fBOverD;
				++X; ++Y; ++Z; ++S;
				//reconstPts3d->m_p3DPointsX[iP] = (float)(scale*col - m_u0)*(fBOverD);
				//reconstPts3d->m_p3DPointsY[iP] = (float)(scale*row - m_v0)*(fBOverD);
				//reconstPts3d->m_p3DPointsZ[iP] = fBOverD * m_fFocalLength;
				//reconstPts3d->m_p3DPointsS[iP] = fBOverD;
				//iP++;
			}
			++ptd_src;
		}
	}
	return RECONST_OK;
}

						