Orthogonal Distance Fit of a Pyramid (Geodäsie/Vermessung)

Hi,

Also, if I have trouble understanding your algorithm, would I be able to ask you a question?

Surely, I will answered at the weekend.

Only short, I'm in a hurry. I just remove the plane 4, the modified code is:

%% 3-Faces-Pyramid
%
%   nx(i) * x(j,i) + ny(i) * y(j,i) + nz(i) * z(j,i) - d(i) = 0
% with i = 1 .. 3
%
%   ||n(i)|| == 1
%
%  and
%
%   n(i)' * Apex -d(i) == 0
%
%
% @version 1.0
% @author Michael Loesler - derletztekick.com

clear all
close all
format long g
clc

plane1 = [
-8.7740 10.2929 -0.2645
-8.7546 10.2701 -0.3177
-8.8042 10.2864 -0.2647
-8.7602 10.2767 -0.2908
-8.7154 10.3041 -0.2639
-8.7347 10.2841 -0.2905
-8.7470 10.2985 -0.2641
-8.8216 10.3067 -0.2113
-8.8498 10.2977 -0.2114
-8.7888 10.3102 -0.2109
-8.8108 10.2940 -0.2379
-8.7844 10.3051 -0.2378
-8.7524 10.3050 -0.2374
-8.7637 10.3184 -0.2109
-8.8621 10.3120 -0.1847
-8.8280 10.3142 -0.1843
-8.7998 10.3231 -0.1842
-8.7734 10.3298 -0.1841
-8.7055 10.3322 -0.2103
-8.7251 10.3155 -0.2372
-8.6989 10.3245 -0.2371
-8.7321 10.3238 -0.2106
-8.6751 10.3405 -0.2103
-8.7436 10.3375 -0.1838
-8.7142 10.3427 -0.1837
-8.6877 10.3555 -0.1837
-8.6584 10.3609 -0.1835
];

plane2 = [
-8.9011 10.3140 -0.4292
-8.8979 10.3103 -0.4021
-8.8503 10.2982 -0.4009
-8.8934 10.3051 -0.3204
-8.8922 10.3037 -0.3744
-8.8957 10.3077 -0.3475
-8.8903 10.3014 -0.2660
-8.8403 10.2866 -0.3192
-8.8439 10.2908 -0.3733
-8.8447 10.2917 -0.3463
-8.7987 10.2798 -0.3454
-8.7972 10.2781 -0.3185
-8.8420 10.2886 -0.2651
-8.8414 10.2879 -0.2920
-8.7972 10.2781 -0.2914
-8.8914 10.3028 -0.2118
-8.8913 10.3026 -0.2390
-8.8911 10.3024 -0.1848
-8.8431 10.2899 -0.2381
];

plane3 = [
-8.7668 10.3218 -0.4267
-8.8744 10.3263 -0.4560
-8.7807 10.3382 -0.4543
-8.8118 10.3373 -0.4551
-8.8147 10.2984 -0.4003
-8.8596 10.3091 -0.4282
-8.8263 10.3121 -0.4275
-8.7545 10.3073 -0.3992
-8.7821 10.3024 -0.3996
-8.7932 10.3154 -0.4269
-8.7576 10.2737 -0.3445
-8.8032 10.2850 -0.3726
-8.7698 10.2880 -0.3719
-8.6894 10.3576 -0.4529
-8.7478 10.3424 -0.4536
-8.7209 10.3506 -0.4534
-8.7354 10.3278 -0.4262
-8.7101 10.3377 -0.4259
-8.7437 10.2946 -0.3717
];

% A-priori Coordinates of Apex
xapex = -8.7;
yapex = 10.3;
zapex = -0.3;

% Plane Parameters of face 1...4
nx1 = -0.211;
ny1 =  0.875;
nz1 = -0.436;
d1  = 10.971;

nx2 =  0.280;
ny2 =  0.959;
nz2 =  0.029;
d2  =  7.389;

nx3 = -0.152;
ny3 =  0.831;
nz3 =  0.536;
d3  =  9.678;

% Vector of unknows
X0 = [xapex, yapex, zapex,...
nx1, ny1, nz1, d1,...
nx2, ny2, nz2, d2,...
nx3, ny3, nz3, d3]';

Qll = eye(3*(length(plane1)+length(plane2)+length(plane3)));

plane10 = plane1;
plane20 = plane2;
plane30 = plane3;

maxIteration = 200;
dx = inf;
v = zeros(3*(length(plane1)+length(plane2)+length(plane3)),1);
while (abs(max(dx)) > sqrt(eps) && maxIteration >= 0)
maxIteration = maxIteration - 1;

% Jacobi
A = zeros(length(plane1)+length(plane2)+length(plane3), length(X0));
w = zeros(size(A,1), 1);
% Restriction
R = zeros(6, length(X0));
r = zeros(6, 1);
% Condition
B = zeros(size(A,1), size(A,1)*3);

A(1:length(plane1), 4:7)                                                                                             = [plane1 -ones(length(plane1), 1)];
A(length(plane1)+1:length(plane1)+length(plane2), 8:11)                                                              = [plane2 -ones(length(plane2), 1)];
A(length(plane1)+length(plane2)+1:length(plane1)+length(plane2)+length(plane3), 12:15)                               = [plane3 -ones(length(plane3), 1)];

w(1:length(plane1), 1)                                                                                           = plane1 * X0(4:6)   - X0(7);
w(length(plane1)+1:length(plane1)+length(plane2), 1)                                                             = plane2 * X0(8:10)  - X0(11);
w(length(plane1)+length(plane2)+1:length(plane1)+length(plane2)+length(plane3), 1)                               = plane3 * X0(12:14) - X0(15);

B(1:length(plane1), 1:3*length(plane1)) = kron(eye(length(plane1)), X0(4:6)');
B(length(plane1)+1:length(plane1)+length(plane2), 3*length(plane1)+1:3*(length(plane1)+length(plane2))) = kron(eye(length(plane2)), X0(8:10)');
B(length(plane1)+length(plane2)+1:length(plane1)+length(plane2)+length(plane3), 3*(length(plane1)+length(plane2))+1:3*(length(plane1)+length(plane2)+length(plane3))) = kron(eye(length(plane3)), X0(12:14)');

R(1, 4:6)   = 2.*X0(4:6);
R(2, 8:10)  = 2.*X0(8:10);
R(3, 12:14) = 2.*X0(12:14);

R(4, [1:3,  4:7])  = [X0(4:6)'   X0(1:3)' -1];
R(5, [1:3, 8:11])  = [X0(8:10)'  X0(1:3)' -1];
R(6, [1:3,12:15])  = [X0(12:14)' X0(1:3)' -1];

r(1,1) = X0(4:6)'  *X0(4:6)   - 1;
r(2,1) = X0(8:10)' *X0(8:10)  - 1;
r(3,1) = X0(12:14)'*X0(12:14) - 1;

r(4,1) = X0(4:6)'  *X0(1:3) - X0(7);
r(5,1) = X0(8:10)' *X0(1:3) - X0(11);
r(6,1) = X0(12:14)'*X0(1:3) - X0(15);

w = -B*v + w;

N = [   B*Qll*B'                          A           zeros(size(A,1),size(R,1))
A'                      zeros(size(A,2))             R'
zeros(size(R,1), size(A,1))      R           zeros(size(R,1),size(R,1))     ];

invN = -inv(N);
kxk = invN * [w; zeros(size(X0,1),1); r];

k1 = kxk(1:1:size(A,1));
dx = kxk(size(k1,1)+1:1:size(X0,1)+size(k1,1));
k2 = kxk(size(k1,1)+size(dx,1)+1:1:size(kxk,1));

X0 = X0+dx;
v = Qll*B'*k1;

for i=1:length(plane1)
plane1(i, :) = plane10(i, :) + v(3*(i-1)+1:3*(i-1)+3)';
end

for i=1:length(plane2)
plane2(i, :) = plane20(i, :) + v(3*length(plane1)+3*(i-1)+1 : 3*length(plane1)+3*(i-1)+3)';
end

for i=1:length(plane3)
plane3(i, :) = plane30(i, :) + v(3*(length(plane1)+length(plane2))+3*(i-1)+1 : 3*(length(plane1)+length(plane2))+3*(i-1)+3)';
end
end

omega = v'*inv(Qll)*v + 2*k1'*(B*v+A*dx+w) + 2*k2'*(R*dx+r);
Qxx = invN(size(A,1)+1:1:size(A,1)+size(X0,1), size(A,1)+1:1:size(A,1)+size(X0,1));
sigma = omega / (size(A,1)+size(R,1)-size(A,2));
Qxx = sigma .* Qxx;

disp([X0 sqrt(diag(Qxx))]);

plot3(plane1(:,1), plane1(:,2), plane1(:,3),'.b',...
plane2(:,1), plane2(:,2), plane2(:,3),'.r',...
plane3(:,1), plane3(:,2), plane3(:,3),'.g',...
X0(1), X0(2), X0(3), '*c');

regards
Micha

--
applied-geodesy.org - OpenSource Least-Squares Adjustment Software for Geodetic Sciences