reconst3D.cpp
资源名称:estereo2.zip [点击查看]
上传用户:fengshi120
上传日期:2014-07-17
资源大小:6155k
文件大小:5k
源码类别:
3D图形编程
开发平台:
C/C++
- /***************************************************************************
- *
- * 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)
- {
- 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++;
- }
- // alloc.
- reconstPts3d->FreeMemoryBuffers();
- reconstPts3d->alloc(reconstPts3d->getNumPoints());
- float fBOverD;
- DWORD iP = 0;
- ptd_src = (FLOAT*)pStereoData->imDepth32f;
- for(UINT row=0;row<(UINT)pStereoData->height;row++)
- {
- for(UINT col=0;col<(UINT)pStereoData->width;col++)
- {
- fDisp = *ptd_src;
- if(fDisp > 0.0f) {
- 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;
- iP++;
- }
- ptd_src++;
- }
- }
- return RECONST_OK;
- }