相机的交会测量（位姿估计的方法之一）即已知图像坐标和相机的内外参数，可以结合两个相机获得的图像坐标，对目标位置进行交会测量。

相机的交会测量（位姿估计的方法之一）即已知图像坐标和相机的内外参数，可以结合两个相机获得的图像坐标，对目标位置进行交会测量。

参数函数：

function [Ai,Bi] = parameter(fx_i,fy_i,cx_i,cy_i,xi,yi,eul_i,T_i)
% fx_1 = 1500;
% fy_1 = 1500;
% cx_1 = 540;
% cy_1 = 540;
% T_1 = [0;20;100];
% eul_1 = [0,0,0];

R = eul2rotm(eul_i);

a1 = (xi-cx_i)*R(3,1)-fx_i*R(1,1);
a2 = (xi-cx_i)*R(3,2)-fx_i*R(1,2);
a3 = (xi-cx_i)*R(3,3)-fx_i*R(1,3);
a4 = (yi-cy_i)*R(3,1)-fy_i*R(2,1);
a5 = (yi-cy_i)*R(3,2)-fy_i*R(2,2);
a6 = (yi-cy_i)*R(3,3)-fy_i*R(2,3);

Ai = [a1,a2,a3;a4,a5,a6];

b1 = (xi-cx_i)*T_i(3) - fx_i*T_i(1);
b2 = (yi-cy_i)*T_i(3) - fy_i*T_i(2);

Bi = -[b1;b2];
end

主函数：

% clear the workspace
clear;
clc;
% set the parameters
%% camera1
fx_1 = 1500;
fy_1 = 1500;
cx_1 = 540;
cy_1 = 540;
T_1 = [0;20;100];
eul_1 = [0,0,0];
x1 = 676.3636;
y1 = 949.0909;

%% camera2
fx_2 = 2000;
fy_2 = 2000;
cx_2 = 540;
cy_2 = 540;
T_2 = [-5;0;120];
eul_2 = [0,-pi/2,0];
x2 = 309.2305;
y2 = 693.8462;

%% camera3
fx_3 = 1500;
fy_3 = 1500;
cx_3 = 540;
cy_3 = 540;
T_3 = [0;0;100];
eul_3 = [0,pi/4,0];
x3 = 752.1320;
y3 = 690.0000;

%% camera4

fx_4 = 1500;
fy_4 = 1500;
cx_4 = 540;
cy_4 = 540;
T_4 = [1;-1;100];
eul_4 = [0,-pi/4,0];
x4 = 553.1415;
y4 = 658.2736;

%% camera5

fx_5 = 2000;
fy_5 = 2000;
cx_5 = 640;
cy_5 = 640;
T_5 = [0;10;100];
eul_5 = [0,-pi/2,0];
x5 = 458.1818;
y5 = 1003.6364;

%% camera6
fx_6 = 1500;
fy_6 = 1500;
cx_6 = 500;
cy_6 = 400;
T_6 = [0;10;100];
eul_6 = [0,-pi/2,0];
x6 = 363.6364;
y6 = 672.7273;

% different settings
[A1,B1] = parameter(fx_1,fy_1,cx_1,cy_1,x1,y1,eul_1,T_1);
[A2,B2] = parameter(fx_2,fy_2,cx_2,cy_2,x2,y2,eul_2,T_2);
[A3,B3] = parameter(fx_3,fy_3,cx_3,cy_3,x3,y3,eul_3,T_3);
[A4,B4] = parameter(fx_4,fy_4,cx_4,cy_4,x4,y4,eul_4,T_4);

A = [A1;A2;A3;A2;];
B = [B1;B2;B3;B4];

% method 1:
X = AB;
X1 = X-10*ones(3,1);%error of got vaule
error = norm(X1,2);%norm of error

% method 2:
X2 = inv(transpose(A)* A) * transpose(A) * B;
error1 = norm(X,2);
%%x,y,x vaule  with error
x_err = zeros(1,100); 
y_err = zeros(1,100);
z_err = zeros(1,100);

可以增加一些误差进行分析，如考虑观测误差

 for i=1:100

%    err =normrnd(0.1,8);     %%随机产生一个服从高斯分布的误差
%     x1_err_i = x1+err;
%     y1_err_i = y1+err;
%     
%     x2_err_i = x2+i;
%     y2_err_i = y2+i-0.5;
% 
%     [A1_err_i,B1_err_i] = parameter(fx_1,fy_1,cx_1,cy_1,x1_err_i,y1_err_i,eul_1,T_1);
%     [A2_err_i,B2_err_i] = parameter(fx_2,fy_2,cx_2,cy_2,x2_err_i,y2_err_i,eul_2,T_2);
%     A_err_i = [A1_err_i;A2_err_i];
%     B_err_i = [B1_err_i;B2_err_i];
% 
%     X_err_i = A_err_iB_err_i;
%     x_err(1,i) = X_err_i(1);
%     y_err(1,i) = X_err_i(2);
%     z_err(1,i) = X_err_i(3);
% end