Mein MATLAB Forum

clear all;
close all;

% ABWLCD 640 Cross
ResX1 = 640;
ResY1 = 640;
PixelSizeX1 = 0.09;
PixelSizeY1 = 0.09;
C1 = 1.7724E+002;
DX1 = 2.1716E-001;
DY1 = 1.7730E-001;
Position1 = [222.0791 373.8957 911.3025];
Rotation1 = [0.998381 -0.005423 -0.056614
-0.005185 0.982621 -0.185551
0.056637 0.185544 0.981002];

%Basler_A113_2
ResX2 = 1296;
ResY2 = 1024;
PixelSizeX2 = 0.0067;
PixelSizeY2 = 0.0067;
C2 = 1.7705E+001;
DX2 = -1.0847E-001;
DY2 = 1.3484E-002;
Position2 = [-90.4753 309.1083 600.8930];
Rotation2 = [0.928724 0.067645 0.364550
0.005060 0.980812 -0.194888
-0.370738 0.182842 0.910561];

c = 400;
r = 740;

dc = PixelSizeX2;
dr = PixelSizeY2;
nc = ResX2;
nr = ResY2;

c0 = (nc+1)/2;
r0 = (nr+1)/2;
x_cam = (c-c0)*dc;
y_cam = -(r-r0)*dr;

x2 = x_cam;
y2 = y_cam;

%%
x_streifen = 346;
y_streifen = 354;

dx = PixelSizeX1;
dy = PixelSizeY1;
nx = ResX1;
ny = ResY1;

x0 = (nx+1)/2; %oder mit dx,dy als korrektur des bildhauptpunktes
y0 = (ny+1)/2;
x_pro = (x_streifen-x0)*dx;
y_pro = (y_streifen-y0)*dy;

x1 = x_pro;
y1 = y_pro;


R1 = Rotation1;
X01 = Position1';
vec1 = {x1;y1;C1};
vec1 = cell2mat(vec1);

v1 = R1*vec1;

R2 = Rotation2;
X02 = Position2'; %ebenenursprung
vec2 = {x2;y2;C2};
vec2 = cell2mat(vec2);

v2 = R2*vec2;

d = cross(v1,v2);

b = X02-X01;

mat1 = [v1(1),v2(1),d(1);v1(2),v2(2),d(2);v1(3),v2(3),d(3)];
lambda = mat1^-1*b;

P0 = X01 + lambda(1)*v1 - 0.5*lambda(3)*d;
display(P0);