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

Hi,

How to calculate σxyz of the pyramid apex.

I used a combined algorithm which estimates the 4 plane parameters and the apex in one pour, formulated as a Gauß-Helmert model with unknowns in the restrictions. The Octave (quick and dirty) code is shown below. The first three parameters represent the apex and its uncertainties.

         -8.74767399063614       0.00221451170576236
10.2640449004265        0.0011073336551347
-0.328875210651509       0.00218131552759802
-0.210956422930505       0.00890919575377389
0.873711747603854       0.00478721066144033
-0.438320852485237       0.00927348301769577
10.9573474841569        0.0779700931038434
0.275432828201474        0.0122221368827549
0.960863952854683       0.00351702329905086
0.0296179211527723       0.00860389157635439
7.44321356781815         0.144466915923314
-0.151048445180385        0.0117018439751209
0.828459948964864       0.00695058710419911
0.539294428090744        0.0104652274965152
9.64731210096934          0.11476352555406
-0.625323330824657       0.00737061968100696
0.775035753685848       0.00594490104169702
0.091051152848087       0.00677300352475538
13.3951819449872        0.0109245236511967

kind regards
Micha


%% 4-Faces-Pyramid
%
%   nx(i) * x(j,i) + ny(i) * y(j,i) + nz(i) * z(j,i) - d(i) = 0
% with i = 1 .. 4
%
%   ||n(i)|| == 1
%
%  and
%
%   n(i)' * Apex -d(i) == 0
%
%
% @version 1.01
% @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
];

plane4 = [
-8.6503 10.3510 -0.3985
-8.6315 10.3681 -0.3985
-8.6334 10.3705 -0.4520
-8.6954 10.3203 -0.3985
-8.6694 10.3337 -0.3982
-8.6744 10.3396 -0.4252
-8.6550 10.3567 -0.4252
-8.6332 10.3702 -0.4251
-8.7059 10.2929 -0.3174
-8.7298 10.2782 -0.3441
-8.7117 10.2996 -0.3713
-8.7069 10.2940 -0.3442
-8.6881 10.3115 -0.3442
-8.7283 10.2765 -0.3173
-8.6858 10.3089 -0.3174
-8.6907 10.3147 -0.3712
-8.6435 10.3430 -0.3173
-8.6684 10.3324 -0.3711
-8.6662 10.3298 -0.3440
-8.6463 10.3463 -0.3711
-8.6288 10.3650 -0.3712
-8.6454 10.3453 -0.3443
-8.6273 10.3631 -0.3443
-8.7063 10.2933 -0.2904
-8.6863 10.3095 -0.2636
-8.6875 10.3109 -0.2905
-8.6632 10.3263 -0.2636
-8.6636 10.3268 -0.2903
-8.6423 10.3415 -0.2903
-8.6206 10.3550 -0.2634
-8.6220 10.3567 -0.3174
-8.6235 10.3586 -0.2903
-8.6664 10.3301 -0.2369
-8.6401 10.3389 -0.2366
-8.6232 10.3582 -0.2099
-8.6199 10.3542 -0.2366
];

% 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;

nx4 = -0.627;
ny4 =  0.774;
nz4 =  0.091;
d4  = 13.396;

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

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

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

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

% Jacobi
A = zeros(length(plane1)+length(plane2)+length(plane3)+length(plane4), length(X0));
w = zeros(size(A,1), 1);
% Restriction
R = zeros(8, length(X0));
r = zeros(8, 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)];
A(length(plane1)+length(plane2)+length(plane3)+1:length(plane1)+length(plane2)+length(plane3)+length(plane4), 16:19) = [plane4 -ones(length(plane4), 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);
w(length(plane1)+length(plane2)+length(plane3)+1:length(plane1)+length(plane2)+length(plane3)+length(plane4), 1) = plane4 * X0(16:18) - X0(19);

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)');
B(length(plane1)+length(plane2)+length(plane3)+1:length(plane1)+length(plane2)+length(plane3)+length(plane4), 3*(length(plane1)+length(plane2)+length(plane3))+1:3*(length(plane1)+length(plane2)+length(plane3)+length(plane4))) = kron(eye(length(plane4)), X0(16:18)');

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, 16:18) = 2.*X0(16:18);

R(5, [1:3,  4:7])  = [X0(4:6)'   X0(1:3)' -1];
R(6, [1:3, 8:11])  = [X0(8:10)'  X0(1:3)' -1];
R(7, [1:3,12:15])  = [X0(12:14)' X0(1:3)' -1];
R(8, [1:3,16:19])  = [X0(16:18)' 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(16:18)'*X0(16:18) - 1;

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

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

for i=1:length(plane4)
plane4(i, :) = plane40(i, :) + v(3*(length(plane1)+length(plane2)+length(plane3))+3*(i-1)+1 : 3*(length(plane1)+length(plane2)+length(plane3))+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',...
plane4(:,1), plane4(:,2), plane4(:,3),'.m',...
X0(1), X0(2), X0(3), '*c');


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