双目对物体定位

writen by wqj1212@yahoo.com.cn

function y=DingWei(ul,vl,ur,vr,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];
Kr=[2180.22/3.368 0  326.79/3.368 0;0 2180.43/3.38 247.89/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     ];
Gctr= [-0.0105    0.9728    0.0239  33.9378;
     -0.9868   -0.0103   -0.0184   89.4880;
     -0.0296   -0.0199    1.0114   305.9265;
       0        0            0         1   ];

%摄像机相对于base的坐标变换 
   G=inv(Gtb);

Ml=Kl*Gctl*G;
Mr=Kr*Gctr*G;

%物体相对于机器人base的坐标
Mll=[ul*Ml(3,1)-Ml(1,1)  ul*Ml(3,2)-Ml(1,2)   ul*Ml(3,3)-Ml(1,3);
     vl*Ml(3,1)-Ml(2,1)  vl*Ml(3,2)-Ml(2,2)   vl*Ml(3,3)-Ml(2,3);
     ur*Mr(3,1)-Mr(1,1)  ur*Mr(3,2)-Mr(1,2)   ur*Mr(3,3)-Mr(1,3);
     vr*Mr(3,1)-Mr(2,1)  vr*Mr(3,2)-Mr(2,2)   vr*Mr(3,3)-Mr(2,3)];
Mlr=[Ml(1,4)-ul*Ml(3,4);Ml(2,4)-vl*Ml(3,4);Mr(1,4)-ur*Mr(3,4);Mr(2,4)-vr*Mr(3,4)];
y=inv(Mll'*Mll)*Mll'*Mlr

void CMObjRecDlg::OnButtonGrab() 
{
    // TODO: Add your control notification handler code here
#if 0
double ul=0.0,vl=0.0,ur=0.0,vr=0.0;   double rl,rr=0.0;

        double a=0.0,b=0.0,r=0.0;
    double spd=2.0;
    short toolno=0;
   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};
    WORD rconf;
    mxArray *Gtb=mxCreateDoubleMatrix(4,4,mxREAL);
        mxArray *Result=mxCreateDoubleMatrix(1,1,mxREAL);
mxArray * prhs[5];

CvPoint pointleft=CGrabProcess::ImageCenter(leftErzhi);
CvPoint pointright=CGrabProcess::ImageCenter(rightErzhi);
ul=8*pointleft.x;vl=8*pointleft.y;
ur=8*pointright.x;vr=8*pointright.y;
mxArray *ull = mxCreateDoubleMatrix(1,1, mxREAL); 
mxArray *vll = mxCreateDoubleMatrix(1,1, mxREAL); 
mxArray *urr = mxCreateDoubleMatrix(1,1, mxREAL); 
mxArray *vrr = mxCreateDoubleMatrix(1,1, mxREAL); 
memcpy(mxGetPr(ull),&ul,sizeof(double));
memcpy(mxGetPr(vll),&vl,sizeof(double));
memcpy(mxGetPr(urr),&ur,sizeof(double));
memcpy(mxGetPr(vrr),&vr,sizeof(double));

    
short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Etherne
    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
        //libDingWeiInitialize();
/**//*        double spd=2.0;
    short toolno=0;
   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};
    WORD rconf;*/

    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;
/**//*    mxArray *Gtb=mxCreateDoubleMatrix(4,4,mxREAL);
        mxArray *Result=mxCreateDoubleMatrix(1,1,mxREAL);
mxArray * prhs[5];*/
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));

prhs[0]=ull;
prhs[1]=vll;
prhs[2]=urr;
prhs[3]=vrr;
prhs[4]=Gtb;

libDingWeiInitialize();
mlxDingwei(1, &Result, 5, prhs);
 libDingWeiTerminate();
/**//*for(int i=0;i<mxGetNumberOfElements(Result);i++)
{ 
    cout<<*(mxGetPr(Result)+i)<<endl;
}*/
p[0]=*mxGetPr(Result)+18;
p[1]=*(mxGetPr(Result)+1)+2;
p[2]=-270;
DWORD T1=GetTickCount();
CHECK_VOID(BscMovj(g_nCid,20,"BASE",rconf,toolno, p)==0);
DWORD T2=GetTickCount();
DWORD T=T2-T1;
 /**//*p[0]=296.288;
 p[1]=483.657;
 p[2]=-219.261;
 p[3]=180;
 p[4]=19.52;
 p[5]=58.51;
CHECK_VOID(BscMovj(g_nCid,20,"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,20,"BASE",rconf,toolno, p)==0);*/


#endif
// 
}