clearvars?-except??cameraParams??estimationerrors??REs?;??clc;
%%?
img_num?=?18;
RobotEffectorPose;??????%機(jī)器人末端位姿
%%?
TE?=?estimationerrors.ExtrinsicsErrors.TranslationVectorsError;
Tex?=?mean（TE（:1））;???Tey?=?mean（TE（:2））;??Tez?=?mean（TE（:3））;
errorT?=?[TexTeyTez];
%%?求末端和攝像機(jī)轉(zhuǎn)軸kl?kr.
kl=[];?kr=[];?theta_r=[];?theta_l=[];
for?i?=?1:img_num-2
????%%?求攝像機(jī)轉(zhuǎn)軸Kr
????Rr1?=?cameraParams.RotationMatrices（::i）;
????Tr1?=?cameraParams.TranslationVectors（i:）+errorT;
????Rr2?=?cameraParams.RotationMatrices（::i+1）;
????Tr2?=?cameraParams.TranslationVectors（i+1:）+errorT;
????Rr3?=?cameraParams.RotationMatrices（::i+2）;
????Tr3?=?cameraParams.TranslationVectors（i+2:）+errorT;
????Rrt1?=?[Rr1‘?（Tr1/1000）‘;0001]/[Rr2‘?（Tr2/1000）‘;0001];
????Rrt2?=?[Rr2‘?（Tr2/1000）‘;0001]/[Rr3‘?（Tr3/1000）‘;0001];
????[fr1?thetar1]?=?InvRot（Rrt1）;
????[fr2?thetar2]?=?InvRot（Rrt2）;
????kr?=?[krfr1fr2cross（fr1fr2）];??theta_r?=?[theta_r;thetar1;thetar2];
????%%?求末端轉(zhuǎn)軸Kl
????Re1?=?REs（4*i-3:4*i:）;
????Re2?=?REs（4*（i+1）-3:4*（i+1）:）;?
????Re3?=?REs（4*（i+2）-3:4*（i+2）:）;?
????Ret1?=?Re1\Re2;
????Ret2?=?Re2\Re3;
????Rlt1?=?Ret1（1:31:3）;
????Rlt2?=?Ret2（1:31:3）;
????[fl1?thetal1]?=?InvRot（Rlt1）;
????[fl2?thetal2]?=?InvRot（Rlt2）;
????kl?=?[klfl1fl2cross（fl1fl2）];??theta_l?=?[theta_l;thetal1;thetal2];
end
%%?求手眼矩陣旋轉(zhuǎn)關(guān)系
Rm?=?kl*pinv（kr）;??????%-----------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%?求手眼關(guān)系平移向量
Prs=[];?Pls=[];
for?i?=?1:img_num-1
%%?（E-Rli）*Pm?=?Pli-Rm*Pri?----------構(gòu)造右端????
????%%?末端平移向量
????TRl1?=?REs（4*i-3:4*i:）;?
????TRl2?=?REs（4*（i+1）-3:4*（i+1）:）;???
????Rl?=?TRl1\TRl2;
????Pli?=?Rl（1:34）;????????????%--------------Pli
????%%?相機(jī)外參平移向量
????TRr1?=?cameraParams.RotationMatrices（::i）;
????TTr1?=?cameraParams.TranslationVectors（i:）+errorT;
????TRr2?=?cameraParams.RotationMatrices（::i+1）;
????TTr2?=?cameraParams.TranslationVectors（i+1:）+errorT;
????TRlt?=?[TRr1‘?（TTr1/1000）‘;0001]/[TRr2‘?（TTr2/1000）‘;0001];
????Pri?=?TRlt（1:34）;
????Pr?=?Pli?-?Rm*Pri;
????Prs?=?[Prs;Pr];????%--------------存儲(chǔ)
%%?（E-Rli）*Pm?=?Pli-Rm*Pri?----------構(gòu)造左端?
????E=[100;010;001];
????Rli?=?Rl（1:31:3）;
????Pl?=?E?-?Rli;
????Pls?=?[Pls;Pl];???%--------------存儲(chǔ)
end
%%?求手眼矩陣平移向量
Pm?=?pinv（Pls）*Prs;??%-----------------------------------------
H_E?=?[RmPm;0001]
%clearvars?-except??cameraParams??estimationerrors??H_E?;

?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----

?????文件???????2645??2018-11-30?14:38??HandEyeCalib-Matlab.m

?????文件????????169??2018-12-05?18:23??說(shuō)明.txt

-----------?---------??----------?-----??----

?????????????????2814????????????????????2