双目对物体定位

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
// 
}