单目对物体定位

Writen by wqj1212@yahoo.com.cn

function y=Dw(u,v,Gtb)
Kl=[2237.01/3.368 0 310.29/3.368 0;0 2237.54/3.38 242.11/3.38 0;0 0 1 0];
//摄像机与工具关系
Gctl= [-0.0057    0.9916    0.0122    9.3578;
   -1.0052   -0.0109   -0.0179      78.9181;
   -0.0224   -0.0267    1.0121       303.7695;
     0        0            0           1     ];
 
G=inv(Gtb);//Gtb是工具在机器人基坐标的位置
z=-270;//指定物体的高度
M=Kl*Gctl*G;
Ml=[u*M(3,1)-M(1,1)  u*M(3,2)-M(1,2)  ;   v*M(3,1)-M(2,1)  v*M(3,2)-M(2,2)];
Mr=[M(1,4)-u*M(3,4)-(u*M(3,3)-M(1,3))*z;  M(2,4)-v*M(3,4)-(v*M(3,3)-M(2,3))*z];
y=inv(Ml)*Mr;//得到物体的位置

void COneCameralGrabDlg::OnTimer(UINT nIDEvent) 
{
    // TODO: Add your message handler code here and/or call default
    DWORD t1=::GetTickCount();
    double p[12]={0,0,0,0,0,0,0,0,0,0,0,0};
   double q[12]={0,0,0,0,0,0,0,0,0,0,0,0};
  Gtb=mxCreateDoubleMatrix(4,4,mxREAL);
    Result=mxCreateDoubleMatrix(1,1,mxREAL);
ul = mxCreateDoubleMatrix(1,1, mxREAL); 
vl = mxCreateDoubleMatrix(1,1, mxREAL); 
    CHECK_EXIT(milcv.MilGetImage(milcv.Mil_CH0,foreImage));
    ObjectImage=cvCreateImage(cvGetSize(foreImage),8,1);
bkMat=cvCreateMat(480,640, CV_32FC1);
    foreMat=cvCreateMat(480,640, CV_32FC1);
ObjectMat=cvCreateMat(480,640, CV_32FC1);
    cvConvert(bkImage,bkMat);
    cvConvert(foreImage,foreMat);
    cvAbsDiff(foreMat,bkMat,ObjectMat);
    cvConvert(ObjectMat,ObjectImage);
    cvThreshold(ObjectImage,ObjectImage,40,255,CV_THRESH_BINARY);
    point=CImageProcess::ImageCenter(ObjectImage);
    u=(double)point.x;
    v=(double)point.y;
    memcpy(mxGetPr(ul),&u,sizeof(double));
memcpy(mxGetPr(vl),&v,sizeof(double));

    CHECK_VOID(BscIsLoc(g_nCid,0,&rconf,p)==0);
    q[0]=p[0];q[1]=p[1];q[2]=p[2];q[3]=p[3];q[4]=p[4];q[5]=p[5];
    
a=p[3]/180 *CV_PI;
b=p[4]/180 *CV_PI;
r=p[5]/180*CV_PI;
double  C[16]={cos(r)*cos(b),sin(r)*cos(b),-sin(b),0,-sin(r)*cos(a)+cos(r)*sin(b)*sin(a),cos(r)*cos(a)+sin(r)*sin(b)*sin(a),cos(b)*sin(a),0,sin(r)*sin(a)+cos(r)*sin(b)*cos(a), -cos(r)*sin(a)+sin(r)*sin(b)*cos(a), cos(b)*cos(a),0,p[0],p[1],p[2],1};

memcpy(mxGetPr(Gtb),C,16*sizeof(double));
ofstream outfile("test.txt");


prhs[0]=ul;
prhs[1]=vl;
prhs[2]=Gtb;

libDwInitialize();
mlxDw(1, &Result, 3, prhs);

//libDwTerminate();
 p[0]=*mxGetPr(Result);
p[1]=*(mxGetPr(Result)+1);
p[2]=-270;


CHECK_VOID(BscMovj(g_nCid,10,"BASE",rconf,toolno, p)==0);
p[0]=466.921;
 p[1]=12.951;
 p[2]=-276.904;
 p[3]=180;
 p[4]=-2.04;
 p[5]=8.88;
CHECK_VOID(BscMovj(g_nCid,10,"BASE",rconf,toolno, p)==0);
p[0]=q[0];
 p[1]=q[1];
 p[2]=q[2];
 p[3]=q[3];
 p[4]=q[4];
 p[5]=q[5];
 CHECK_VOID(BscMovj(g_nCid,10,"BASE",rconf,toolno, p)==0);
mxDestroyArray(Gtb);
mxDestroyArray(ul);
mxDestroyArray(vl);
mxDestroyArray(Result);
cvReleaseMat(&bkMat);
cvReleaseMat(&foreMat);
cvReleaseMat(&ObjectMat);
cvReleaseImage(&ObjectImage);
DWORD t2=::GetTickCount();
DWORD t=t2-t1;
    CDialog::OnTimer(nIDEvent);
}