双目对物体定位

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()