// MatchImpl.cpp: implementation of the CMatchImpl class.
|
//
|
//////////////////////////////////////////////////////////////////////
|
|
#include "stdafx.h"
|
#include "MatchImpl.h"
|
#include "SISMath.h"
|
|
#ifdef _DEBUG
|
#define new DEBUG_NEW
|
#undef THIS_FILE
|
static char THIS_FILE[] = __FILE__;
|
#endif
|
|
//////////////////////////////////////////////////////////////////////
|
// Construction/Destruction
|
//////////////////////////////////////////////////////////////////////
|
#include <emmintrin.h>
|
//#include <tmmintrin.h>
|
//#include <ia32intrin.h>
|
#include <math.h>
|
|
//#define MOSIS_ASM
|
|
extern "C"{
|
int64 _asm_Get_CCU8(const uchar * vec1, const uchar * vec2, int len, int flag);// 0= aa, 1=au, 2=uu
|
};
|
|
|
float GetRegressionCoeffInPoly(float* points , float *scores, int nSize);
|
|
CMatchImpl::CMatchImpl()
|
{
|
m_bBigBufferChanged= TRUE;
|
m_bSmallBufferChanged= TRUE;
|
}
|
|
CMatchImpl::~CMatchImpl()
|
{
|
|
m_BigBuff16.ReleaseSpace();
|
m_SmallBuff16.ReleaseSpace();
|
|
m_BigBuff8.ReleaseSpace();
|
m_SmallBuff8.ReleaseSpace();
|
}
|
BOOL CMatchImpl::SetBigImage(CSISBuffer bigBuffer, CRect roi)
|
{
|
m_BigOriginal= bigBuffer;
|
m_BigRect= roi;
|
|
m_bBigBufferChanged= TRUE;
|
|
return TRUE;
|
}
|
BOOL CMatchImpl::SetSmallImage(CSISBuffer smallBuffer, CRect roi)
|
{
|
m_SmallOriginal= smallBuffer;
|
m_SmallRect= roi;
|
|
// m_SmallRect.bottom= (m_SmallRect.top+ m_SmallRect.bottom)/2;
|
// m_SmallRect.right= (m_SmallRect.left+ m_SmallRect.right)/2;
|
|
m_bSmallBufferChanged= TRUE;
|
m_SmallBuff8.SetSize(m_SmallRect.Width(), m_SmallRect.Height());
|
m_SmallBuff8.CopyFrom(m_SmallOriginal, m_SmallRect);
|
|
return TRUE;
|
}
|
|
BOOL CMatchImpl::Check_Buffer(CUShortBuff &smallBuffer, CUShortBuff &bigBuffer)
|
{
|
if(!smallBuffer.IsValidBuffer())
|
return FALSE;
|
if(!bigBuffer.IsValidBuffer())
|
return FALSE;
|
if(smallBuffer.GetWidth() >= bigBuffer.GetWidth())
|
return FALSE;
|
if(smallBuffer.GetHeight() >= bigBuffer.GetHeight())
|
return FALSE;
|
return TRUE;
|
}
|
BOOL CMatchImpl::Check_Buffer(CByteBuff &smallBuffer, CByteBuff &bigBuffer)
|
{
|
if(!smallBuffer.IsValidBuffer())
|
return FALSE;
|
if(!bigBuffer.IsValidBuffer())
|
return FALSE;
|
if(smallBuffer.GetWidth() >= bigBuffer.GetWidth())
|
return FALSE;
|
if(smallBuffer.GetHeight() >= bigBuffer.GetHeight())
|
return FALSE;
|
return TRUE;
|
}
|
|
BOOL CMatchImpl::DoTemplateMatch(stSISMatchResult &matchResult, stSISMatchParam param)
|
{
|
stSISMatchResult result;
|
result.Invalidate();
|
matchResult.Invalidate();
|
m_MatchParam= param;
|
m_MatchParam.ValidateParam();
|
|
|
if(m_SmallRect.Width()+ 10 >= m_BigRect.Width())
|
{
|
m_MatchParam.m_Pyramid= 0;
|
}
|
else
|
{
|
// m_MatchParam.m_Pyramid= 2;
|
}
|
|
// m_MatchParam.m_Acceptance= 0.3;
|
// m_MatchParam.m_bInterpol= TRUE;
|
// m_MatchParam.m_Method= stSISMatchParam::CV_TM_CCOEFF_NORMED;
|
// m_MatchParam.m_Method= stSISMatchParam::CV_TM_CCORR_NORMED;
|
|
if(m_SmallRect.Width() >= m_BigRect.Width())
|
return FALSE;
|
if(m_SmallRect.Height() >= m_BigRect.Height())
|
return FALSE;
|
|
|
stSISMatchResult result2;
|
BOOL bRet= TRUE;
|
int sWidth, sHeight;
|
int hWidth, hHeight;
|
CRect roi;
|
CPoint mpt;
|
|
if(m_MatchParam.m_Method == stSISMatchParam::CV_TM_CCORR_NORMED)
|
{
|
if(m_MatchParam.m_Pyramid > 0)
|
{
|
if(! m_SmallBuff16.IsValidBuffer() || m_bSmallBufferChanged)
|
{
|
if(m_SmallBuff16.SetBuffPyramid(m_SmallOriginal, m_SmallRect, m_MatchParam.GetMulti()))
|
m_bSmallBufferChanged= FALSE;
|
}
|
if(! m_BigBuff16.IsValidBuffer() || m_bBigBufferChanged)
|
{
|
if(m_BigBuff16.SetBuffPyramid(m_BigOriginal, m_BigRect, m_MatchParam.GetMulti()))
|
m_bBigBufferChanged= FALSE;
|
}
|
|
|
result.Invalidate();
|
bRet= MatchImpl(m_SmallBuff16, m_BigBuff16, result, 0, m_MatchParam.m_bInterpol);
|
|
// result.m_X= m_BigRect.left+ result.m_X*m_MatchParam.GetMulti();
|
// result.m_Y= m_BigRect.top+ result.m_Y*m_MatchParam.GetMulti();
|
mpt.x= m_BigRect.left+ (int)(result.m_X*m_MatchParam.GetMulti()+ 0.5);
|
mpt.y= m_BigRect.top+ (int)(result.m_Y*m_MatchParam.GetMulti()+ 0.5);
|
|
matchResult= result;
|
if(bRet == FALSE)
|
{
|
return FALSE;
|
}
|
|
|
sWidth= m_SmallRect.Width();
|
sHeight= m_SmallRect.Height();
|
|
hWidth= m_MatchParam.GetMulti()+1;
|
hHeight= m_MatchParam.GetMulti()+1;
|
roi.SetRect(mpt.x- hWidth, mpt.y- hHeight, mpt.x+ sWidth+ hWidth, mpt.y+ sHeight+ hHeight);
|
m_BigBuff8.SetSize(roi.Width(), roi.Height());
|
m_BigBuff8.CopyFrom(m_BigOriginal, roi);
|
|
result.Invalidate();
|
bRet= MatchImpl(m_SmallBuff8, m_BigBuff8, result, 0, m_MatchParam.m_bInterpol);
|
|
matchResult.m_X= roi.left+ result.m_X- m_BigRect.left;
|
matchResult.m_Y= roi.top+ result.m_Y- m_BigRect.top;
|
matchResult.m_Score= result.m_Score;
|
|
result.m_X= matchResult.m_X+ m_BigRect.left;
|
result.m_Y= matchResult.m_Y+ m_BigRect.top;
|
|
return matchResult.m_Score >= m_MatchParam.m_Acceptance;
|
}
|
|
m_BigBuff8.SetSize(m_BigRect.Width(), m_BigRect.Height());
|
if(!m_BigBuff8.CopyFrom(m_BigOriginal, m_BigRect))
|
return FALSE;
|
|
result.Invalidate();
|
bRet= MatchImpl(m_SmallBuff8, m_BigBuff8, result, 0, m_MatchParam.m_bInterpol);
|
matchResult= result;
|
return matchResult.m_Score >= m_MatchParam.m_Acceptance;
|
}
|
|
if(m_MatchParam.m_Pyramid > 0)
|
{
|
if(! m_SmallBuff16.IsValidBuffer() || m_bSmallBufferChanged)
|
{
|
if(m_SmallBuff16.SetBuffPyramid(m_SmallOriginal, m_SmallRect, m_MatchParam.GetMulti()))
|
m_bSmallBufferChanged= FALSE;
|
}
|
if(! m_BigBuff16.IsValidBuffer() || m_bBigBufferChanged)
|
{
|
if(m_BigBuff16.SetBuffPyramid(m_BigOriginal, m_BigRect, m_MatchParam.GetMulti()))
|
m_bBigBufferChanged= FALSE;
|
}
|
|
|
result.Invalidate();
|
bRet= MatchImpl_Coef(m_SmallBuff16, m_BigBuff16, result, 0, m_MatchParam.m_bInterpol);
|
// result.m_X= m_BigRect.left+ result.m_X*m_MatchParam.GetMulti();
|
// result.m_Y= m_BigRect.top+ result.m_Y*m_MatchParam.GetMulti();
|
mpt.x= m_BigRect.left+ (int)(result.m_X*m_MatchParam.GetMulti()+ 0.5);
|
mpt.y= m_BigRect.top+ (int)(result.m_Y*m_MatchParam.GetMulti()+ 0.5);
|
|
matchResult= result;
|
if(bRet == FALSE)
|
{
|
return FALSE;
|
}
|
|
sWidth= m_SmallRect.Width();
|
sHeight= m_SmallRect.Height();
|
|
hWidth= m_MatchParam.GetMulti()+1;
|
hHeight= m_MatchParam.GetMulti()+1;
|
|
stSISMatchResult resultBest;
|
|
//#define MOSIS_DEBUG_INSPECTION_MATCHING
|
#if defined(MOSIS_DEBUG_INSPECTION_MATCHING)// ¸ÅĪ À̹ÌÁö°¡ 2ÁÖ±â ÀÌ¸ç ¾ÐÃà(ÇǶó¹Ìµå)¸ÅĪ½Ã 1Áֱ⾿ Ʋ¾îÁüÀ» °¨¾ÈÇØ 4¹øÀÇ ¼¼ºÎ ¸ÅĪÀ» ÇÏ¿© ¸ÅĪ ¿ÀÂ÷¸¦ ¾ø¾Ø´Ù.
|
|
int hsWidth= sWidth/2-1;
|
int hsHeight= sHeight/2-1;
|
|
int xMatch= 0, yMatch;
|
int x, y;
|
for(xMatch = 0; xMatch < 2; xMatch++)
|
{
|
for(yMatch= 0; yMatch < 2; yMatch++)
|
{
|
x= result.m_X+ xMatch*sWidth/2;
|
y= result.m_Y+ yMatch*sHeight/2;
|
roi.SetRect(x- hWidth, y- hHeight, x+ sWidth+ hWidth, y+ sHeight+ hHeight);
|
m_BigBuff8.SetSize(roi.Width(), roi.Height());
|
m_BigBuff8.CopyFrom(m_BigOriginal, roi);
|
|
matchResult.Invalidate();
|
bRet= MatchImpl_Coef(m_SmallBuff8, m_BigBuff8, matchResult, 0, m_MatchParam.m_bInterpol);
|
|
matchResult.m_X= roi.left+ matchResult.m_X- m_BigRect.left;
|
matchResult.m_Y= roi.top+ matchResult.m_Y- m_BigRect.top;
|
//matchResult.m_Score= result.m_Score;
|
if(matchResult.m_Score > resultBest.m_Score)
|
{
|
resultBest= matchResult;
|
}
|
}
|
}
|
#else
|
//roi.SetRect(result.m_X- hWidth, result.m_Y- hHeight, result.m_X+ sWidth+ hWidth, result.m_Y+ sHeight+ hHeight);
|
roi.SetRect(mpt.x- hWidth, mpt.y- hHeight, mpt.x+ sWidth+ hWidth, mpt.y+ sHeight+ hHeight);
|
m_BigBuff8.SetSize(roi.Width(), roi.Height());
|
m_BigBuff8.CopyFrom(m_BigOriginal, roi);
|
|
resultBest.Invalidate();
|
bRet= MatchImpl_Coef(m_SmallBuff8, m_BigBuff8, resultBest, 0, m_MatchParam.m_bInterpol);
|
|
resultBest.m_X= roi.left+ resultBest.m_X- m_BigRect.left;
|
resultBest.m_Y= roi.top+ resultBest.m_Y- m_BigRect.top;
|
|
#endif
|
|
matchResult= resultBest;
|
result.m_X= resultBest.m_X+ m_BigRect.left;
|
result.m_Y= resultBest.m_Y+ m_BigRect.top;
|
|
return matchResult.m_Score >= m_MatchParam.m_Acceptance;
|
}
|
|
m_BigBuff8.SetSize(m_BigRect.Width(), m_BigRect.Height());
|
if(!m_BigBuff8.CopyFrom(m_BigOriginal, m_BigRect))
|
return FALSE;
|
|
result.Invalidate();
|
bRet= MatchImpl_Coef(m_SmallBuff8, m_BigBuff8, result, 0, m_MatchParam.m_bInterpol);
|
matchResult= result;
|
return matchResult.m_Score >= m_MatchParam.m_Acceptance;
|
}
|
/* calculates 1/sqrt(val) */
|
#define FLT_EPSILON 1.192092896e-07F /* smallest such that 1.0+FLT_EPSILON != 1.0 */
|
#define _CV_SQRT_MAGIC 0xbe6f0000
|
double
|
h_InvSqrt64d( double arg )
|
{
|
double x, y;
|
float t = (float) arg;
|
|
*((unsigned *) &t) = (_CV_SQRT_MAGIC - *((unsigned *) &t)) >> 1;
|
y = arg * 0.5;
|
x = t;
|
x *= 1.5 - y * x * x;
|
x *= 1.5 - y * x * x;
|
x *= 1.5 - y * x * x;
|
x *= 1.5 - y * x * x;
|
|
|
return x;
|
}
|
#define MOSIS_SQRT(A) sqrt(fabs(A))
|
#define MOSIS_INVERSE_SQRT(A) h_InvSqrt64d(fabs((long double)A)+ FLT_EPSILON)
|
//#define MOSIS_INVERSE_SQRT(A) h_InvSqrt64d(fabs(A))
|
//#define MOSIS_INVERSE_SQRT(A) (1/sqrt(fabs((double)A)))
|
|
BOOL CMatchImpl::MatchImpl(CUShortBuff &smallBuffer, CUShortBuff &bigBuffer, stSISMatchResult &matchResult, double acceptance, BOOL bUseInterpolate)
|
{
|
int64 smallSqsum;
|
|
if(! Check_Buffer(smallBuffer, bigBuffer))
|
return FALSE;
|
|
|
int x, y;
|
|
int64 sqsval;
|
m_sqSumBuffer.SetSize(1, bigBuffer.GetHeight());
|
m_resultBuffer.SetSize(bigBuffer.GetWidth()- smallBuffer.GetWidth()+ 1, bigBuffer.GetHeight()- smallBuffer.GetHeight()+ 1);
|
m_resNumBuffer.SetSize(1, m_resultBuffer.GetHeight());
|
m_resDenomBuffer.SetSize(1, m_resultBuffer.GetHeight()*2);
|
|
|
m_tempBuffer16.SetSize(smallBuffer.GetWidth(), bigBuffer.GetHeight()+ bigBuffer.GetWidth()/smallBuffer.GetWidth()+ 10);
|
if(!m_tempBuffer16.CopyFrom(bigBuffer, CRect(0, 0, smallBuffer.GetWidth(), bigBuffer.GetHeight())))
|
return FALSE;
|
|
int64 *sqsumBuf= m_sqSumBuffer.GetDataAddress(0, 0);
|
smallSqsum = CSISMath::Get_CCU16_A(smallBuffer.GetDataAddress(), smallBuffer.GetDataSize());
|
double smallCoeff = (double)smallSqsum;
|
smallCoeff = h_InvSqrt64d( fabs( smallCoeff ) + FLT_EPSILON );
|
|
for(y= 0; y< bigBuffer.GetHeight(); y++)
|
{
|
sqsval = CSISMath::Get_CCU16_UA(bigBuffer.GetDataAddress(0, y), smallBuffer.GetWidth() );
|
m_sqSumBuffer.SetData(0, y, sqsval);
|
}
|
|
int64 *resNum= m_resNumBuffer.GetDataAddress(0, 0);
|
int64 *resDenom= m_resDenomBuffer.GetDataAddress(0, 0);
|
for(x= 0; x< m_resultBuffer.GetWidth(); x++)
|
{
|
sqsval= 0;
|
ushort *imgPtr = m_tempBuffer16.GetDataAddress(x, 0);
|
|
if(x > 0)
|
{
|
const ushort *src = bigBuffer.GetDataAddress(x + smallBuffer.GetWidth() - 1, 0);
|
ushort *dst = imgPtr - 1;
|
int out_val = dst[0];
|
|
dst += smallBuffer.GetWidth();
|
|
for( y = 0; y < bigBuffer.GetHeight(); y++, src += bigBuffer.GetWidth(), dst += smallBuffer.GetWidth() )
|
{
|
int in_val = src[0];
|
|
sqsumBuf[y] += (in_val - out_val) * (in_val + out_val);
|
out_val = dst[0];
|
dst[0] = (ushort) in_val;
|
}
|
}
|
for( y = 0; y < smallBuffer.GetHeight(); y++ )
|
{
|
sqsval += sqsumBuf[y];
|
}
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++, imgPtr += smallBuffer.GetWidth() )
|
{
|
int64 res = CSISMath::Get_CCU16_AUA(smallBuffer.GetDataAddress(), imgPtr, smallBuffer.GetDataSize() );
|
|
if( y > 0 )
|
{
|
sqsval -= sqsumBuf[y - 1];
|
sqsval += sqsumBuf[y + smallBuffer.GetHeight() - 1];
|
}
|
resNum[y] = res;
|
resDenom[y] = sqsval;
|
}
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++ )
|
{
|
double res = ((double) resNum[y]) * smallCoeff *
|
h_InvSqrt64d( fabs( (double) resDenom[y] ) + FLT_EPSILON );
|
|
//pResult[x + y * resultStep] = (float) res;
|
m_resultBuffer.SetData(x, y, (float)res);
|
}
|
}
|
|
return m_resultBuffer.GetBestMatchResult(matchResult, acceptance, bUseInterpolate);
|
}
|
|
BOOL CMatchImpl::MatchImpl(CSISBuffer &smallBuffer, CSISBuffer &bigBuffer, stSISMatchResult &matchResult, double acceptance, BOOL bUseInterpolate)
|
{
|
|
int64 smallSqsum;
|
m_sqSumBuffer.SetSize(1, bigBuffer.GetHeight());
|
m_resultBuffer.SetSize(bigBuffer.GetWidth()- smallBuffer.GetWidth()+ 1, bigBuffer.GetHeight()- smallBuffer.GetHeight()+ 1);
|
m_resNumBuffer.SetSize(1, m_resultBuffer.GetHeight());
|
m_resDenomBuffer.SetSize(1, m_resultBuffer.GetHeight());
|
|
int width, height;
|
int x, y;
|
int dx, dy;
|
int64 sqsum, sqssum2;
|
width= smallBuffer.GetWidth();
|
height= smallBuffer.GetHeight();
|
dx= bigBuffer.GetWidth()- width;
|
dy= bigBuffer.GetHeight()- height;
|
|
// CByteBuff aBuffer, bBuffer;
|
// aBuffer.SetBuffer(buffer1, x1, y1, width, height);
|
// bBuffer.SetBuffer(buffer2, x2, y2, width, height);
|
// return Get_CCU8(aBuffer.GetDataAddress(0, 0), bBuffer.GetDataAddress(0, 0), width*height);
|
|
|
smallSqsum= CSISMath::Get_CCU8(smallBuffer, 0, 0, width, height, smallBuffer, 0, 0);
|
// smallSqsum= CrossCorr_8u_C1(smallBuffer, 0, 0, width, height, smallBuffer, 0, 0);
|
double smallCoeff;
|
smallCoeff= MOSIS_INVERSE_SQRT(smallSqsum);
|
double res;
|
|
|
|
for(y= 0; y< dy; y++)
|
{
|
for(x= 0; x< dx; x++)
|
{
|
// sqsum= CrossCorr_8u_C1(smallBuffer, 0, 0, width, height, bigBuffer, x, y);
|
// sqssum2= CrossCorr_8u_C1(bigBuffer, x, y, width, height, bigBuffer, x, y);
|
sqsum= CSISMath::Get_CCU8(smallBuffer, 0, 0, width, height, bigBuffer, x, y);
|
sqssum2= CSISMath::Get_CCU8(bigBuffer, x, y, width, height, bigBuffer, x, y);
|
res= sqsum*smallCoeff*MOSIS_INVERSE_SQRT(sqssum2);
|
m_resultBuffer.SetData(x, y, (float)res);
|
}
|
}
|
|
|
return m_resultBuffer.GetBestMatchResult(matchResult, acceptance, bUseInterpolate);
|
}
|
|
|
#define DBL_EPSILON 2.2204460492503131e-016 /* smallest such that 1.0+DBL_EPSILON != 1.0 */
|
|
|
BOOL CMatchImpl::MatchImpl(CByteBuff &smallBuffer, CByteBuff &bigBuffer, stSISMatchResult &matchResult, double acceptance, BOOL bUseInterpolate)
|
{
|
int64 smallSqsum;
|
|
int x, y;
|
if(! Check_Buffer(smallBuffer, bigBuffer))
|
return FALSE;
|
|
int64 sqsval;
|
m_resultBuffer.SetSize(bigBuffer.GetWidth()- smallBuffer.GetWidth()+ 1, bigBuffer.GetHeight()- smallBuffer.GetHeight()+ 1);
|
m_sqSumBuffer.SetSize(1, bigBuffer.GetHeight());
|
m_resNumBuffer.SetSize(1, m_resultBuffer.GetHeight());
|
m_resDenomBuffer.SetSize(1, m_resultBuffer.GetHeight()*2);
|
|
m_tempBuffer8.SetSize(smallBuffer.GetWidth(), bigBuffer.GetHeight()+ bigBuffer.GetWidth()/smallBuffer.GetWidth()+ 10);
|
if(!m_tempBuffer8.CopyFrom(bigBuffer, CRect(0, 0, smallBuffer.GetWidth(), bigBuffer.GetHeight())))
|
return FALSE;
|
|
int64 *sqsumBuf= m_sqSumBuffer.GetDataAddress(0, 0);
|
#if defined(MOSIS_ASM)
|
smallSqsum = _asm_Get_CCU8(smallBuffer.GetDataAddress(), NULL, smallBuffer.GetDataSize(), 0);
|
#else
|
smallSqsum = CSISMath::Get_CCU8_A(smallBuffer.GetDataAddress(), smallBuffer.GetDataSize());
|
#endif
|
double smallCoeff = (double)smallSqsum;
|
smallCoeff = MOSIS_INVERSE_SQRT(smallCoeff);//h_InvSqrt64d( fabs( smallCoeff ) + FLT_EPSILON );
|
|
for(y= 0; y< bigBuffer.GetHeight(); y++)
|
{
|
#if defined(MOSIS_ASM)
|
sqsval = _asm_Get_CCU8(bigBuffer.GetDataAddress(0, y), NULL, smallBuffer.GetWidth(), 2);
|
#else
|
sqsval = CSISMath::Get_CCU8_UA(bigBuffer.GetDataAddress(0, y), smallBuffer.GetWidth() );
|
#endif
|
m_sqSumBuffer.SetData(0, y, sqsval);
|
}
|
|
int64 *resNum= m_resNumBuffer.GetDataAddress(0, 0);
|
int64 *resDenom= m_resDenomBuffer.GetDataAddress(0, 0);
|
|
|
for(x= 0; x< m_resultBuffer.GetWidth(); x++)
|
{
|
sqsval= 0;
|
uchar *imgPtr = m_tempBuffer8.GetDataAddress(x, 0);
|
|
if(x > 0)
|
{
|
const uchar *src = bigBuffer.GetDataAddress(x + smallBuffer.GetWidth() - 1, 0);
|
uchar *dst = imgPtr - 1;
|
int out_val = dst[0];
|
|
dst += smallBuffer.GetWidth();
|
|
for( y = 0; y < bigBuffer.GetHeight(); y++, src += bigBuffer.GetWidth(), dst += smallBuffer.GetWidth() )
|
{
|
int in_val = src[0];
|
|
sqsumBuf[y] += (in_val - out_val) * (in_val + out_val);
|
out_val = dst[0];
|
dst[0] = (uchar) in_val;
|
}
|
}
|
for( y = 0; y < smallBuffer.GetHeight(); y++ )
|
{
|
sqsval += sqsumBuf[y];
|
}
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++, imgPtr += smallBuffer.GetWidth() )
|
{
|
int64 res= 0;
|
#if defined(MOSIS_ASM)
|
res = _asm_Get_CCU8(smallBuffer.GetDataAddress(), imgPtr, smallBuffer.GetDataSize(), 1);
|
#else
|
res = CSISMath::Get_CCU8_AUA(smallBuffer.GetDataAddress(), imgPtr, smallBuffer.GetDataSize() );
|
#endif
|
|
if( y > 0 )
|
{
|
sqsval -= sqsumBuf[y - 1];
|
sqsval += sqsumBuf[y + smallBuffer.GetHeight() - 1];
|
}
|
resNum[y] = res;
|
resDenom[y] = sqsval;
|
}
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++ )
|
{
|
double res = ((double) resNum[y]) * smallCoeff * MOSIS_INVERSE_SQRT(resDenom[y]);
|
//h_InvSqrt64d( fabs( (double) resDenom[y] ) + FLT_EPSILON );
|
|
//pResult[x + y * resultStep] = (float) res;
|
m_resultBuffer.SetData(x, y, (float)res);
|
}
|
}
|
|
|
return m_resultBuffer.GetBestMatchResult(matchResult, acceptance, bUseInterpolate);
|
}
|
|
|
|
|
BOOL CMatchImpl::MatchImpl_Coef(CByteBuff &smallBuffer, CByteBuff &bigBuffer, stSISMatchResult &matchResult, double acceptance, BOOL bUseInterpolate)
|
{
|
int64 smallSqsum;
|
int x, y;
|
if(! Check_Buffer(smallBuffer, bigBuffer))
|
return FALSE;
|
|
|
// int64 sqsval;
|
int64 sum= 0, sqsum= 0;
|
int64 templSum;
|
double winCoeff = 1. / (smallBuffer.GetDataSize() + DBL_EPSILON);
|
m_sumBuffer.SetSize(1, bigBuffer.GetHeight());
|
m_sqSumBuffer.SetSize(1, bigBuffer.GetHeight());
|
m_resultBuffer.SetSize(bigBuffer.GetWidth()- smallBuffer.GetWidth()+ 1, bigBuffer.GetHeight()- smallBuffer.GetHeight()+ 1);
|
m_resNumBuffer.SetSize(1, m_resultBuffer.GetHeight());
|
m_resDenomBuffer.SetSize(1, (m_resultBuffer.GetHeight())*2);
|
|
m_tempBuffer8.SetSize(smallBuffer.GetWidth(), bigBuffer.GetHeight()+ bigBuffer.GetWidth()/smallBuffer.GetWidth()+ 10);
|
if(!m_tempBuffer8.CopyFrom(bigBuffer, CRect(0, 0, smallBuffer.GetWidth(), bigBuffer.GetHeight())))
|
return FALSE;
|
|
int64 *sumBuf= m_sumBuffer.GetDataAddress(0, 0);
|
int64 *sqsumBuf= m_sqSumBuffer.GetDataAddress(0, 0);
|
#if defined(MOSIS_ASM)
|
smallSqsum = _asm_Get_CCU8(smallBuffer.GetDataAddress(), NULL, smallBuffer.GetDataSize(), 0);
|
#else
|
smallSqsum = CSISMath::Get_CCU8_A(smallBuffer.GetDataAddress(), smallBuffer.GetDataSize());
|
#endif
|
templSum = CSISMath::SumPixels_8u(smallBuffer.GetDataAddress(), smallBuffer.GetDataSize());
|
double smallCoeff = (double)smallSqsum;
|
smallCoeff = (double) smallSqsum - ((double) templSum) * templSum * winCoeff;
|
smallCoeff = MOSIS_INVERSE_SQRT(smallCoeff);//h_InvSqrt64d( fabs( smallCoeff ) + FLT_EPSILON );
|
|
for(y= 0; y< bigBuffer.GetHeight(); y++)
|
{
|
sumBuf[y] = CSISMath::SumPixels_8u( bigBuffer.GetDataAddress(0, y), smallBuffer.GetWidth() );
|
#if defined(MOSIS_ASM)
|
sqsumBuf[y] = _asm_Get_CCU8(bigBuffer.GetDataAddress(), NULL, smallBuffer.GetWidth(), 2);
|
#else
|
sqsumBuf[y] = CSISMath::Get_CCU8_UA(bigBuffer.GetDataAddress(0, y), smallBuffer.GetWidth() );
|
#endif
|
}
|
|
int64 *resNum= m_resNumBuffer.GetDataAddress(0, 0);
|
int64 *resDenom= m_resDenomBuffer.GetDataAddress(0, 0);
|
for(x= 0; x< m_resultBuffer.GetWidth(); x++)
|
{
|
sum= 0;
|
sqsum= 0;
|
uchar *imgPtr = m_tempBuffer8.GetDataAddress(x, 0);
|
|
if(x > 0)
|
{
|
const uchar *src = bigBuffer.GetDataAddress(x + smallBuffer.GetWidth() - 1, 0);
|
uchar *dst = imgPtr - 1;
|
int out_val = dst[0];
|
|
dst += smallBuffer.GetWidth();
|
|
for( y = 0; y < bigBuffer.GetHeight(); y++, src += bigBuffer.GetWidth(), dst += smallBuffer.GetWidth() )
|
{
|
int in_val = src[0];
|
|
sumBuf[y] += in_val - out_val;
|
sqsumBuf[y] += (in_val - out_val) * (in_val + out_val);
|
out_val = dst[0];
|
dst[0] = (uchar) in_val;
|
}
|
}
|
for( y = 0; y < smallBuffer.GetHeight(); y++ )
|
{
|
sum += sumBuf[y];
|
sqsum += sqsumBuf[y];
|
}
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++, imgPtr += smallBuffer.GetWidth() )
|
{
|
int64 res;
|
#if defined(MOSIS_ASM)
|
res = _asm_Get_CCU8(smallBuffer.GetDataAddress(), imgPtr, smallBuffer.GetDataSize(), 1);
|
#else
|
res = CSISMath::Get_CCU8_AUA(smallBuffer.GetDataAddress(), imgPtr, smallBuffer.GetDataSize() );
|
#endif
|
|
if( y > 0 )
|
{
|
sum -= sumBuf[y - 1];
|
sum += sumBuf[y + smallBuffer.GetHeight() - 1];
|
sqsum -= sqsumBuf[y - 1];
|
sqsum += sqsumBuf[y + smallBuffer.GetHeight() - 1];
|
}
|
resNum[y] = res;
|
resDenom[y] = sum;
|
resDenom[y + m_resultBuffer.GetHeight()] = sqsum;
|
}
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++ )
|
{
|
double Dsum = ((double) resDenom[y]);
|
double wsum = winCoeff * Dsum;
|
double res = ((double) resNum[y]) - wsum * templSum;
|
double nrm_s = ((double) resDenom[y + m_resultBuffer.GetHeight()]) - wsum * Dsum;
|
res *= smallCoeff * MOSIS_INVERSE_SQRT(nrm_s);//icvInvSqrt64d( fabs( nrm_s ) + FLT_EPSILON );
|
m_resultBuffer.SetData(x, y, (float)res);
|
}
|
}
|
|
BOOL bRet= m_resultBuffer.GetBestMatchResult(matchResult, acceptance, bUseInterpolate);
|
return bRet;
|
}
|
|
|
BOOL CMatchImpl::MatchImpl_Coef(CUShortBuff &smallBuffer, CUShortBuff &bigBuffer, stSISMatchResult &matchResult, double acceptance, BOOL bUseInterpolate)
|
{
|
int64 smallSqsum;
|
int x, y;
|
if(! Check_Buffer(smallBuffer, bigBuffer))
|
return FALSE;
|
|
// int64 sqsval;
|
double winCoeff = 1. / (smallBuffer.GetDataSize() + DBL_EPSILON);
|
int64 sum= 0, sqsum= 0;
|
int64 templSum;
|
m_sumBuffer.SetSize(1, bigBuffer.GetHeight());
|
m_sqSumBuffer.SetSize(1, bigBuffer.GetHeight());
|
m_resultBuffer.SetSize(bigBuffer.GetWidth()- smallBuffer.GetWidth()+ 1, bigBuffer.GetHeight()- smallBuffer.GetHeight()+ 1);
|
m_resNumBuffer.SetSize(1, m_resultBuffer.GetHeight());
|
m_resDenomBuffer.SetSize(1, (m_resultBuffer.GetHeight())*2);
|
|
m_tempBuffer16.SetSize(smallBuffer.GetWidth(), bigBuffer.GetHeight()+ bigBuffer.GetWidth()/smallBuffer.GetWidth()+ 10);
|
if(!m_tempBuffer16.CopyFrom(bigBuffer, CRect(0, 0, smallBuffer.GetWidth(), bigBuffer.GetHeight())))
|
return FALSE;
|
|
int64 *sumBuf= m_sumBuffer.GetDataAddress(0, 0);
|
int64 *sqsumBuf= m_sqSumBuffer.GetDataAddress(0, 0);
|
smallSqsum = CSISMath::Get_CCU16_A(smallBuffer.GetDataAddress(), smallBuffer.GetDataSize());
|
templSum = CSISMath::SumPixels_16u(smallBuffer.GetDataAddress(), smallBuffer.GetDataSize());
|
double smallCoeff = (double)smallSqsum;
|
smallCoeff = (double) smallSqsum - ((double) templSum) * templSum * winCoeff;
|
smallCoeff = MOSIS_INVERSE_SQRT(smallCoeff);//h_InvSqrt64d( fabs( smallCoeff ) + FLT_EPSILON );
|
|
for(y= 0; y< bigBuffer.GetHeight(); y++)
|
{
|
sumBuf[y] = CSISMath::SumPixels_16u( bigBuffer.GetDataAddress(0, y), smallBuffer.GetWidth() );
|
sqsumBuf[y] = CSISMath::Get_CCU16_UA(bigBuffer.GetDataAddress(0, y), smallBuffer.GetWidth() );
|
}
|
|
int64 *resNum= m_resNumBuffer.GetDataAddress(0, 0);
|
int64 *resDenom= m_resDenomBuffer.GetDataAddress(0, 0);
|
for(x= 0; x< m_resultBuffer.GetWidth(); x++)
|
{
|
sum= 0;
|
sqsum= 0;
|
ushort *imgPtr = m_tempBuffer16.GetDataAddress(x, 0);
|
|
if(x > 0)
|
{
|
const ushort *src = bigBuffer.GetDataAddress(x + smallBuffer.GetWidth() - 1, 0);
|
ushort *dst = imgPtr - 1;
|
int out_val = dst[0];
|
|
dst += smallBuffer.GetWidth();
|
|
for( y = 0; y < bigBuffer.GetHeight(); y++, src += bigBuffer.GetWidth(), dst += smallBuffer.GetWidth() )
|
{
|
int in_val = src[0];
|
|
sumBuf[y] += in_val - out_val;
|
sqsumBuf[y] += (in_val - out_val) * (in_val + out_val);
|
out_val = dst[0];
|
dst[0] = (ushort) in_val;
|
}
|
}
|
for( y = 0; y < smallBuffer.GetHeight(); y++ )
|
{
|
sum += sumBuf[y];
|
sqsum += sqsumBuf[y];
|
}
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++, imgPtr += smallBuffer.GetWidth() )
|
{
|
int64 res = CSISMath::Get_CCU16_AUA(smallBuffer.GetDataAddress(), imgPtr, smallBuffer.GetDataSize() );
|
|
if( y > 0 )
|
{
|
sum -= sumBuf[y - 1];
|
sum += sumBuf[y + smallBuffer.GetHeight() - 1];
|
sqsum -= sqsumBuf[y - 1];
|
sqsum += sqsumBuf[y + smallBuffer.GetHeight() - 1];
|
}
|
resNum[y] = res;
|
resDenom[y] = sum;
|
resDenom[y + m_resultBuffer.GetHeight()] = sqsum;
|
}
|
|
|
for( y = 0; y < m_resultBuffer.GetHeight(); y++ )
|
{
|
double dsum = ((double) resDenom[y]);
|
double wsum = winCoeff * dsum;
|
double res = ((double) resNum[y]) - wsum * templSum;
|
double nrm_s = ((double) resDenom[y + m_resultBuffer.GetHeight()]) - wsum * dsum;
|
res *= smallCoeff * MOSIS_INVERSE_SQRT(nrm_s);//icvInvSqrt64d( fabs( nrm_s ) + FLT_EPSILON );
|
m_resultBuffer.SetData(x, y, (float)res);
|
}
|
|
}
|
|
return m_resultBuffer.GetBestMatchResult(matchResult, acceptance, bUseInterpolate);
|
}
|
|
|
void CResultBufferFloat::ReleaseSpace()
|
{
|
if(m_pData)
|
delete[] m_pData;
|
m_pData= NULL;
|
m_Width= 0;
|
m_Height= 0;
|
m_DataSpace= 0;
|
}
|
void CResultBufferInt64::ReleaseSpace()
|
{
|
if(m_pData)
|
delete[] m_pData;
|
m_pData= NULL;
|
m_Width= 0;
|
m_Height= 0;
|
m_DataSpace= 0;
|
}
|
BOOL CResultBufferFloat::SetSize(int width, int height)
|
{
|
int space= width*height;
|
if(space < 1)
|
return FALSE;
|
|
if(m_DataSpace < space)
|
{
|
ReleaseSpace();
|
}
|
|
if(m_pData == NULL)
|
{
|
m_pData= new float[space];
|
m_DataSpace= space;
|
}
|
|
m_Width= width;
|
m_Height= height;
|
|
return IsValidBuffer();
|
}
|
BOOL CResultBufferInt64::SetSize(int width, int height)
|
{
|
int space= width*height;
|
if(space < 1)
|
return FALSE;
|
if(m_DataSpace < space)
|
{
|
ReleaseSpace();
|
}
|
|
if(m_pData == NULL)
|
{
|
m_pData= new int64[space];
|
m_DataSpace= space;
|
}
|
|
m_Width= width;
|
m_Height= height;
|
|
return IsValidBuffer();
|
}
|
CResultBufferFloat::~CResultBufferFloat()
|
{
|
if(m_pData)
|
delete[] m_pData;
|
}
|
CResultBufferInt64::~CResultBufferInt64()
|
{
|
if(m_pData)
|
delete[] m_pData;
|
}
|
|
BOOL CResultBufferFloat::GetBestMatchResult(stSISMatchResult &result, double acceptance, BOOL bUseInterpolate)
|
{
|
int width, height;
|
float best= GetData(0, 0);
|
int i, j;
|
int xx= 0, yy= 0;
|
double xxx, yyy;
|
|
width= GetWidth();
|
height= GetHeight();
|
|
for(i= 0; i< width; i++)
|
{
|
for(j= 0; j< height; j++)
|
{
|
if(best < GetData(i, j))//*pBest)
|
{
|
best= GetData(i, j);
|
xx= i;
|
yy= j;
|
}
|
}
|
}
|
result.m_X= xx;
|
result.m_Y= yy;
|
result.m_Score= best;
|
if(!bUseInterpolate)
|
return result.m_Score >= acceptance;
|
|
|
float best1, best3;
|
// float points[4];
|
// float data[4];
|
if(xx == 0)
|
{
|
xxx= xx;
|
// points[0]= 0;
|
// points[1]= 1;
|
// points[2]= 2;
|
// data[0]= GetData(points[0], yy);
|
// data[1]= GetData(points[1], yy);
|
// data[2]= GetData(points[2], yy);
|
// xxx= GetRegressionCoeffInPoly(points, data, 3);
|
}
|
else if(xx == width-1)
|
{
|
xxx= xx;
|
// points[0]= width- 4;
|
// points[1]= width- 3;
|
// points[2]= width- 2;
|
// points[3]= width- 1;
|
// data[0]= GetData(points[0], yy);
|
// data[1]= GetData(points[1], yy);
|
// data[2]= GetData(points[2], yy);
|
// data[3]= GetData(points[3], yy);
|
// xxx= GetRegressionCoeffInPoly(points, data, 4);
|
}
|
else
|
{
|
best1= GetData(xx- 1, yy);//*(pBest- 1);
|
best3= GetData(xx+ 1, yy);//*(pBest+ 1);
|
if(best == best3)
|
{
|
xxx= xx+ 0.5;
|
}
|
else if(best == best1)
|
{
|
xxx= xx- 0.5;
|
}
|
else if(best1 < best3)
|
{
|
xxx= xx+ 0.5- 0.5*(best- best3)/(best- best1);
|
}
|
else// if(*(pBest-1) > best3)
|
{
|
xxx= xx- 0.5+ 0.5/(best- best3)*(best- best1);
|
}
|
|
// points[0]= xx-1;
|
// points[1]= xx;
|
// points[2]= xx+1;
|
// data[0]= GetData(points[0], yy);
|
// data[1]= GetData(points[1], yy);
|
// data[2]= GetData(points[2], yy);
|
// xxx= GetRegressionCoeffInPoly(points, data, 3);
|
}
|
|
if(yy == 0)
|
{
|
// points[0]= 0;
|
// points[1]= 1;
|
// points[2]= 2;
|
// data[0]= GetData(xx, points[0]);
|
// data[1]= GetData(xx, points[1]);
|
// data[2]= GetData(xx, points[2]);
|
// yyy= GetRegressionCoeffInPoly(points, data, 3);
|
yyy= yy;
|
}
|
else if(yy == height-1)
|
{
|
// points[0]= height- 3;
|
// points[1]= height- 2;
|
// points[2]= height- 1;
|
// data[0]= GetData(xx, points[0]);
|
// data[1]= GetData(xx, points[1]);
|
// data[2]= GetData(xx, points[2]);
|
// yyy= GetRegressionCoeffInPoly(points, data, 3);
|
yyy= yy;
|
}
|
else
|
{
|
|
best1= GetData(xx, yy- 1);//*(pBest- width);
|
best3= GetData(xx, yy+ 1);//*(pBest+ height);
|
if(best == best3)
|
{
|
yyy= yy+ 0.5;
|
}
|
else if(best == best1)
|
{
|
yyy= yy- 0.5;
|
}
|
else if(best1 < best3)
|
{
|
yyy= yy+ 0.5- 0.5*(best- best3)/(best- best1);
|
}
|
else// if(best1 > best3)
|
{
|
yyy= yy- 0.5+ 0.5/(best- best3)*(best- best1);
|
}
|
|
// points[0]= yy-1;
|
// points[1]= yy;
|
// points[2]= yy+1;
|
// data[0]= GetData(xx, points[0]);
|
// data[1]= GetData(xx, points[1]);
|
// data[2]= GetData(xx, points[2]);
|
// yyy= GetRegressionCoeffInPoly(points, data, 3);
|
}
|
result.m_X= xxx;
|
result.m_Y= yyy;
|
result.m_Score= best;
|
return result.m_Score >= acceptance;
|
}
|
|
BOOL CResultBufferInt64::GetBestMatchResult(stSISMatchResult &result, double acceptance, BOOL bUseInterpolate)
|
{
|
return FALSE;
|
/* int width, height;
|
int64 best= 0;
|
int i, j;
|
int xx, yy;
|
double xxx, yyy;
|
|
width= GetWidth();
|
height= GetHeight();
|
|
for(i= 0; i< width; i++)
|
{
|
for(j= 0; j< height; j++)
|
{
|
if(best < GetData(i, j))//*pBest)
|
{
|
best= GetData(i, j);
|
xx= i;
|
yy= j;
|
}
|
}
|
}
|
result.m_X= xx;
|
result.m_Y= yy;
|
result.m_Score= best;
|
if(!bUseInterpolate)
|
return result.m_Score >= acceptance;
|
|
|
int64 best1, best3;
|
// float points[3];
|
// float data[3];
|
if(xx == 0)
|
{
|
// points[0]= 0;
|
// points[1]= 1;
|
// points[2]= 2;
|
// data[0]= GetData(points[0], yy);
|
// data[1]= GetData(points[1], yy);
|
// data[2]= GetData(points[2], yy);
|
// xxx= GetRegressionCoeffInPoly(points, data, 3);
|
xxx= xx;
|
}
|
else if(xx == width-1)
|
{
|
// points[0]= width- 3;
|
// points[1]= width- 2;
|
// points[2]= width- 1;
|
// data[0]= GetData(points[0], yy);
|
// data[1]= GetData(points[1], yy);
|
// data[2]= GetData(points[2], yy);
|
// xxx= GetRegressionCoeffInPoly(points, data, 3);
|
xxx= xx;
|
}
|
else
|
{
|
best1= GetData(xx- 1, yy);//*(pBest- 1);
|
best3= GetData(xx+ 1, yy);//*(pBest+ 1);
|
if(best == best3)
|
{
|
xxx= xx+ 0.5;
|
}
|
else if(best == best1)
|
{
|
xxx= xx- 0.5;
|
}
|
else if(best1 < best3)
|
{
|
xxx= xx+ 0.5- 0.5*(best- best3)/(best- best1);
|
}
|
else// if(*(pBest-1) > best3)
|
{
|
xxx= xx- 0.5+ 0.5/(best- best3)*(best- best1);
|
}
|
|
// points[0]= xx-1;
|
// points[1]= xx;
|
// points[2]= xx+1;
|
// data[0]= GetData(points[0], yy);
|
// data[1]= GetData(points[1], yy);
|
// data[2]= GetData(points[2], yy);
|
// xxx= GetRegressionCoeffInPoly(points, data, 3);
|
}
|
|
if(yy == 0)
|
{
|
// points[0]= 0;
|
// points[1]= 1;
|
// points[2]= 2;
|
// data[0]= GetData(xx, points[0]);
|
// data[1]= GetData(xx, points[1]);
|
// data[2]= GetData(xx, points[2]);
|
// yyy= GetRegressionCoeffInPoly(points, data, 3);
|
yyy= yy;
|
}
|
else if(yy == height-1)
|
{
|
// points[0]= height- 3;
|
// points[1]= height- 2;
|
// points[2]= height- 1;
|
// data[0]= GetData(xx, points[0]);
|
// data[1]= GetData(xx, points[1]);
|
// data[2]= GetData(xx, points[2]);
|
// yyy= GetRegressionCoeffInPoly(points, data, 3);
|
yyy= yy;
|
}
|
else
|
{
|
|
best1= GetData(xx, yy- 1);//*(pBest- width);
|
best3= GetData(xx, yy+ 1);//*(pBest+ height);
|
if(best == best3)
|
{
|
yyy= yy+ 0.5;
|
}
|
else if(best == best1)
|
{
|
yyy= yy- 0.5;
|
}
|
else if(best1 < best3)
|
{
|
yyy= yy+ 0.5- 0.5*(best- best3)/(best- best1);
|
}
|
else// if(best1 > best3)
|
{
|
yyy= yy- 0.5+ 0.5/(best- best3)*(best- best1);
|
}
|
|
// points[0]= yy-1;
|
// points[1]= yy;
|
// points[2]= yy+1;
|
// data[0]= GetData(xx, points[0]);
|
// data[1]= GetData(xx, points[1]);
|
// data[2]= GetData(xx, points[2]);
|
// yyy= GetRegressionCoeffInPoly(points, data, 3);
|
}
|
result.m_X= xxx;
|
result.m_Y= yyy;
|
result.m_Score= best;
|
return result.m_Score >= acceptance;
|
*/
|
}
|
|
#include "LeastSquare.h"
|
float GetRegressionCoeffInPoly(float* points , float *scores, int nSize)
|
{
|
float aCoeff[3];
|
LeastSquareFns fns;
|
|
fns.GetRegressionCoeffInPoly(points, scores, aCoeff, 2, nSize);
|
|
float xx= -aCoeff[1]/(2*aCoeff[2]);
|
return xx;
|
|
}
|
