my code is saying aren't matching on line 130-132

Question

Spencer 2024-3-19

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2096501-my-code-is-saying-aren-t-matching-on-line-130-132

移动：Walter Roberson 2024-3-20

clc;
clear;
%format bank;
format
d2r=180/pi;
%%camera parameters
% focal length
f=153.411;
% principal point offset
xp=0.0;
yp=0.0;
%%fiducial marks
fiducialImg=[9211.42 338.36
    409.13 9197.81
    9239.98 9169.63
    381.43 366.29
    9475.49 4753.47
    145.81 4782.73
    4825.44 9434.13
    4795.81 102.60];
fiduclalPho=[105.998 105.994
    -105.996 -106.004
    106.004 -106.004
    -105.997 105.991
    112.002 -0.003
    -111.993 -0.009
    0.002 -112.004
    0.002 111.994];
% affine transformation of left
[AFF,sigma]=Affine_Transformation(fiducialImg,fiduclalPho);
Unrecognized function or variable 'Affine_Transformation'.
GCPXYZ=[3 6251489.939 2024652.910 390.391
    6 6252118.372 2024733.835 390.151
    10 6252117.285 2023978.197 405.108
    12 6251650.262 2024354.615 370.148
    13 6251005.042 2023989.666 359.481];
ID=GCPXYZ(:,1);
XA=GCPXYZ(:,2);
YA=GCPXYZ(:,3);
ZA=GCPXYZ(:,4);
GCPIMG=[3 7928.86 3729.87
    6 8259.76 6314.77
    10 516448 6341.35
    12 6683.78 4395.82
    13 5191.85 1774.61];
%This converts GCP points you measured (row/col) to phtoto coordinates
[xa,ya]=img2photo(GCPIMG(:,2),GCPIMG(:,3),AFF);
% now we have GCp(xyz) and GCP(xa,ya)
%% Aproximation
% we need to approximate xo,yo,zo,w,p,k
omega=0;
Phi=0;
ConformalT=Conformal_Transformation([XA,YA],[xa,ya]);
Kappa=atan2(ConformalT(2),ConformalT(1));
Xo=ConformalT(3);
Yo=ConformalT(4);
%% Approximation, Zo
GroundD = sqrt( (XA(1)-XA(2))^2 + (YA(1)-YA(2))^2);
PhotoD  = sqrt( (xa(1)-xa(2))^2 + (ya(1)-ya(2))^2);
Zo=GroundD*f/PhotoD+mean(ZA);
%Compute XYZ wpk of a camera, least squares
% initial approximation of xyz wpk
EOP=[Xo
    Yo
    Zo
    omega
    Phi
    Kappa];
IOP=[f xp yp];
numofGCP=5;
A = zeros(numofGCP*2,6);
Y = zeros(numofGCP*2,1);
for iteration =1:20
    fprintf('iteration: %d/n',iteration);
    
    R = RotationM(EOP(4),EOP(5),EOP(6));
    sk=sin(EOP(6));
    ck=cos(EOP(6));
    for i=1:numofGCP
        dx=XA(i)-EOP(1);
        dy=YA(i)-EOP(2);
        dz=ZA(i)-EOP(3);
        Nx=R(1,1)*dx+R(2,1)*dy+R(3,1)*dz;
        Ny=R(1,2)*dx+R(2,2)*dy+R(3,2)*dz;
        D=R(1,3)*dx+R(2,3)*dy+R(3,3)*dz;
        
        % par. Der. of F w r t XA Ya ZA w p k
        
        dFdXA=f/(D*D)*(R(1,1)*D-R(1,3)*Nx);
        dFdYA=f/(D*D)*(R(2,1)*D-R(2,3)*Nx);
        dFdZA=f/(D*D)*(R(3,1)*D-R(3,3)*Nx);
        dFdw=-f/(D*D)*(D*(-R(3,1)*dy+R(2,1)*dz)+Nx*(R(3,3)*dy-R(2,3)*dz));
        dFdp=-f/(D*D)*(-D*D*ck+Nx*(-Nx*ck+Ny*sk));
        dFdk=-f*Ny/D;
        
        % par. der. of gw r t cxa ya za w p k
        
        dGdXA=f/(D*D)*(R(1,2)*D-R(1,3)*Ny);
        dGdYA=f/(D*D)*(R(2,2)*D-R(2,3)*Ny);
        dGdZA=f/(D*D)*(R(3,2)*D-R(3,3)*Ny);
        dGdW=-f/(D*D)*(D*(-R(3,2)*dy+R(2,2)*dz)+Nx*(R(3,3)*dy-R(2,3)*dz));
        dGdp=f/(D*D)*(D*D*sk+Nx*(Nx*ck+Ny*sk));
        dGdk=f*Nx/D;
        % jacobian matrix
        A(2*i-1,1:6)=[dFdXA dFdYA dFdZA dFdw dFdp dFdk];
        A(2*i+0,1:6)=[dGdXA dGdYA dGdZA dGdW dGdp dGdk];
        
        xao=xp-f*Nx/D;
        yao=yp-f*Ny/D;
        
        Y(2*i-1,1)=xa(i,1)-xao;
        Y(2*i-0,1)=ya(i,1)-yao;
    end
    N=A'*A;
    C=A'*Y;
    Kh=N/C;
    
    e=Y-(A*Kh);
    so = sqrt( (e'*e)/(length(A)-rank(A)));
    % update XYZ
    EOP(1:3)=EOP(1:3)+Kh(1:3);
    EOP(4:6)=EOP(4:6)+Kh(4:6);
    
    if abs(Kh(1:3))<1/1000000
        dia = so*sqrt(diag(inv(N)));
        fprintf('Correction XO, Yo, ZO is lass that 1/10000000\n');
        fprintf('estimated sigma0: %3.7f\n',so);
        
        fprintf('estimated Xo: %3.3f %s %3.5f \n',EOP(1),charr(177),dia(1));
        fprintf('estimated Yo: %3.3f %s %3.5f \n',EOP(2),charr(177),dia(2));
        fprintf('estimated Zo: %3.3f %s %3.5f \n\n',EOP(3),charr(177),dia(3));
        
        fprintf('estimated W: %11.8f %s %3.7f rad \n',EOP(4),charr(177),dia(4));
        fprintf('estimated P: %11.8f %s %3.7f rad \n',EOP(5),charr(177),dia(5));
        fprintf('estimated K: %11.8f %s %3.7f rad \n\n',EOP(6),charr(177),dia(6));
        
        
        fprintf('estimated W: %11.7f %s %3.7f deg \n',EOP(4),charr(177),dia(4)/d2r);
        fprintf('estimated P: %11.7f %s %3.7f deg \n',EOP(5),charr(177),dia(5)/d2r);
        fprintf('estimated K: %11f %s %3.7f deg \n\n',EOP(6),charr(177),dia(6)/d2r);
        break;
    end
end
%% Ground to image and image to photo
IMG=imrad('01-0011.jpg');
[px,py]=Object2Photo(XA,YA,ZA,EOP,IOP);
[Ix,Iy]=photo2img(px,py,AFF);
figure(2);
imshow(Img);hold on;
plot(Ix,Iy,'ro','Markersize',10);
plot(GCPIMG(:,2),GCPIMG(:,3),'b*','MArkersize',10);
Legend('Projected','Measured');
for i=1:numofGCP
    text(Ix(i),Iy(i),num2str(ID(i)),'color',[1 0 0], 'fontsize',20);
end
impxelinfo;
%%Reprojection error
Reproj_Error=sum(sqrt((GCPIMG(:,2)-IX).^2+(GCPIMG(:,3)-IY).^2))/numofGCP;
fprintf('reporjection error: %3.4f pixels\n',Reproj_Error);