www.pudn.com > LSMatching.rar > LSMatching.cpp
// LSMatching.cpp: implementation of the CLSMatching class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "LSMatching.h"
#include
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
#define MAXITERATION 15
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
BYTE CLSMatching::GetPixel(LPBYTE lpImg,int ImgHei,int ImgWid,int x,int y)
{
return *(lpImg+y*ImgWid+x);
}
void CLSMatching::ReadPartBlock(BYTE *lpData,int ImgWid,int ImgHei,BYTE *lpPart,CPoint Centerpos,int PartImgWid,int PartImgHei)
{
int i;
Centerpos.x-=PartImgWid/2;Centerpos.y-=PartImgHei/2;
for(i=0;iImgWid-1||y<0||y>ImgHei-1) return 0;
double dx=xR-x,dy=yR-y;
sample=(1-dx)*(1-dy)**(lpImg+ImgWid*y+x)+
(1-dx)*dy**(lpImg+ImgWid*(y+1)+x)+
dx*(1-dy)**(lpImg+ImgWid*y+x+1)+
dx*dy**(lpImg+ImgWid*(y+1)+x+1);
return sample;
}
////////////////////////////////////////////////////////////////////////////
//函数名： ErrorEquation
//函数功能： 组建误差方程以及法方程
//输入参数： 无
//返回值： 无
//说明： 边计算边法化
//异常： 无
/////////////////////////////////////////////////////////////////////////////////
void CLSMatching::ErrorEquation()
{
int i,j;
double A[8],l,dgx,dgy;
memset(m_AA,0,sizeof(double)*64);
memset(m_Al,0,sizeof(double)*8);
double x,y;
for(i=-m_MatchWindowsHei/2;i<=m_MatchWindowsHei/2;i++)
{
for(j=-m_MatchWindowsWid/2;j<=m_MatchWindowsWid/2;j++)
{
x=xshiftR+m_t0[2]+m_t0[3]*j+m_t0[4]*i;
y=yshiftR+m_t0[5]+m_t0[6]*j+m_t0[7]*i;
dgx=(resample(m_lpImgR,m_RImgHei,m_RImgWid,x+1,y)-
resample(m_lpImgR,m_RImgHei,m_RImgWid,x-1,y))/2;
dgy=(resample(m_lpImgR,m_RImgHei,m_RImgWid,x,y+1)-
resample(m_lpImgR,m_RImgHei,m_RImgWid,x,y-1))/2;
A[0]=1.0;A[1]=resample(m_lpImgR,m_RImgHei,m_RImgWid,x,y);
A[2]=m_t0[1]*dgx; A[3]=(j)*A[2]; A[4]=(i)*A[2];
A[5]=m_t0[1]*dgy; A[6]=(j)*A[5]; A[7]=(i)*A[5];
l=m_t0[0]+m_t0[1]*A[1]-GetPixel(m_lpImgL,m_LImgHei,m_LImgWid,
(int)(xshiftL+j+0.5),(int)(yshiftL+i+0.5));
pNormal(A,8,-l,m_AA, m_Al,1.0);
}
}
}
////////////////////////////////////////////////////////////////////////////
//函数名： Iteration
//函数功能： 迭代求解过程
//输入参数： 无
//返回值： 无
//说明： 以相关系数作为中止条件
//异常： 无
/////////////////////////////////////////////////////////////////////////////////
void CLSMatching::Iteration()
{
int time,i,j;
double t[8],variance;
double coef0=0.0,coef;
//读取左影像块
ReadPartBlock(m_lpImgL,m_LImgWid,m_LImgHei,m_lpLImgBlock,
CPoint((int)xshiftL,(int)yshiftL),m_MatchWindowsWid,m_MatchWindowsHei);
//读取右影像块
double x,y;
for(i=-m_MatchWindowsHei/2;i<=m_MatchWindowsHei/2;i++)
{
for(j=-m_MatchWindowsWid/2;j<=m_MatchWindowsWid/2;j++)
{
x=xshiftR+m_t0[2]+m_t0[3]*j+m_t0[4]*i;
y=yshiftR+m_t0[5]+m_t0[6]*j+m_t0[7]*i;
m_lpRImgBlock[(int)(i+m_MatchWindowsHei/2)*m_MatchWindowsWid+j+(int)(m_MatchWindowsWid/2)]=(BYTE)(m_t0[0]+m_t0[1]*resample(m_lpImgR,m_RImgHei,m_RImgWid,x,y)+0.5);
}
}
coef0=Correlation_Coeficient(m_lpLImgBlock,m_MatchWindowsWid,
m_lpRImgBlock,m_MatchWindowsWid,m_MatchWindowsWid,m_MatchWindowsHei,&variance);
do
{
//保存上次结果
coef0 = coef;
memcpy(m_t0,m_t,sizeof(double)*8);
//组建误差方程
ErrorEquation();
Gauss(m_AA,m_Al,8);
memcpy(t,m_Al,sizeof(double)*8);
//解方程
//参数更新
m_t[0]=m_t0[0]+t[0]+m_t0[0]*t[1];//h0
m_t[1]=m_t0[1]+ m_t0[1]*t[1];//h1
m_t[2]=m_t0[2]+t[2]+m_t0[2]*t[3]+m_t0[5]*t[4];//a0
m_t[3]=m_t0[3]+ m_t0[3]*t[3]+m_t0[6]*t[4];//a1
m_t[4]=m_t0[4]+ m_t0[4]*t[3]+m_t0[7]*t[4];//a2
m_t[5]=m_t0[5]+t[5]+m_t0[2]*t[6]+m_t0[5]*t[7];//b0
m_t[6]=m_t0[6]+ m_t0[3]*t[6]+m_t0[6]*t[7];//b1
m_t[7]=m_t0[7]+ m_t0[4]*t[6]+m_t0[7]*t[7];//b2
//计算相关系数
//读取右影像块
double x,y;
for(i=-m_MatchWindowsHei/2;i<=m_MatchWindowsHei/2;i++)
{
for(j=-m_MatchWindowsWid/2;j<=m_MatchWindowsWid/2;j++)
{
x=xshiftR+m_t[2]+m_t[3]*j+m_t[4]*i;
y=yshiftR+m_t[5]+m_t[6]*j+m_t[7]*i;
m_lpRImgBlock[(int)(i+m_MatchWindowsHei/2)*m_MatchWindowsWid+j+(int)(m_MatchWindowsWid/2)]=(BYTE)(m_t[0]+m_t[1]*resample(m_lpImgR,m_RImgHei,m_RImgWid,x,y)+0.5);
}
}
//计算相关系数
coef=Correlation_Coeficient(m_lpLImgBlock,m_MatchWindowsWid,
m_lpRImgBlock,m_MatchWindowsWid,m_MatchWindowsWid,m_MatchWindowsHei,&variance);
//根据相关系数确定是否继续迭代
time++;
if(time>1000) break;
}while(coef > coef0);
m_time=time;
}
////////////////////////////////////////////////////////////////////////////
//函数名： GetResult
//函数功能： 结果给出
//输入参数： double coef0 相关系数阈值
//输出参数 double *xL,double *yL,double *xR,double *yR 左右同名点
//返回值： 无
//说明： 无
//异常： 无
/////////////////////////////////////////////////////////////////////////////////
int CLSMatching::GetResult(double *xL,double *yL,double *xR,double *yR)
{
Iteration();
*xR=xshiftR+m_t0[2];//+m_t0[3]*m_xL+m_t0[4]*m_yL;
*yR=yshiftR+m_t0[5];//+m_t0[6]*m_xL+m_t0[7]*m_yL;
return m_time;
*xL=m_xL; *yL=m_yL;
if(m_time==MAXITERATION)
{
*xL=0;
*yL=0;
*xR=0;
*yR=0;
}
else
{
*xR=xshiftR+m_t0[2];//+m_t0[3]*m_xL+m_t0[4]*m_yL;
*yR=yshiftR+m_t0[5];//+m_t0[6]*m_xL+m_t0[7]*m_yL;
}
return m_time;
}