reconst3D.cpp
上传用户:fengshi120
上传日期:2014-07-17
资源大小:6155k
文件大小:5k
源码类别:

3D图形编程

开发平台:

C/C++

  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 "reconst3d.h"
  24. #include "processingMMX.h"
  25. #ifdef _DEBUG
  26. #define new DEBUG_NEW
  27. #undef THIS_FILE
  28. static char THIS_FILE[] = __FILE__;
  29. #endif
  30. // ************************************************************
  31. // ************************************************************
  32. Reconst3D::Reconst3D()
  33. {
  34. m_fFocalLength = 0.0f;
  35. m_fBaseLine = 0.0f;
  36. m_u0 = 0.0f;
  37. m_v0 = 0.0f;
  38. }
  39. Reconst3D::~Reconst3D()
  40. {
  41. }
  42. void Reconst3D::setCameraParameters(float f, float b, float u, float v)
  43. {
  44. m_fFocalLength = f;
  45. m_fBaseLine = b;
  46. m_u0 = u;
  47. m_v0 = v;
  48. }
  49. void Reconst3D::getCameraParameters(float& f, float& b, float& u, float& v) const
  50. {
  51. f = m_fFocalLength;
  52. b = m_fBaseLine;
  53. u = m_u0;
  54. v = m_v0;
  55. }
  56. #ifdef _USEGDI
  57. // 3D reconstruction
  58. int Reconst3D::doReconstruction(ReconstPoints3D* reconstPts3d, const StereoImage* pStereoData,
  59. Bitmap *bitmap)
  60. {
  61. if(m_fFocalLength == 0.0f)
  62. return CAMERA_PARAM_NOT_SET;
  63. if(!bitmap)
  64. return IMAGE_BUFFER_NULL;
  65. UINT uImageWidth = 0,uImageHeight = 0;
  66. if (bitmap) {
  67. uImageWidth = bitmap->GetWidth();
  68. uImageHeight = bitmap->GetHeight();
  69. if( pStereoData->width != (int)uImageWidth || pStereoData->height > (int)uImageHeight)
  70. return WRONG_IMAGE_SIZE;
  71. }
  72. // verify valid pixel count. and get disparity range
  73. float fDisp;
  74. reconstPts3d->m_nGood3DPoints = 0;
  75. FLOAT* ptd_src = (FLOAT*)pStereoData->imDepth32f;
  76. for(UINT i=0;i<pStereoData->height*pStereoData->width;++i, ++ptd_src) {
  77. if (*ptd_src > 0 ) reconstPts3d->m_nGood3DPoints++;
  78. }
  79. // alloc.
  80. reconstPts3d->FreeMemoryBuffers();
  81. reconstPts3d->alloc(reconstPts3d->getNumPoints());
  82. float fBOverD;
  83. DWORD iP = 0;
  84. Rect rect(0,0,uImageWidth,uImageHeight);
  85. BitmapData *pBitmapData = new BitmapData;
  86. if(bitmap->LockBits(&rect,ImageLockModeRead,PixelFormat32bppARGB,pBitmapData) == Ok)
  87. {
  88. ptd_src = (FLOAT*)pStereoData->imDepth32f;
  89. Color* pLine = (Color*)pBitmapData->Scan0;
  90. UINT uPixelsPerLine = pBitmapData->Stride / 4;
  91. for(UINT row=0;row<(UINT)pStereoData->height;row++)
  92. {
  93. for(UINT col=0;col<(UINT)pStereoData->width;col++)
  94. {
  95. fDisp = *ptd_src;
  96. if (fDisp> 0 ) {
  97. fBOverD = m_fBaseLine / fDisp;
  98. reconstPts3d->m_p3DPointsX[iP] = (float)(col - m_u0)*(fBOverD);
  99. reconstPts3d->m_p3DPointsY[iP] = (float)(row - m_v0)*(fBOverD);
  100. reconstPts3d->m_p3DPointsZ[iP] = fBOverD * m_fFocalLength;
  101. reconstPts3d->m_p3DPointsS[iP] = fBOverD;
  102. reconstPts3d->m_p3DPointsC[4*iP] = pLine[col].GetR();
  103. reconstPts3d->m_p3DPointsC[4*iP+1] = pLine[col].GetG();
  104. reconstPts3d->m_p3DPointsC[4*iP+2] = pLine[col].GetB();
  105. reconstPts3d->m_p3DPointsC[4*iP+3] = pLine[col].GetA();
  106. iP++;
  107. }
  108. ptd_src++;
  109. }
  110. pLine += uPixelsPerLine;
  111. }
  112. bitmap->UnlockBits(pBitmapData);
  113. }
  114. delete pBitmapData;
  115. return RECONST_OK;
  116. }
  117. #endif
  118. // 3D reconstruction
  119. int Reconst3D::doReconstruction(ReconstPoints3D* reconstPts3d, const StereoImage* pStereoData)
  120. {
  121. if(m_fFocalLength == 0.0f)
  122. return CAMERA_PARAM_NOT_SET;
  123. // verify valid pixel count. and get disparity range
  124. float fDisp;
  125. reconstPts3d->m_nGood3DPoints = 0;
  126. FLOAT* ptd_src = (FLOAT*)pStereoData->imDepth32f;
  127. for(UINT i=0;i<pStereoData->height*pStereoData->width;++i, ++ptd_src) {
  128. if (*ptd_src> 0 ) reconstPts3d->m_nGood3DPoints++;
  129. }
  130. // alloc.
  131. reconstPts3d->FreeMemoryBuffers();
  132. reconstPts3d->alloc(reconstPts3d->getNumPoints());
  133. float fBOverD;
  134. DWORD iP = 0;
  135. ptd_src = (FLOAT*)pStereoData->imDepth32f;
  136. for(UINT row=0;row<(UINT)pStereoData->height;row++)
  137. {
  138. for(UINT col=0;col<(UINT)pStereoData->width;col++)
  139. {
  140. fDisp = *ptd_src;
  141. if(fDisp > 0.0f) {
  142. fBOverD = m_fBaseLine / fDisp;
  143. reconstPts3d->m_p3DPointsX[iP] = (float)(col - m_u0)*(fBOverD);
  144. reconstPts3d->m_p3DPointsY[iP] = (float)(row - m_v0)*(fBOverD);
  145. reconstPts3d->m_p3DPointsZ[iP] = fBOverD * m_fFocalLength;
  146. reconstPts3d->m_p3DPointsS[iP] = fBOverD;
  147. iP++;
  148. }
  149. ptd_src++;
  150. }
  151. }
  152. return RECONST_OK;
  153. }