📄 matchpeak.cpp
字号:
// MatchPeak.cpp: implementation of the CMatchPeak class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
//#include "Radar.h"
#include "MatchPeak.h"
#include "Comlib.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
#define BUFFERSIZE 100
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
static CString Memo[8] ={
"C",
"L",
"R",
"Tgt",
"Add",
"309",
"313",
"315"
};
CMatchPeak::CMatchPeak(float ** p ,int row,int col,int Tsz,float sgm)
{
cor = p;
r = row;
c = col;
TSize = Tsz; // 峰尺度大小，取0表示极大值为正确匹配点
// 取-1表示极小值为正确匹配点
sigma = sgm;
bProced = FALSE;
}
CMatchPeak::CMatchPeak()
{
bProced = FALSE;
}
CMatchPeak::~CMatchPeak()
{
}
void CMatchPeak::Open(float ** p ,int row,int col,int Tsz,float sgm)
{
cor = p;
r = row;
c = col;
TSize = Tsz; // 峰尺度大小，取0表示极大值为正确匹配点
// 取-1表示极小值为正确匹配点
sigma = sgm;
bProced = FALSE;
}
void CMatchPeak::PeakDelmem(FPOINT* peak,int pos,int* len)
{
int i;
for (i=pos;i<*len-1;i++) {
peak[i].x = peak[i+1].x;
peak[i].y = peak[i+1].y;
}
(*len)--;
}
inline void CMatchPeak::PeakAddmem(FPOINT* peak,int pos,int* len,int row,int col)
{
peak[pos].x = (float)col;
peak[pos].y = (float)row;
peak[pos].time = 1;
(*len)+=1;
}
inline float CMatchPeak::PeakDistance(FPOINT p1,FPOINT p2)
{
return (p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y);
}
void CMatchPeak::Processing()
{
float nei,total;
int neicou,count;
FPOINT posspeak[BUFFERSIZE];
int len , lenbak , num ;
float totalx,totaly;
len = lenbak = num = 0;
bProced = TRUE;
/* 找相关面中的最大最小值点 */
NPOT pMax = CComlib::GetMaxisum( cor , r , c);// , 1); // 间隔1+1粗采样搜索
NPOT pMin = CComlib::GetMinisum( cor , r , c);// , 1); // 间隔1+1粗采样搜索
float fMax = pMax.cor;
float fMin = pMin.cor;
float fDist = fMax-fMin;
m_MaxPot.x = pMax.x;
m_MaxPot.y = pMax.y;
m_MaxPot.cor = fMax;
m_MinPot.x = pMin.x;
m_MinPot.y = pMin.y;
m_MinPot.cor = fMin;
// 匹配点选为极大值点
if(TSize == 0)
{
m_CorPot = m_MaxPot;
return ;
}
else if(TSize == -1) // 匹配点选为极小值点
{
m_CorPot = m_MinPot;
return;
}
NPOT ret = {0,0,0};
float frow,fcol;
/* invert lead the max to be the lest point */
if(fMax < -fMin)
{
for (int i=0;i<r;i++)
for (int j=0;j<c;j++)
cor[i][j] = fMax - cor[i][j];
ret = pMin;
}
else
{
for (int i=0;i<r;i++)
for (int j=0;j<c;j++)
cor[i][j] -= fMin;
ret = pMax;
}
frow = (float) ret.y;
fcol = (float) ret.x ;
/* 从模板尺寸为(TSize-1)/2开始搜索 */
for ( int HalfSize= (int) ((TSize-1)/2) ; HalfSize>=3 ; HalfSize--)
{
len = num = 0;
for (int i=0;i<r;i++)
for (int j=0;j<c;j++)
{
count = neicou =0;
nei = total = 0;
BOOL bLocalMaxFlag = TRUE;
for (int m=-HalfSize;m<=HalfSize;m++)
for (int n=-HalfSize;n<=HalfSize;n++)
{
int k = m+i;
int l = n+j;
if ( (k>=0) && (k<r) && (l>=0) && (l<c) )
{ ; }
else if ( ((k<0) && (l>=0) && (l<c)) ||
((k>=r) && (l>=0) &&(l<c)) )
{
k = i-m; l = j+n;
}
else if ( ((k>=0) && (k<r) && (l<0)) ||
((k>=0) && (k<r) && (l>=c)) )
{
k = m+i; l = j-n;
}
else
{
k = i-m; l = j-n;
}
if( k<0 || l<0 || k>=r || l>=c)
{
TRACE("k = %d, l= %d , HalfSize=%d\n", k, l , HalfSize);
TRACE("i = %d, j= %d , r=%d , c=%d\n", i, j , r,c);
}
if ( cor[i][j] < cor[k][l] ) /* 当前点必须为局部极大值 **/
{
m = 50;
bLocalMaxFlag = FALSE;
break ;
}
if ( (m*m+n*n) * 9 > HalfSize*HalfSize )
{
total += cor[k][l];
count++;
}
else
{
nei += cor[k][l];
neicou++;
}
}
if( ( cor[i][j] > (float) fDist*sigma ) && bLocalMaxFlag &&
( nei * count > PEAKPROPB * total * neicou ) )
{
ASSERT(len < BUFFERSIZE );
PeakAddmem(posspeak,num,&len,i,j);
num++;
}
}
/* 对预选出的峰进行合并，筛选 */
do
{
lenbak = len;
for (int m=0;m<len;m++)
{
count=1;
totalx = posspeak[m].x;
totaly = posspeak[m].y;
int n = m;
while(++n < len)
{
if (PeakDistance(posspeak[m],posspeak[n])<9)
{
totalx += posspeak[n].x*posspeak[n].time;
totaly += posspeak[n].y*posspeak[n].time;
count += posspeak[n].time;
PeakDelmem(posspeak,n,&len);
}
}
posspeak[m].x = totalx/count;
posspeak[m].y = totaly/count;
posspeak[m].time = count;
}
} while (lenbak!=len);
/* 多于一个预选峰时，减小模板尺寸继续搜索 */
if (len>1)
{
float max = cor[(int) posspeak[0].y ][ (int) posspeak[0].x ] ;
frow = posspeak[0].y ;
fcol = posspeak[0].x ;
for (i=0;i<len;i++)
{
/* 保存此级搜索出的满足条件的峰 */
if (cor[(int)(posspeak[i].y) ][ (int)(posspeak[i].x)] > max)
{
frow = posspeak[i].y ;
fcol = posspeak[i].x ;
max = cor[(int)(posspeak[i].y)][(int)(posspeak[i].x)];
}
}
}
else /* 否则作出判决 */
break;
}
/* 只有一个满足约束条件的峰，即为所求 */
if (len==1)
{
frow = posspeak[0].y ;
fcol = posspeak[0].x ;
}
/* 输出选择的峰 */
ret.x = (int) (fcol+0.5) ;
ret.y = (int) (frow+0.5) ;
m_CorPot.x = ret.x;
m_CorPot.y = ret.y;
if(fMax < -fMin)
m_CorPot.cor = fMax - cor[ret.y][ret.x];
else
m_CorPot.cor = cor[ret.y][ret.x] + fMin;
return;
}
NPOT CMatchPeak::GetPeakPot()
{
if( ! bProced )
Processing();
return m_CorPot;
}
NPOT CMatchPeak::GetMinPot()
{
if( ! bProced )
Processing();
return m_MinPot;
}
NPOT CMatchPeak::GetMaxPot()
{
if( ! bProced )
Processing();
return m_MaxPot;
}
BOOL CMatchPeak::SaveDBSOrgPeakPotToFile( CString txtfile , SPARMS parm , CString memo)
{
if( ! bProced )
Processing();
if( ! parm.bValid ) return TRUE;
FILE *fp;
if( (fp = fopen( txtfile, "a+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return FALSE;
}
FPOINT maxpt = parm.TranToOrg(m_MaxPot.x, m_MaxPot.y);
FPOINT minpt = parm.TranToOrg(m_MinPot.x, m_MinPot.y);
FPOINT findpt = parm.TranToOrg(m_CorPot.x, m_CorPot.y);
float x = parm.localx;
float y = parm.localy;
fprintf(fp,"peak: Max: (%6.2f,%6.2f) Min: (%6.2f,%6.2f) Find: (%6.2f,%6.2f) %s \n",
maxpt.x , maxpt.y , minpt.x , minpt.y , findpt.x , findpt.y , memo );
fprintf(fp,"Err: Max: (%6.2f,%6.2f) Min: (%6.2f,%6.2f) Find: (%6.2f,%6.2f) %s \n",
maxpt.x-x , maxpt.y-y , minpt.x-x , minpt.y-y , findpt.x-x , findpt.y-y , memo );
fclose(fp);
return TRUE;
}
BOOL CMatchPeak::SaveDBSMapPeakPotToFile( CString txtfile , SPARMS parm , CString memo)
{
if( ! bProced )
Processing();
FILE *fp;
if( (fp = fopen( txtfile, "a+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return FALSE;
}
int x = parm.findx;
int y = parm.findy;
fprintf(fp,"peak: Max: % .3f(%3d,%3d) Min: % .3f(%3d,%3d) Find: % .3f(%3d,%3d) %s \n",
(float)m_MaxPot.cor , m_MaxPot.x, m_MaxPot.y ,
(float)m_MinPot.cor , m_MinPot.x, m_MinPot.y ,
(float)m_CorPot.cor, m_CorPot.x, m_CorPot.y,
memo );
if(parm.bValid )
{
fprintf(fp,"Err: Max: (%3d,%3d) Min: (%3d,%3d) Find: (%3d,%3d) %s \n",
m_MaxPot.x - x, m_MaxPot.y - y ,
m_MinPot.x - x, m_MinPot.y - y ,
m_CorPot.x - x, m_CorPot.y - y,
memo );
}
fclose(fp);
return TRUE;
}
BOOL CMatchPeak::SavePeakPotToFile( COORDINATE local , CString txtfile , CString memo)
{
if( ! bProced )
Processing();
FILE *fp;
if( (fp = fopen( txtfile, "a+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return FALSE;
}
fprintf(fp,"NO=%02d local:(%3d,%3d) Max: % .3f (%3d,%3d) dif:(%3d,%3d) Min: % .3f (%3d,%3d) dif:(%3d,%3d) Find: % .3f (%3d,%3d) dif:(%3d,%3d) %s \n",
local.numb , local.x , local.y ,
(float)m_MaxPot.cor , m_MaxPot.x, m_MaxPot.y , m_MaxPot.x - local.x, m_MaxPot.y - local.y,
(float)m_MinPot.cor , m_MinPot.x, m_MinPot.y , m_MinPot.x - local.x, m_MinPot.y - local.y,
(float)m_CorPot.cor , m_CorPot.x, m_CorPot.y , m_CorPot.x - local.x, m_CorPot.y - local.y,
memo );
fclose(fp);
return TRUE;
}
void CMatchPeak::ClearFile( CString txtfile)
{
FILE *fp;
if( (fp = fopen( txtfile, "wt+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return ;
}
fclose(fp);
return ;
}
void CMatchPeak::SaveStringToFile( CString txtfile , CString memo)
{
FILE *fp;
if( (fp = fopen( txtfile, "a+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return ;
}
fprintf(fp,"%s \n", memo );
fclose(fp);
return ;
}
void CMatchPeak::SaveDBSResultToFile( CString txtfile, NPOT retc , NPOT retl, NPOT retr, COORDINATE local ,int ErrSize)
{
FILE *fp;
if( (fp = fopen( txtfile, "a+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return ;
}
#if ERROR_RADIUS
BOOL bC = (-local.x+retc.x)*(-local.x+retc.x) + (-local.y+retc.y)*(-local.y+retc.y) > ErrSize*ErrSize ;
BOOL bL = (-local.x+retl.x)*(-local.x+retl.x) + (-local.y+retl.y)*(-local.y+retl.y) > ErrSize*ErrSize ;
BOOL bR = (-local.x+retr.x)*(-local.x+retr.x) + (-local.y+retr.y)*(-local.y+retr.y) > ErrSize*ErrSize ;
#else
BOOL bC = fabs(-local.x+retc.x) > ErrSize || fabs(-local.y+retc.y) > ErrSize ;
BOOL bL = fabs(-local.x+retl.x) > ErrSize || fabs(-local.y+retl.y) > ErrSize ;
BOOL bR = fabs(-local.x+retr.x) > ErrSize || fabs(-local.y+retr.y) > ErrSize ;
#endif
fprintf(fp,"NO=%02d local: (%3d,%3d) ", local.numb, local.x, local.y );
fprintf(fp,"C:% .3f(%3d,%3d) dif:(%3d,%3d) %c ", retc.cor, retc.x , retc.y, -local.x + retc.x , -local.y + retc.y , (bC ? 'x':' ') );
fprintf(fp,"L:% .3f(%3d,%3d) dif:(%3d,%3d) %c ", retl.cor, retl.x , retl.y, -local.x + retl.x , -local.y + retl.y , (bL ? 'x':' ') );
fprintf(fp,"R:% .3f(%3d,%3d) dif:(%3d,%3d) %c ", retr.cor, retr.x , retr.y, -local.x + retr.x , -local.y + retr.y , (bR ? 'x':' ') );
fprintf(fp,"\n");
fclose(fp);
return ;
}
void CMatchPeak::SaveDBSResultToFile( CString txtfile, NPOT ret[9], int Numbers , COORDINATE local ,int ErrSize)
{
FILE *fp;
if( (fp = fopen( txtfile, "a+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return ;
}
BOOL flag[9];
int errx[9],erry[9];
for(int i=0;i<Numbers;i++)
{
errx[i] = -local.x+ret[i].x;
erry[i] = -local.y+ret[i].y;
#if ERROR_RADIUS
flag[i] = errx[i]*errx[i] + erry[i]*erry[i] > ErrSize*ErrSize ;
#else
flag[i] = fabs( errx[i] ) > ErrSize || fabs( erry[i] ) > ErrSize ;
#endif
}
errx[8] = -local.x+ret[8].x;
erry[8] = -local.y+ret[8].y;
#if ERROR_RADIUS
flag[8] = errx[8]*errx[8] + erry[8]*erry[8] > ErrSize*ErrSize ;
#else
flag[8] = fabs( errx[8] ) > ErrSize || fabs( erry[8] ) > ErrSize ;
#endif
fprintf(fp,"NO=%02d local: (%3d,%3d) ", local.numb, local.x, local.y );
fprintf(fp,"Fus:");
fprintf(fp,"% .3f(%3d,%3d) dif:(%3d,%3d) %c ", ret[8].cor, ret[8].x , ret[8].y, errx[8], erry[8] , ( flag[8] ? 'x':' ') );
for( i=0;i<Numbers;i++)
{
fprintf(fp,"%s:" , Memo[i]);
fprintf(fp,"% .3f(%3d,%3d) dif:(%3d,%3d) %c ", ret[i].cor, ret[i].x , ret[i].y, errx[i], erry[i] , ( flag[i] ? 'x':' ') );
}
fprintf(fp,"\n");
fclose(fp);
return ;
}
void CMatchPeak::SaveResultToFile( CString txtfile, NPOT ret , COORDINATE local ,int ErrSize)
{
FILE *fp;
if( (fp = fopen( txtfile, "a+" )) == NULL )
{
AfxMessageBox(txtfile + " can not be opened for writting!");
return ;
}
if(ErrSize == -1)
{
fprintf(fp,"NO=%04d %.4f %d %d\n", local.numb, ret.cor , ret.x , ret.y );
}
else
{
fprintf(fp,"NO=%02d local: (%3d,%3d) find: % .5f (%3d,%3d) diff: (%3d,%3d)",
local.numb, local.x, local.y,
ret.cor , ret.x , ret.y,
-local.x + ret.x , -local.y + ret.y );
#if ERROR_RADIUS
if( (-local.x+ret.x)*(-local.x+ret.x) + (-local.y+ret.y)*(-local.y+ret.y) > ErrSize*ErrSize )
fprintf(fp," x\n");
#else
if( abs(-local.x+ret.x) > ErrSize || abs(-local.y+ret.y) > ErrSize )
fprintf(fp," x\n");
#endif
else
fprintf(fp," \n");
}
fclose(fp);
return ;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -