单目对物体定位

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)