www.pudn.com > Estereo.rar > stereoMatching.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"
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 _DDMIN(X,Y) ((X<=Y)?(X):(Y))
#define _DDMAX(X,Y) ((X>=Y)?(X):(Y))
StereoMatching::StereoMatching()
{
UNDEFINED_DEPTH = 0;
maxNbDepth = 64;
}
StereoMatching::~StereoMatching()
{
destroyContext();
}
void StereoMatching::setHoropter (const int horopt)
{
horopter = horopt;
}
void StereoMatching::setNumDepth (const int nbD)
{
if (nbD % 8 == 0)
nbDepth= nbD;
else
nbDepth= 8*(1 +nbD/8);
}
void StereoMatching::setImageSize(int w, int h)
{
width=w;
height=h;
}
void StereoMatching::setUndefinedDepthValue(const uchar undefined_val)
{
UNDEFINED_DEPTH = undefined_val ;
}
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;
}
void StereoMatching::initializeContext()
{
ALLOC_ALIGN_MEMORY(imDepth_ref, imDepth_ref_origin, uchar, 2*width*height);
ALLOC_ALIGN_MEMORY(imDepth, imDepth_origin, uchar, width*height);
ALLOC_ALIGN_MEMORY(Depth16, Depth16_origin, ushort, width*height);
ALLOC_ALIGN_MEMORY(corrScore, corrScore_origin, uchar, width*height);
ALLOC_ALIGN_MEMORY(corrScoreSec, corrScoreSec_origin, uchar, width*height);
// initialization of buffers ... with 64 bits alignment
ALLOC_ALIGN_MEMORY(buffTemp, buffTemp_origin, ushort, 2*width*height)
ALLOC_ALIGN_MEMORY(bigBuffer, bigBuffer_origin, uchar, width*height*maxNbDepth);
ALLOC_ALIGN_MEMORY(bigBuffer16, bigBuffer16_origin, ushort, width*height*maxNbDepth);
buff = new uchar*[maxNbDepth];
buff16 = new ushort*[maxNbDepth];
for (int i=0; ia2)
multiply(data1, a2/a1, siz);
else
multiply(data2, a1/a2, siz);
}
void normalizeImages(const uchar* data1, const uchar* data2, const uchar* data3,
uchar* out1, uchar* out2, uchar* out3, int siz)
{
float a1 = pixelMean(data1,siz);
float a2 = pixelMean(data2,siz);
float a3 = pixelMean(data3,siz);
float minI = __min(a1, __min(a2,a3));
if (a2==minI)
{
multiply(data1, out1, a2/a1, siz);
multiply(data3, out3, a2/a3, siz);
copyMMX(out2, data2, siz);
}
else if (a1==minI)
{
multiply(data2, out2, a1/a2, siz);
multiply(data3, out3, a1/a3, siz);
copyMMX(out1, data1, siz);
}
else {
multiply(data2, out2, a3/a2, siz);
multiply(data1, out1, a3/a1, siz);
copyMMX(out3, data3, siz);
}
}
void normalizeImages(const uchar* data1, const uchar* data2,
uchar* out1, uchar* out2, int siz)
{
float a1 = pixelMean(data1,siz);
float a2 = pixelMean(data2,siz);
// normalize the image which average intensity is the highest
if (a1>a2)
{
multiply(data1, out1, a2/a1, siz);
copyMMX(out2, data2, siz);
}
else
{
multiply(data2, out2, a1/a2, siz);
copyMMX(out1, data1, siz);
}
}
uchar* StereoMatching::getDisparityImage() const
{
return imDepth;
}
uchar* StereoMatching::getCorrScores() const
{
return corrScore;
}
uchar* StereoMatching::getCorrScores_Sec() const
{
return corrScoreSec;
}
// ------------------------ FULL IMAGE -----------------------------------
// --------- stereo ---------------------------------------------
void StereoMatching::doStereo(const uchar* iml8_bw,
const uchar* imr8_bw,
const uchar* imtop8_bw)
{
normalizeImages(iml8_bw, imr8_bw, imtop8_bw, subIm_l, subIm_r, subIm_t, width*height);
estimateStereo_sse2(subIm_l, subIm_r, subIm_t,
width,height, maskSizeX,maskSizeY,
(uchar)peakValidationThreshold, nbDepth,
buff, width*height,
corrScore, imDepth, 8);
setUndefinedPixels(imDepth);
subPixelPerformed = false;
}
// --------- stereo ---------------------------------------------
void StereoMatching::doStereo(const uchar* iml8_bw, const uchar* imr8_bw)
{
normalizeImages(iml8_bw, imr8_bw, subIm_l, subIm_r, width*height);
estimateStereo_Horiz(subIm_l, subIm_r,
width,height, maskSizeX,maskSizeY,
(uchar)peakValidationThreshold, nbDepth,
buff, width*height,
corrScore, imDepth, 8);
setUndefinedPixels(imDepth);
subPixelPerformed = false;
}
// --------- stereo + filling -----------------------------------
void StereoMatching::doStereo_grow(int mode, const uchar acceptNew, int nbIteration, uchar* imDepth_start)
{
int backStep = (maskSizeY/2)*width;
// if no image given in input, use 'imDepth'
if (imDepth_start == NULL) imDepth_start = imDepth;
// save original imDepth_start in imDepth_ref
memcpy(imDepth_ref , imDepth_start , width*height);
for (int i=0; i=maskSizeY/2 && x=maskSizeY/2 && y=0 && d= 0 && actual_disp != UNDEFINED_DEPTH)
// return (*(buff[d]+idx) < *(buff[actual_disp]+idx) + tol);
//else
// return true;
return (*(buff[d]+idx) < *(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=maskSizeY/2 && y=0 && d