zoukankan
html css js c++ java
双目对物体定位
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
//
}
查看全文
相关阅读:
Move site collections to a new database
SharePoint 2010 Site Workflow
Customizing the Server Ribbon
[转]深入浅出之正则表达式
asp.net中MD5加密写法代码
asp.net中过滤非法字符防止SQL注入
jQuery中ajax的使用方法
gridview中用NewSelectedIndex获取选中的ID值
GridView中得到选中的某行的ID值
用javascript伪装链接地址状态代码
原文地址:https://www.cnblogs.com/wqj1212/p/1006414.html
最新文章
结束Excel的进程的方法
短信开发相关
结合MSHTML与WebBrowser生成美观实用的WinForm应用程序(By Lostinet)
如何在C#中加载自己编写的动态链接库(DLL)
c#启动单个程序(互斥机制)
WinForm 窗体之间交互的一些方法兼托管事件
Winform中的parent和owner属性
使用BackgroundWorker组件
利用委托实现异步调用
WinForm 布局问题
热门文章
c#自定义事件和委托
C#委托和事件
C#中自定义事件的触发
C#随笔记录
[MSDN]How to: Design a Workflow Task Form to Use Task Data
How to Display a SharePoint Dialog from Ribbon Button and Get Selected Item Context
How to Use Metadata Navigation in Enterprise Wiki Site (Sharepoint 2010)
[转]Setting a SharePoint 2010 Config DB failover server with PowerShell
SharePoint 2010多语言UI,以及开发人员需要注意的
[Override]Using Developer Dashboard in SharePoint 2010
Copyright © 2011-2022 走看看