- Email: [email protected]
Fitting Geometric Primitives With Least Squares
C H A P T E R
8 Fitting Geometric Primitives With Least Squares
8.1 INTRODUCTION There is a wide application of fitting points in 3D space to geometric primitive objects such as spheres, cylinders, planes, cones, and lines. Currently, point clouds acquired by laser scanners, range cameras, or from photographs are used to predict objects in outdoor or indoor environments such as road furniture extraction (poles, buildings, cars, trees, etc.) or for robotics navigation, autonomous driving, and other scene-understanding applications as shown in Fig. 8.1. There are different mathematical techniques to solve the least squares fitting problems either by conventional least squares adjustment or by the homogeneous least squares (Section 3.5) using SVD technique. Further, least squares adjustment methods can also vary as linear or nonlinear depending on the way of dealing with the mathematical model as will be shown. It is worth mentioning that the direct linear solutions are preferred over the nonlinear solutions, which need proper initial values to converge to optimal minimal.
Adjustment Models in 3D Geomatics and Computational Geophysics https://doi.org/10.1016/B978-0-12-817588-0.00008-8
243
# 2019 Elsevier Inc. All rights reserved.
244
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
FIG. 8.1 Fitting primitives from point clouds.
8.2 FITTING A PLANE Two techniques for fitting a plane are presented as follows: • Linear least squares approach A linear model of the plane is presented in Eq. (8.1) as follows: z ¼ ax + by + c
(8.1)
where a, b, and c are the plane unknown fitting parameters and are, in fact, the plane normal vector parameters or the cosine direction (Fig. 8.2). The system of equations can be solved if a minimum of three points are observed and a least squares adjustment is used when redundant observations exist, as will be shown in Example 8.1.
8.2 FITTING A PLANE
245
FIG. 8.2 Plane fitting.
In matrix form, the observation equation is: 2 32 3 2 3 x1 y1 1 a z1 4 ⋮ ⋱ ⋮ 54 b 5 ¼ 4 ⋮ 5,n 3 xn y n 1 zn c Δ ¼ F B |{z} |{z} |{z} n3
31
(8.2)
n1
Least squares solution with redundant observations is followed as: Δ ¼ (BtB)1BtF • Homogenous least squares using SVD The plane equation can be formulated as: ax + by + cz + d ¼ 0
(8.3)
where a, b, c, are the direction cosine of the normal vector of the plane and d represents the distance from a point to the plane. Here the plane can be solved by defining the following (Fig. 8.2): – An origin point (xo, yo, zo) on the plane computed as a mean point. – The direction cosine of the normal (a, b, c) with a unit length (a2 + b2 + c2 ¼ 1). The point on the plane should satisfy the following condition: aðx xo Þ + bðy yo Þ + cðz zo Þ ¼ 0
(8.4)
t
Then decompose the coefficient matrix B B using SVD as will be shown in Example 8.1 where: 2 3 x1 xo y1 yo z1 zo ⋮ ⋮ 5 B ¼4 ⋮ (8.5) |{z} xo y yo z zo x n n n n3
EXAMPLE 8.1 Given 25 points as listed in Table 8.1, and it is required to compute the best fit plane for them using both methods of linear and homogeneous least squares-based techniques.
246
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
TABLE 8.1 Plane fitting points
(1) Solution by the homogenous least squares using SVD (Section 3.5) xo ¼ 0:00104, yo ¼ 0:0058, zo ¼ 1:0018 t
Coefficient matrix B B is computed as in Eq. (8.5): 12.4230 0.1620 0.1539
0.1620 12.7380 0.0186
0.1539 0.0186 0.0116
SVD ¼ Bt B The computed eigenvector V matrix and normal vector parameters are:
247
8.2 FITTING A PLANE
(2) Solution by linear least squares Prepare the matrix of coefficients B and then compute the normal equation matrix as: 12:423 0:162 0:026 0:1799 t t B B ¼ 0:162 12:739 0:145 B F ¼ 0:1638 0:026 25:0440 0:145 25:000 0:012408 t 1 t Δ¼ B B B F ¼ 0:001617 1:0017 The results of both techniques are identical, and the best fit plane is plotted as shown in Fig. 8.3. For clarification, the last column of Table 8.1 is given to show the predicted Z values of the plane.
1.04 1.02 1 0.98 1
0.8
0.6
0.4
0.2
0
–0.2
–0.4
–0.6
–0.8
–1
1
0.8
0.6
0.4
0.2
0
–0.2
–0.4
–0.6
–0.8
–1
.04 .02 1 .98 1
1 0.8
0.6
0.4
0.2
0.2 0
–0.2
–0.4
–0.6
–0.8
–1
–1
–0.8
–0.6
–0.4
–0.2
0.4
0.6
0.8
0
1.04 1.02 1 0.98
1
0
–1 –1
–0.8
FIG. 8.3 Best fit plane of Example 8.1.
–0.6
–0.4
–0.2
0
0.2
0.4
0.6
0.8
1
248
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
8.3 FITTING A 3D LINE There are different possible formulations for the description of a 3D line [23,25,26], a popular method being a six-parameter representation as illustrated in Fig. 8.4.
t
d Z
s P
Y X
FIG. 8.4 Best fit 3D line.
– Three of the parameters represent a single point s(xo, yo, zo), on the line. – Three direction cosines unit vector d (a, b, c) along the line. Vector P describes an arbitrary point P (x, y, z) on the line, and point P is fixed on the line by the arbitrary scalar factor t. Therefore, the line parametric formulation can be given in Eq. (8.6) as: P ¼ s + td
(8.6)
Or x ¼ xo + t dð1;1Þ y ¼ yo + t dð1;2Þ z ¼ zo + t dð1; 3Þ The solution is based on applying the SVD to the matrix whose ith row is (x xo, y yo, z zo) as will be presented in Example 8.4.
EXAMPLE 8.2 Given 20 points in space in a linear form, and it is required to fit the best 3D line to them. After computing the mean of the points coordinates xo,yo, zo, the difference between every point coordinate and the mean value is computed as shown in the last three columns in Table 8.2. Then the SVD is applied to them, and the eigenvector matrix V is computed as:
249
8.3 FITTING A 3D LINE
TABLE 8.2 3D Line fitting points Point
X
Y
Z
x x0
y y0
z z0
1
0.248
1.160
0.555
3.886
6.902
2.890
2
1.260
2.321
1.203
4.418
5.710
3.567
3
0.728
3.513
0.525
4.030
5.279
2.953
4
1.116
3.944
1.139
3.595
4.914
1.257
5
1.550
4.309
2.835
1.697
3.679
2.332
6
3.449
5.544
1.761
2.050
2.698
0.749
7
3.096
6.525
3.343
0.588
2.016
1.542
8
4.558
7.207
2.550
0.738
0.811
0.060
9
4.407
8.412
4.152
0.057
0.506
0.090
10
5.202
8.717
4.003
0.120
0.737
0.235
11
5.266
9.959
4.327
1.095
1.100
0.665
12
6.241
10.323
4.757
1.050
2.848
0.748
13
6.195
12.071
4.841
2.214
1.930
1.769
14
7.360
11.153
5.862
2.176
4.084
1.440
15
7.322
13.307
5.532
3.202
4.372
1.820
16
8.348
13.595
5.913
2.876
4.752
2.631
17
8.022
13.975
6.724
4.414
6.450
2.471
18
9.560
15.673
6.563
3.784
6.746
4.075
19
8.930
15.969
8.167
4.909
7.560
3.004
20
10.054
16.783
7.096
4.898
8.063
3.538
Mean
5.1456
9.2228
4.0925
The best fit 3D line is plotted as shown in Fig. 8.5.
250
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
20
20
18
18
16 14
16
12
14
10
12
8
10
6
8
4 2
6
0 50
4 40
2
30
50
25
30
0 0
20
20
15
40 5
10
10 0
30 10
5
15
0
20 20
25
10 30
0
FIG. 8.5 Best fit 3D line.
8.4 FITTING A SPHERE There are two methods to solve the problem of sphere best fit: nonlinear and linear methods with a minimum of four observed points. • Linear least squares adjustment method of the sphere best fitting 2
1 61 B ¼6 |{z} 4 ⋮ n4 1
x1 x2 ⋮ xn
y1 y2 ⋮ yn
2 2 2 2 3 3 3 2 2 x1 + y1 + z1 z1 r xc 2 yc 2 z c 2 6 x2 + y2 + z2 7 z2 7 6 7 2 2 2 7 2xc 7 F ¼6 X ¼4 5 7 |{z} 5 ⋮ |{z} 6 2yc 4 5 ⋮ n1 41 2zc zv x 2 + y2 + z 2 n
n
X ¼ Bt B
n
1
Bt F
xc ¼ X2 =2 yc ¼ X3 =2 zc ¼ X4 =2 r¼
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi X 1 + xc 2 + y c 2 + z c 2
(8.7)
8.4 FITTING A SPHERE
251
• Nonlinear least squares solution method of the sphere best fitting. The sphere equation can be formulated as: 2 2 2 x xoc + y yoc + z zoc R2 ¼ 0
(8.8)
where – xc, yc, and zc are the best fit coordinates of the sphere center. – R is the radius of the best fit sphere. – x, y, and z are the fitting points. The adjustment observation model is v + B Δ ¼ F, and the partial derivative values in the adjustment matrices of observation will be: 2
vx1 6 vy1 6 6 vz1 6 ⋮ 6 4v yn vzn
3 2
∂F1 ∂F1 7 6 ∂xc ∂yc 7 6 7 6 ⋮ ⋮ 7+6 7 6 5 4 ∂Fn ∂Fn ∂xc ∂yc
∂Fn ∂zc
3
2 3 2 3 δxc 2 o 2 o 2 o 2 7 x x x y x z R 1 1 1 c c c 76 δy 7 6 7 76 c 7 6 7 ⋮ ⋮ ⋮ ⋮ 7¼4 76 5 74 δzc 5 2 o 2 o 2 o 2 ∂Fn 5 x x x y x z R n n n c c c δR ∂R
∂F1 ∂F1 ∂zc ∂R ⋮ ⋮
(8.8)
where the partial derivatives using the first fitting point are: ∂F1 ¼ 2ðx1 xc o Þ ∂xc ∂F1 ¼ 2ðy1 yc o Þ ∂yc ∂F1 ¼ 2ðz1 zc o Þ ∂zc ∂F1 ¼ 2Ro ∂R
EXAMPLE 8.3 Given • 3D points as follows (Fig. 8.6 and Table 8.3): • Initial values of sphere parameters are: xc o ¼ 0,yc o ¼ 2,zc o ¼ 5,R ¼ 10.
Required: • Find the best fit sphere using the linear and nonlinear least squares adjustment methods using the given fitting points.
252
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
12 10 8 6 4 2 0 –2 –10
–4 –5
–6
0
–5 0
5
5 10
FIG. 8.6 Sphere fit [27].
TABLE 8.3 Sphere fitting points
10
8.4 FITTING A SPHERE
(1) Nonlinear LS adjustment Iteration 1
Iteration 2
Iteration 3
The final adjusted best fit sphere parameters are: Center ¼0.95657, 2.0361, 3.0057 and R ¼ 7.2193.
253
254
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
(2) Linear LS solution B 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000
10.000 70.723 31.660 26.043
70.723 506.590 215.660 186.980
8.050 5.845 7.785 7.154 8.065 6.765 7.943 6.595 6.425 6.097
N 31.660 215.660 188.180 86.204
0.789 6.655 2.277 5.225 2.670 3.943 0.122 6.550 6.149 2.719
26.043 186.980 86.204 106.250
T 801.02 5660.50 2900.90 2337.70
2.663 0.336 0.734 5.001 1.987 6.783 3.425 3.112 0.751 1.251
F 72.515 78.564 66.326 103.490 76.115 107.320 74.840 96.076 79.654 46.124
! X¼ N-1T ¼
38.024 1.9131 4.0721 6.0114
Adjusted parameters are: xc ¼ 0.95657 yc ¼ 2.0361 zc ¼ 3.0057 R¼ 7.2193
MATLAB code %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %%%%%%%%%%%%%%%%%%%%%% chapter 8 - example 8.3 %%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc,clear close all %%%%%%%%%%%%%%%%% initial values xo=0; yo=2; zo=5; r=10; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % observed points P=[ 8.0498 0.7892 2.6632 5.8447 6.6551 7.7848 2.2768
0.3359 0.7343
7.1542 5.2246
5.0008
8.4 FITTING A SPHERE
8.0647 2.6698
1.9869
6.7650 3.9429
6.7827
7.9432 0.1222
3.4250
6.5949 6.5498
3.1118
6.4254 6.1485
0.7514
255
6.0966 -2.7186 1.2508]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%% LS method %%%%%%%%%%%%%%%%%%%%%%%%%%%% [Center,Radius,DD] = sphereFit_18(P, xo, yo, zo, r); disp(’ adjusted values’) Center,Radius %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure plot3(P(:,1),P(:,2),P(:,3),’ro’,’markersize’,8,’markerfacecolor’,’k’) hold on; [Base_X,Base_Y,Base_Z] = sphere(20); surf(Radius*Base_X+Center(1), Radius*Base_Y+Center(2),... Radius*Base_Z+Center(3),’faceAlpha’,0.3,’Facecolor’,’c’) view([45,28]) grid on axis image legend({’Data Points’,’Best Fit Sphere’,’LSE Sphere’},’location’,’W’) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%% figure d1=DD(1:4,:); plot((1:size(d1,2)), d1(1,:),’-ro’,’Markerfacecolor’,’r’,’LineWidth’,2),hold on plot((1:size(d1,2)), d1(2,:),’-b^’,’Markerfacecolor’,’b’,’LineWidth’,2),hold on plot((1:size(d1,2)), d1(3,:),’-ms’,’Markerfacecolor’,’k’,’LineWidth’,2),hold on plot((1:size(d1,2)), d1(4,:),’-gs’,’Markerfacecolor’,’m’,’LineWidth’,2),hold on xlabel(’Iterations’) ylabel(’Corrections [m]’) grid on axis tight legend(’xo correction’,’yo correction’,’zo correction’,’Radius correction’) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [Center,Radius,DD] = sphereFit(X,xo,yo,zo,R) DD=[0;0;0;0]; D=[1;1;1;1];vv=0; while (max(abs(D)))>.00001 vv=vv+1; for i=1:size(X,1) S=sqrt((X(i,1)-xo)^2+(X(i,2)-yo)^2+(X(i,3)-zo)^2); B(i,1)=(X(i,1)-xo)/S;B(i,2)=(X(i,2)-yo)/S; B(i,3)=(X(i,3)-zo)/S;B(i,4)=1; f(i,1)=-R+S ; end
256
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
%%%%%%%%%%5 least square D= (B’*B)\(B’* f ); xo=xo+D(1,1); yo=yo+D(2,1); zo=zo+D(3,1); Center=[xo,yo,zo]; R=R+D(4,1) ; DD=[DD,D]; end DD(:,1)=[] Radius=R;
8.5 FITTING A CIRCLE IN 3D SPACE Fitting a circle in 3D space (Fig. 8.7) needs a multistep solution approach as follows [28]: (1) Compute the best fit least squares plane of the data as explained previously. (2) Rotate the points such that the least squares plane is the XY plane. Z
X
Y
FIG. 8.7 3D circle fit.
(3) Rotate data points onto the XY plane. (4) Compute the 2D circle to fit in the XY plane. (5) Rotate back to the original orientation in 3D space for plotting.
8.5.1 Fitting a 2D Circle Because the best fit of a plane is explained in Section 8.2, we will start with the fourth step by explaining the least squares adjustment of a circle best fit in a 2D space as follows (Fig. 8.8): Given – XY points, i ¼ 1 : n
8.5 FITTING A CIRCLE IN 3D SPACE
257
FIG. 8.8 2D circle fit.
Required – To compute the adjusted radius r, and circle center xc, yc Circle equation can be formulated as: ðx xc Þ2 + ðy yc Þ2 r2 ¼ 0
(8.9)
(A) Nonlinear LS adjustment approach The observation equation will be as follows in the form v + B Δ ¼ F: 2
vx1 6 vy1 6 6 vx2 6 ⋮ 6 4v yn vzn
3
2 ∂F1 7 7 6 ∂xc 7 6 7+6 ⋮ 7 4 ∂Fn 5 ∂xc
∂F1 ∂yc ⋮ ∂Fn ∂yc
3 3 ∂F1 2 3 2 2 o 2 o 2 r x x x y 1 1 c c δx c ∂r 7 6 7 7 ⋮ ⋮ ⋮ ⋮7 ⋮ 74 δyc 5 ¼ 6 4 5 ∂Fn 5 δr 2 o 2 o 2 r x n xc x n yc ∂r
The partial derivatives of the function to the unknowns and observations will be as follows: ∂F1 ¼ 2 x xoc ∂xc ∂F1 ¼ 2 y yoc ∂yc ∂F1 ¼ 2ro ∂R (B) Linear LS adjustment approach The circle from Eq. (8.9) is repeated here for derivation:
258
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
ðx xc Þ2 + ðy yc Þ2 r2 ¼ 0 ð2xc Þx + ð2yc Þy + r2 x2c y2c ¼ x2 + y2
(8.10)
c0x + c1y + c2 ¼ x2 + y2
(8.11)
Or
where c0, c1, and c2 are the unknown parameters. Then we will have a linear observation system of the fitting equation as v + BΔ ¼ F: 2 3
3 32 3 2 y1 1 c0 x21 + y21 4 ⋮ 5 ¼ 4 ⋮ ⋮ ⋮ 5 4 c1 5 4 ⋮ 5 x n yn 1 c2 vn x2n + y2n |fflfflfflffl{zfflfflfflffl} |fflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflffl} |ffl{zffl} |fflfflfflfflffl{zfflfflfflfflffl} v1
2
x1
Vn∗1
B n ∗3
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c0 c1 Then xc ¼ , yc ¼ and r ¼ c2 + x2c + y2c 2 2
Δn∗1
(8.12)
Fn ∗1
8.5.2 Rodrigues Rotation Formula To apply the second step of rotating the points to the best fit plane of points, we can utilize the Rodrigues rotation formula to rotate 3D points and get their XY coordinates in the coordinate system of the plane. This is simply a problem of rotating from normal vector n1 to normal n2 as shown in Fig. 8.9. To compute the rotation matrix, we will apply the following steps: (1) Find axis and angle of rotation using cross product and dot product, respectively. The axis of rotation k is a cross product () between plane normal n1 and the normal of the new XY
FIG. 8.9 Rotating from normal vector n1 to normal n2.
coordinates. Thus, n2 ¼ (0, 0, 1)t and k ¼ n1 n2. ! ! 1 n 1:n 2 . Further, θ ¼ cos k!n 1k:k!n 2k (2) Find the rotation matrix M using exponential map: M ¼ I3∗3 + k^sin ðθÞ + k^2 ð1 cos θÞ
(8.13)
259
8.5 FITTING A CIRCLE IN 3D SPACE
3 0 kð3Þ kð2Þ – Skew – symmetric matrix k^ ¼ 4 kð3Þ 0 kð1Þ 5 kð2Þ kð1Þ 0 where
2
– k ¼ (n1 n2), then normalized. The third step after rotation of points is to find the best fit circle Cpoints in 2D space (xc, yc, r). Finally rotate back to 3D space by taking n2 as the plane normal and n1 ¼ (0, 0, 1)t. C ¼ Cpoints ½xc, yc, 0
(8.14)
C ¼ C Mt + Points_mean
(8.15)
A python code to solve this problem is also published in [28].
EXAMPLE 8.4 Given the following set of 3D points (P) in Table 8.4, it is required to find the best fit circle in 3D space.
Solution (1) Find the best fit plane using the methods described in Section 8.2. The cosine direction normal is: A¼ 0.000 B¼0.414 C¼ 1.082 (2) Rotate the plane normal vector into XY plane, which should have a normal vector of [0 0 1]t. – Compute the cross-product vector k (n1 n2) as:
0.41433
-0.00012
0
– Then normalize as w ¼ w/ norm (w):
1 – Skew-symmetric matrix:
-0.00028
0
260
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
2
3 2 3 0 kð3Þ kð2Þ 0 0 :00028 k^ ¼ 4 kð3Þ 0 kð1Þ 5 ¼ 4 0 0 1 5 kð2Þ kð1Þ 0 0:00028 1 0
– Dot product n1, n2 produce θ as 0.36564 rad. – Using Rodrigues’ formula to rotate to XY plane:
M¼
1 1.87E 05 0.000101
1.87E 05 0.9339 0.35755
0.0001 0.35755 0.9339
– Apply the rotation to the points in Table 8.4 as P_rot ¼ P ∗ M as illustrated in Fig. 8.10 in red dots. – Apply the LS adjustment to find the best fit parameters of the XY circle as shown in
TABLE 8.4
3D circle fitting points
261
8.5 FITTING A CIRCLE IN 3D SPACE
1.5
1.8 1.6
1
1.4 1.2
0.5
1 0.8
0 2.5 1.5
2 1.5
2.5 1
2 1.5
0.5
1
0
0.5
–0.5
0
2.5 1
2 1.5
0.5 1
0
0.5 –0.5
–0.5
0 –0.5
FIG. 8.10 Least squares fitting of a 3D circle. xc ¼ 0:98506, yc ¼ 0:96453 and r ¼ 1:5095 Then apply the transformation back to 3D space by: Pcircle ¼ Pcircle 2D ðxc, yc, 0Þt Mt + Pmean where Pmean ¼ [0.99047, 0.5426, 1.3071]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% section 8.4 - fitting 3D circle %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc; clear ; close all P=[0.963 1.904 1.871 1.276 1.901 1.870 1.600
1.819
1.836
2.057
1.491
1.700
2.392
1.266
1.607
2.435
0.937
1.471
2.627
0.380
1.240
2.388
0.056
1.106
2.129 1.897
-0.102 -0.622
1.040 0.825
1.281
-0.851
0.730
0.955
-0.920
0.701
0.734
-0.771
0.763
0.466
-0.761
0.767
-0.165
-0.396
0.918
262
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
-0.394
-0.067
1.054
-0.377
0.166
1.151
-0.496
0.659
1.355
-0.519
0.987
1.491
-0.291
1.250
1.600
-0.041
1.475
1.694
0.087 0.630
1.648 1.962
1.765 1.895];
xo=mean(P(:,1));yo=mean(P(:,2));zo=mean(P(:,3)) ; x=P(:,1);y=P(:,2);z=P(:,3); format short g for i=1:size(x,1) F(i,1)= z(i,1) ; bb(i,3)=1;bb(i,1)=x(i,1);bb(i,2)=y(i,1); end Delta=inv(bb’*bb)*bb’* F; a= Delta(1); b= Delta(2); c= Delta(3); v1=[a;b;c];v2=[0;0;1]; % % rotation vector w=cross(v1,v2); w=w/norm(w); w_hat=[0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; cos_tht=v1’*v2/norm(v1)/norm(v2); tht=acos(cos_tht); %% rotation matrix using Rodrigues formula M=eye(size(v1,1))+w_hat*sin(tht)+w_hat^2*(1-cos(tht)); Prot= P*M X=Prot(:,1);Y=Prot(:,2); for i=1:size(x,1) F(i,1)= X(i,1)^2+Y(i,1)^2; B(i,1)=X(i,1);B(i,2)=Y(i,1);B(i,3)=1; end u=inv(B’*B)*B’*F; xo=u(1,1)/2 yo=u(2,1)/2 r=sqrt(u(3,1)+xo^2+yo^2) angle=linspace(0,2*pi,500); xx=xo+r*cos(angle); yy=yo+r*sin(angle); R_comp=sqrt((X(:,1)-xo).^2+(Y(:,1)-yo).^2) ; resid = R_comp-r; figure(1)
8.6 FITTING A CYLINDER
263
bar( resid) ylabel(’RESIDUALS OF THE OBSERVED CIRCUMFERENCE POINTS [m]’) xlabel(’No. OF OBSERVED CIRCUMFERENCE POINTS ’) grid on axis square %%%%%%%%%%%%%% back rotation figure(2) Pc=[xx’-xo,yy’-yo,zeros(size(xx,2),1)];Pm=mean(P) %% M1=fcn_RotationFromTwoVectors(v1, v2) T= Pm-[xo,yo,0] for i=1:size(Pc,1) PP(i,:)= Pc(i,:)*M1’ +Pm ; end plot3(PP(:,1),PP(:,2),PP(:,3),’-’,’Linewidth’,2);hold on plot3(P(:,1),P(:,2),P(:,3),’r.’, ’markersize’,12);hold on axis image grid on %%%%%%%%%%%%%%%%% max resid, min resid, mean resid %%%%%%%%%%%%%%%%%%5%%%% result=[max(resid),min(resid),mean(resid)] MSE=sqrt(sum(resid.^2)/size(resid,1))%% mean squared errors
8.6 FITTING A CYLINDER There are different methods of least squares fitting of a cylindrical shape either linear- or nonlinear-based solutions [29,30,31]. Normally seven parameters are used to define a cylinder as (Fig. 8.12): • Point of origin lying on the cylinder axis and define as xo, yo, zo. • Cosine direction normal a, b, c of the cylinder axis. • Radius of the cylinder R. In this section, we will introduce the author-modified approach in computing the unknowns parameters of xo, yo, zo, R and normal cosine direction [a, b, c] of the cylinder axis using least squares adjustment that complies with our book context as follows: – The first derivation step is to apply the cross product between the vector derived from the coordinates differences and the cosine direction as: 2 3 2 3 a x xo E ¼ 4 b 5 4 y yo 5 ! d ¼ kEk c z zo
(8.16)
264
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
where refers to cross product and k k refers to norm. Therefore, the objective function is to minimize the difference between the distances from the fitting points to the cylinder axis and its radius R as illustrated in Fig. 8.11.
FIG. 8.11
Or
Fitting of a cylinder defined by seven parameters.
X F ¼ min : ðdi RÞ2
The observation equation v ¼ B Δ F can be formed as follows: 2 3 2 3 Δa ∂F1 ∂F1 ∂F1 ∂F1 ∂F1 ∂F1 ∂F1 6 Δb 7 2 3 2 3 7 v1 F1 6 ∂a ∂b ∂c ∂R ∂xo ∂yo ∂zo 7 6 6 7 7 6 Δc 7 7 6 v2 7 6 6 ΔR 7 ⋮ ⋮ ⋮ 7 ⋮ ⋮ ⋮ 4 ⋮ 5 ¼6 4 F⋮2 5 6 ⋮ 76 6 7 4 ∂Fn ∂Fn ∂Fn ∂Fn ∂Fn ∂Fn ∂Fn 5 6 Δxo 7 vn Fn 4 Δyo 5 |fflffl{zfflffl} |fflffl{zfflffl} ∂a ∂b ∂c ∂R ∂xo ∂yo ∂zo Vn1 Fn1 |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} Δzo |fflfflffl{zfflfflffl} Δ71 Bn7 where the partial derivatives are given as: ∂ jvj signðvÞðz zoÞ jwj signðwÞðy yoÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂a ðw2 + v2 + u2 Þ ∂ juj signðuÞðz zoÞ jwj signðwÞðx xoÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂b ðw2 + v2 + u2 Þ ∂ juj signðuÞðy yoÞ jvj signðvÞðx xoÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂c ð w 2 + v2 + u 2 Þ ∂ c jvj signðvÞ b jwj signðwÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂xo ðw2 + v2 + u2 Þ ∂ c juj signðuÞ a jwj signðwÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂yo ðw2 + v2 + u2 Þ ∂ b juj signðuÞ a jvj signðvÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂zo ð w 2 + v2 + u 2 Þ ∂ ¼ 1 ∂R
(8.17)
(8.18)
8.6 FITTING A CYLINDER
265
and 9 w ¼ bðx xoÞ aðy yoÞ = v ¼ aðz zoÞ cðx xoÞ ; u ¼ cðy yoÞ bðz zoÞ
(8.19)
j j refers to the absolute value. If the value is > 0 then Sign return 1 If the value is < 0 then Sign return 1 Further, to avoid the rank deficiency in the B matrix, two constraint equations (Chapter 6) are added to system as follows: First constraint The cosine direction vector a, b, and c should be normalized as (a2 + b2 + c2 ¼ 1). Therefore, the constraint equation C1 will be:
C1 ¼ ½ 2a 2b 2c 0 0 0 0 , g1 ¼ 1 a2 b2 c2
(8.20)
Second constraint The second constraint equation which should be added is based on the dot product between the cosine direction a, b, and c of the cylinder axis and the origin center point to be 0 or: a:xo + b:yo + c:zo ¼ 0
(8.21)
C2 ¼ ½ xo yo zo a b c 0 , g2 ¼ ½a:xo + b:yo + c:zo
(8.22)
Therefore:
Accordingly, the Helmert system is used to solve the constrained problem as presented in Chapter 6 of Eq. (6.21) as follows:
N Ct C 0
t g Δ C1 ¼ where C ¼ , g ¼ 1 , N ¼ Bt B, and t ¼ Bt F g g2 C2 kc
266
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
EXAMPLE 8.5 Given The following cylindrical distributed points are given in [m] in Table 8.5 as: TABLE 8.5 Cylinder fitting points
• The initial values of the axis normal vector and the radius will be given randomly as: a ¼ 1,b ¼ 1, c ¼ 1,R ¼ 1 • xo, yo, and zo are computed from the average of the point coordinates as: 1.9081, 5.1363, 0.0106.
Required Find the seven parameters of the best fitting cylinder.
Solution The constrained adjustment can be solved by composing the Helmert system of equations as discussed in Chapter 6.
First iteration
8.6 FITTING A CYLINDER
Accordingly, the Helmert system of equations is prepared as:
Second iteration
And directly to the sixth iteration:
The cylinder can then be plotted using the computed adjusted seven parameters. a ¼ 0.0299 b ¼ -0.0070 c ¼ 0.9995 R ¼ 1.5371 xo ¼ 1.9358 yo ¼ 5.1271 zo ¼ -0.0218
267
268
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
Fig. 8.12 illustrates the convergence of corrections to zero value during the least squares adjustment of nine iterations. Fig. 8.13 shows the best fit cylinder of the points listed in Table 8.5. Convergence of cosine direction normal a b c
0.2 Corrections
0
−0.2
−0.4 −0.6 −0.8
Corrections
1
2
3
4
3
4
5 6 Number of iterations Convergence of cylinder radius
7
8
9
0.2 0
−0.2 1
2
5 Number of iterations
6
7
8
9
Convergence of cylinder center 1
xo yo
Corrections
zo
0.5
0
−0.5 1
2
3
4
5
6
7
8
9
Number of iterations
FIG. 8.12
The adjustment convergence to optimal minimum.
6
6
6 4 4
4 2
2
2
6.5
0 0
6
–2
5.5
0 –2
–2
–4
5
–4
4.5
–6
–4
–6 4
6 –6
3 2
5 6 5 4
FIG. 8.13
1
2
3
4
Best fit cylinder.
1
2
3
1 6
5
4
3.5 0.5
1
1.5
2
2.5
3
3.5
8.6 FITTING A CYLINDER
MATLAB code %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%% chapter 8 - example 8.5 %%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc,clear close all %%%%%%%%% given points %%%%%%%%%%%%%%%%%%5 P=[ 1.73 6.41 -6.04 3.38
4.98
-5.94
1.93
3.64
-5.88
0.25
5.69
-6.52
1.26
6.84
-2.19
3.56
4.73
-2.55
1.57
3.88
-1.85
0.07 2.30
5.37 6.54
-1.89 2.19
3.40
4.68
1.74
1.82
3.63
2.06
0.59
5.37
2.34
2.31
6.65
6.71
3.49
4.73
6.13
2.02
3.40
5.58
0.85 5.64 5.94]; x= P(:,1);y= P(:,2);z= P(:,3); P=[x,y,z]; %%%%%%%%%%%%%%%%%%%%5 initial values a=1; b=1;c=1; xo=mean(P(:,1));yo=mean(P(:,2));zo=mean(P(:,3)) ;r=1; format short g ini= [a;b;c;xo;yo;zo;r] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 D=[1;1;1;1;1;1];vv=0; DD=[0 0 0 0 0 0 0]; while vv<7 vv=vv+1; x=P(:,1);y=P(:,2);z=P(:,3); for i=1:size(x,1) u=c*(y(i,1) - yo) - b*(z(i,1) - zo); v=a*(z(i,1) - zo) - c*(x(i,1) - xo); w=b*(x(i,1) - xo) - a*(y(i,1) - yo); d =(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2) - r;
269
270
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
fa(i,1) =-(2*abs(w)*sign(w)*(y(i,1) - yo) + 2*abs(-v)*sign(-v)*(z(i,1) - zo))/(2* (abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fb(i,1) =(2*abs(w)*sign(w)*(x(i,1) - xo) - 2*abs(u)*sign(u)*(z(i,1) - zo))/(2*(abs (w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fc(i,1) =(2*abs(-v)*sign(-v)*(x(i,1) - xo) + 2*abs(u)*sign(u)*(y(i,1) - yo))/(2* (abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fxo(i,1) =-(2*b*abs(w)*sign(w) + 2*c*abs(-v)*sign(-v))/(2*(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fyo(i,1) =(2*a*abs(w)*sign(w) - 2*c*abs(u)*sign(u))/(2*(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fzo(i,1) =(2*a*abs(-v)*sign(-v) + 2*b*abs(u)*sign(u))/(2*(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fr(i,1) =-1; F(i,1)= - d; end B=[ fa(:,1),fb(:,1),fc(:,1),fxo(:,1),fyo(:,1),fzo(:,1),fr(:,1) ]; %%%%%%%%%% 1st constraints %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% C1=[2*a 2*b 2*c 0 0 0 0 ]; g1=[1-a^2-b^2-c^2 ]; %##2nd constraint dot[xo;yo;zo][a,b,c]=0 a1=[xo;yo;zo]; a2=[a;b;c]; C2=[xo,yo,zo,a,b,c,0]; C=[C1;C2]; g2=-(xo*a+yo*b+zo*c); g=[g1;g2]; %%%%%%%%%%%%%%% Helmert method %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% N=[[B’*B; C][C’;zeros(2,2)]]; T=[B’* F;g] ; dt= inv(N)*T; a=a+dt(1); b=b+dt(2); c=c+dt(3); xo=xo+dt(4); yo=yo+dt(5); zo=zo+dt(6); r=r+dt(7); DD=[DD;dt(1:7)’]; end disp(’Adjusted cylinder parameters’) [a;b;c;xo;yo;zo;r] %%%%%%%%%%%%%% statistics dt(end-1:end)=[]; v=B*dt-F; sigma=(v’*v)/(size(B,1)-size(B,2)+1); vcov=sigma*pinv(N);vcov(end-1:end,:)=[];vcov(:,end-1:end)=[]; disp(’std. dev.’) sqrt(diag(vcov)) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
8.6 FITTING A CYLINDER
DD(1,:)=[];D=DD; figure(10) subplot(3,1,1) plot((1:size(D,1)),D(:,1),’-ro’,’Markerfacecolor’,’r’,’LineWidth’,3),hold on plot((1:size(D,1)),D(:,2),’-b^’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on plot((1:size(D,1)),D(:,3),’-ms’,’Markerfacecolor’,’k’,’LineWidth’,3),hold on title(’CONVERGENCE OF COSINE DIRECTION NORMAL’) xlabel(’NUMBER OF ITERATIONS’) ylabel(’CORRECTIONS’) grid on; legend(’a’,’b’,’c’) axis image subplot(3,1,2) plot((1:size(D,1)), D(:,4),’–k’,’Markerfacecolor’,’k’,’LineWidth’,3),hold on title(’CONVERGENCE OF CYLINDER RADIUS’) xlabel(’NUMBER OF ITERATIONS’) ylabel(’CORRECTIONS’) grid on axis image subplot(3,1,3) plot((1:size(D,1)), D(:,5),’-r’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on plot((1:size(D,1)), D(:,6),’-g’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on plot((1:size(D,1)), D(:,7),’-k’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on title(’CONVERGENCE OF CYLINDER CENTER’) xlabel(’NUMBER OF ITERATIONS’) ylabel(’CORRECTIONS’) legend(’xo’,’yo’,’zo’) grid on axis image
271
8 Fitting Geometric Primitives With Least Squares
8.1 INTRODUCTION There is a wide application of fitting points in 3D space to geometric primitive objects such as spheres, cylinders, planes, cones, and lines. Currently, point clouds acquired by laser scanners, range cameras, or from photographs are used to predict objects in outdoor or indoor environments such as road furniture extraction (poles, buildings, cars, trees, etc.) or for robotics navigation, autonomous driving, and other scene-understanding applications as shown in Fig. 8.1. There are different mathematical techniques to solve the least squares fitting problems either by conventional least squares adjustment or by the homogeneous least squares (Section 3.5) using SVD technique. Further, least squares adjustment methods can also vary as linear or nonlinear depending on the way of dealing with the mathematical model as will be shown. It is worth mentioning that the direct linear solutions are preferred over the nonlinear solutions, which need proper initial values to converge to optimal minimal.
Adjustment Models in 3D Geomatics and Computational Geophysics https://doi.org/10.1016/B978-0-12-817588-0.00008-8
243
# 2019 Elsevier Inc. All rights reserved.
244
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
FIG. 8.1 Fitting primitives from point clouds.
8.2 FITTING A PLANE Two techniques for fitting a plane are presented as follows: • Linear least squares approach A linear model of the plane is presented in Eq. (8.1) as follows: z ¼ ax + by + c
(8.1)
where a, b, and c are the plane unknown fitting parameters and are, in fact, the plane normal vector parameters or the cosine direction (Fig. 8.2). The system of equations can be solved if a minimum of three points are observed and a least squares adjustment is used when redundant observations exist, as will be shown in Example 8.1.
8.2 FITTING A PLANE
245
FIG. 8.2 Plane fitting.
In matrix form, the observation equation is: 2 32 3 2 3 x1 y1 1 a z1 4 ⋮ ⋱ ⋮ 54 b 5 ¼ 4 ⋮ 5,n 3 xn y n 1 zn c Δ ¼ F B |{z} |{z} |{z} n3
31
(8.2)
n1
Least squares solution with redundant observations is followed as: Δ ¼ (BtB)1BtF • Homogenous least squares using SVD The plane equation can be formulated as: ax + by + cz + d ¼ 0
(8.3)
where a, b, c, are the direction cosine of the normal vector of the plane and d represents the distance from a point to the plane. Here the plane can be solved by defining the following (Fig. 8.2): – An origin point (xo, yo, zo) on the plane computed as a mean point. – The direction cosine of the normal (a, b, c) with a unit length (a2 + b2 + c2 ¼ 1). The point on the plane should satisfy the following condition: aðx xo Þ + bðy yo Þ + cðz zo Þ ¼ 0
(8.4)
t
Then decompose the coefficient matrix B B using SVD as will be shown in Example 8.1 where: 2 3 x1 xo y1 yo z1 zo ⋮ ⋮ 5 B ¼4 ⋮ (8.5) |{z} xo y yo z zo x n n n n3
EXAMPLE 8.1 Given 25 points as listed in Table 8.1, and it is required to compute the best fit plane for them using both methods of linear and homogeneous least squares-based techniques.
246
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
TABLE 8.1 Plane fitting points
(1) Solution by the homogenous least squares using SVD (Section 3.5) xo ¼ 0:00104, yo ¼ 0:0058, zo ¼ 1:0018 t
Coefficient matrix B B is computed as in Eq. (8.5): 12.4230 0.1620 0.1539
0.1620 12.7380 0.0186
0.1539 0.0186 0.0116
SVD ¼ Bt B The computed eigenvector V matrix and normal vector parameters are:
247
8.2 FITTING A PLANE
(2) Solution by linear least squares Prepare the matrix of coefficients B and then compute the normal equation matrix as: 12:423 0:162 0:026 0:1799 t t B B ¼ 0:162 12:739 0:145 B F ¼ 0:1638 0:026 25:0440 0:145 25:000 0:012408 t 1 t Δ¼ B B B F ¼ 0:001617 1:0017 The results of both techniques are identical, and the best fit plane is plotted as shown in Fig. 8.3. For clarification, the last column of Table 8.1 is given to show the predicted Z values of the plane.
1.04 1.02 1 0.98 1
0.8
0.6
0.4
0.2
0
–0.2
–0.4
–0.6
–0.8
–1
1
0.8
0.6
0.4
0.2
0
–0.2
–0.4
–0.6
–0.8
–1
.04 .02 1 .98 1
1 0.8
0.6
0.4
0.2
0.2 0
–0.2
–0.4
–0.6
–0.8
–1
–1
–0.8
–0.6
–0.4
–0.2
0.4
0.6
0.8
0
1.04 1.02 1 0.98
1
0
–1 –1
–0.8
FIG. 8.3 Best fit plane of Example 8.1.
–0.6
–0.4
–0.2
0
0.2
0.4
0.6
0.8
1
248
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
8.3 FITTING A 3D LINE There are different possible formulations for the description of a 3D line [23,25,26], a popular method being a six-parameter representation as illustrated in Fig. 8.4.
t
d Z
s P
Y X
FIG. 8.4 Best fit 3D line.
– Three of the parameters represent a single point s(xo, yo, zo), on the line. – Three direction cosines unit vector d (a, b, c) along the line. Vector P describes an arbitrary point P (x, y, z) on the line, and point P is fixed on the line by the arbitrary scalar factor t. Therefore, the line parametric formulation can be given in Eq. (8.6) as: P ¼ s + td
(8.6)
Or x ¼ xo + t dð1;1Þ y ¼ yo + t dð1;2Þ z ¼ zo + t dð1; 3Þ The solution is based on applying the SVD to the matrix whose ith row is (x xo, y yo, z zo) as will be presented in Example 8.4.
EXAMPLE 8.2 Given 20 points in space in a linear form, and it is required to fit the best 3D line to them. After computing the mean of the points coordinates xo,yo, zo, the difference between every point coordinate and the mean value is computed as shown in the last three columns in Table 8.2. Then the SVD is applied to them, and the eigenvector matrix V is computed as:
249
8.3 FITTING A 3D LINE
TABLE 8.2 3D Line fitting points Point
X
Y
Z
x x0
y y0
z z0
1
0.248
1.160
0.555
3.886
6.902
2.890
2
1.260
2.321
1.203
4.418
5.710
3.567
3
0.728
3.513
0.525
4.030
5.279
2.953
4
1.116
3.944
1.139
3.595
4.914
1.257
5
1.550
4.309
2.835
1.697
3.679
2.332
6
3.449
5.544
1.761
2.050
2.698
0.749
7
3.096
6.525
3.343
0.588
2.016
1.542
8
4.558
7.207
2.550
0.738
0.811
0.060
9
4.407
8.412
4.152
0.057
0.506
0.090
10
5.202
8.717
4.003
0.120
0.737
0.235
11
5.266
9.959
4.327
1.095
1.100
0.665
12
6.241
10.323
4.757
1.050
2.848
0.748
13
6.195
12.071
4.841
2.214
1.930
1.769
14
7.360
11.153
5.862
2.176
4.084
1.440
15
7.322
13.307
5.532
3.202
4.372
1.820
16
8.348
13.595
5.913
2.876
4.752
2.631
17
8.022
13.975
6.724
4.414
6.450
2.471
18
9.560
15.673
6.563
3.784
6.746
4.075
19
8.930
15.969
8.167
4.909
7.560
3.004
20
10.054
16.783
7.096
4.898
8.063
3.538
Mean
5.1456
9.2228
4.0925
The best fit 3D line is plotted as shown in Fig. 8.5.
250
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
20
20
18
18
16 14
16
12
14
10
12
8
10
6
8
4 2
6
0 50
4 40
2
30
50
25
30
0 0
20
20
15
40 5
10
10 0
30 10
5
15
0
20 20
25
10 30
0
FIG. 8.5 Best fit 3D line.
8.4 FITTING A SPHERE There are two methods to solve the problem of sphere best fit: nonlinear and linear methods with a minimum of four observed points. • Linear least squares adjustment method of the sphere best fitting 2
1 61 B ¼6 |{z} 4 ⋮ n4 1
x1 x2 ⋮ xn
y1 y2 ⋮ yn
2 2 2 2 3 3 3 2 2 x1 + y1 + z1 z1 r xc 2 yc 2 z c 2 6 x2 + y2 + z2 7 z2 7 6 7 2 2 2 7 2xc 7 F ¼6 X ¼4 5 7 |{z} 5 ⋮ |{z} 6 2yc 4 5 ⋮ n1 41 2zc zv x 2 + y2 + z 2 n
n
X ¼ Bt B
n
1
Bt F
xc ¼ X2 =2 yc ¼ X3 =2 zc ¼ X4 =2 r¼
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi X 1 + xc 2 + y c 2 + z c 2
(8.7)
8.4 FITTING A SPHERE
251
• Nonlinear least squares solution method of the sphere best fitting. The sphere equation can be formulated as: 2 2 2 x xoc + y yoc + z zoc R2 ¼ 0
(8.8)
where – xc, yc, and zc are the best fit coordinates of the sphere center. – R is the radius of the best fit sphere. – x, y, and z are the fitting points. The adjustment observation model is v + B Δ ¼ F, and the partial derivative values in the adjustment matrices of observation will be: 2
vx1 6 vy1 6 6 vz1 6 ⋮ 6 4v yn vzn
3 2
∂F1 ∂F1 7 6 ∂xc ∂yc 7 6 7 6 ⋮ ⋮ 7+6 7 6 5 4 ∂Fn ∂Fn ∂xc ∂yc
∂Fn ∂zc
3
2 3 2 3 δxc 2 o 2 o 2 o 2 7 x x x y x z R 1 1 1 c c c 76 δy 7 6 7 76 c 7 6 7 ⋮ ⋮ ⋮ ⋮ 7¼4 76 5 74 δzc 5 2 o 2 o 2 o 2 ∂Fn 5 x x x y x z R n n n c c c δR ∂R
∂F1 ∂F1 ∂zc ∂R ⋮ ⋮
(8.8)
where the partial derivatives using the first fitting point are: ∂F1 ¼ 2ðx1 xc o Þ ∂xc ∂F1 ¼ 2ðy1 yc o Þ ∂yc ∂F1 ¼ 2ðz1 zc o Þ ∂zc ∂F1 ¼ 2Ro ∂R
EXAMPLE 8.3 Given • 3D points as follows (Fig. 8.6 and Table 8.3): • Initial values of sphere parameters are: xc o ¼ 0,yc o ¼ 2,zc o ¼ 5,R ¼ 10.
Required: • Find the best fit sphere using the linear and nonlinear least squares adjustment methods using the given fitting points.
252
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
12 10 8 6 4 2 0 –2 –10
–4 –5
–6
0
–5 0
5
5 10
FIG. 8.6 Sphere fit [27].
TABLE 8.3 Sphere fitting points
10
8.4 FITTING A SPHERE
(1) Nonlinear LS adjustment Iteration 1
Iteration 2
Iteration 3
The final adjusted best fit sphere parameters are: Center ¼0.95657, 2.0361, 3.0057 and R ¼ 7.2193.
253
254
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
(2) Linear LS solution B 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000
10.000 70.723 31.660 26.043
70.723 506.590 215.660 186.980
8.050 5.845 7.785 7.154 8.065 6.765 7.943 6.595 6.425 6.097
N 31.660 215.660 188.180 86.204
0.789 6.655 2.277 5.225 2.670 3.943 0.122 6.550 6.149 2.719
26.043 186.980 86.204 106.250
T 801.02 5660.50 2900.90 2337.70
2.663 0.336 0.734 5.001 1.987 6.783 3.425 3.112 0.751 1.251
F 72.515 78.564 66.326 103.490 76.115 107.320 74.840 96.076 79.654 46.124
! X¼ N-1T ¼
38.024 1.9131 4.0721 6.0114
Adjusted parameters are: xc ¼ 0.95657 yc ¼ 2.0361 zc ¼ 3.0057 R¼ 7.2193
MATLAB code %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %%%%%%%%%%%%%%%%%%%%%% chapter 8 - example 8.3 %%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc,clear close all %%%%%%%%%%%%%%%%% initial values xo=0; yo=2; zo=5; r=10; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % observed points P=[ 8.0498 0.7892 2.6632 5.8447 6.6551 7.7848 2.2768
0.3359 0.7343
7.1542 5.2246
5.0008
8.4 FITTING A SPHERE
8.0647 2.6698
1.9869
6.7650 3.9429
6.7827
7.9432 0.1222
3.4250
6.5949 6.5498
3.1118
6.4254 6.1485
0.7514
255
6.0966 -2.7186 1.2508]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%% LS method %%%%%%%%%%%%%%%%%%%%%%%%%%%% [Center,Radius,DD] = sphereFit_18(P, xo, yo, zo, r); disp(’ adjusted values’) Center,Radius %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure plot3(P(:,1),P(:,2),P(:,3),’ro’,’markersize’,8,’markerfacecolor’,’k’) hold on; [Base_X,Base_Y,Base_Z] = sphere(20); surf(Radius*Base_X+Center(1), Radius*Base_Y+Center(2),... Radius*Base_Z+Center(3),’faceAlpha’,0.3,’Facecolor’,’c’) view([45,28]) grid on axis image legend({’Data Points’,’Best Fit Sphere’,’LSE Sphere’},’location’,’W’) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%% figure d1=DD(1:4,:); plot((1:size(d1,2)), d1(1,:),’-ro’,’Markerfacecolor’,’r’,’LineWidth’,2),hold on plot((1:size(d1,2)), d1(2,:),’-b^’,’Markerfacecolor’,’b’,’LineWidth’,2),hold on plot((1:size(d1,2)), d1(3,:),’-ms’,’Markerfacecolor’,’k’,’LineWidth’,2),hold on plot((1:size(d1,2)), d1(4,:),’-gs’,’Markerfacecolor’,’m’,’LineWidth’,2),hold on xlabel(’Iterations’) ylabel(’Corrections [m]’) grid on axis tight legend(’xo correction’,’yo correction’,’zo correction’,’Radius correction’) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [Center,Radius,DD] = sphereFit(X,xo,yo,zo,R) DD=[0;0;0;0]; D=[1;1;1;1];vv=0; while (max(abs(D)))>.00001 vv=vv+1; for i=1:size(X,1) S=sqrt((X(i,1)-xo)^2+(X(i,2)-yo)^2+(X(i,3)-zo)^2); B(i,1)=(X(i,1)-xo)/S;B(i,2)=(X(i,2)-yo)/S; B(i,3)=(X(i,3)-zo)/S;B(i,4)=1; f(i,1)=-R+S ; end
256
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
%%%%%%%%%%5 least square D= (B’*B)\(B’* f ); xo=xo+D(1,1); yo=yo+D(2,1); zo=zo+D(3,1); Center=[xo,yo,zo]; R=R+D(4,1) ; DD=[DD,D]; end DD(:,1)=[] Radius=R;
8.5 FITTING A CIRCLE IN 3D SPACE Fitting a circle in 3D space (Fig. 8.7) needs a multistep solution approach as follows [28]: (1) Compute the best fit least squares plane of the data as explained previously. (2) Rotate the points such that the least squares plane is the XY plane. Z
X
Y
FIG. 8.7 3D circle fit.
(3) Rotate data points onto the XY plane. (4) Compute the 2D circle to fit in the XY plane. (5) Rotate back to the original orientation in 3D space for plotting.
8.5.1 Fitting a 2D Circle Because the best fit of a plane is explained in Section 8.2, we will start with the fourth step by explaining the least squares adjustment of a circle best fit in a 2D space as follows (Fig. 8.8): Given – XY points, i ¼ 1 : n
8.5 FITTING A CIRCLE IN 3D SPACE
257
FIG. 8.8 2D circle fit.
Required – To compute the adjusted radius r, and circle center xc, yc Circle equation can be formulated as: ðx xc Þ2 + ðy yc Þ2 r2 ¼ 0
(8.9)
(A) Nonlinear LS adjustment approach The observation equation will be as follows in the form v + B Δ ¼ F: 2
vx1 6 vy1 6 6 vx2 6 ⋮ 6 4v yn vzn
3
2 ∂F1 7 7 6 ∂xc 7 6 7+6 ⋮ 7 4 ∂Fn 5 ∂xc
∂F1 ∂yc ⋮ ∂Fn ∂yc
3 3 ∂F1 2 3 2 2 o 2 o 2 r x x x y 1 1 c c δx c ∂r 7 6 7 7 ⋮ ⋮ ⋮ ⋮7 ⋮ 74 δyc 5 ¼ 6 4 5 ∂Fn 5 δr 2 o 2 o 2 r x n xc x n yc ∂r
The partial derivatives of the function to the unknowns and observations will be as follows: ∂F1 ¼ 2 x xoc ∂xc ∂F1 ¼ 2 y yoc ∂yc ∂F1 ¼ 2ro ∂R (B) Linear LS adjustment approach The circle from Eq. (8.9) is repeated here for derivation:
258
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
ðx xc Þ2 + ðy yc Þ2 r2 ¼ 0 ð2xc Þx + ð2yc Þy + r2 x2c y2c ¼ x2 + y2
(8.10)
c0x + c1y + c2 ¼ x2 + y2
(8.11)
Or
where c0, c1, and c2 are the unknown parameters. Then we will have a linear observation system of the fitting equation as v + BΔ ¼ F: 2 3
3 32 3 2 y1 1 c0 x21 + y21 4 ⋮ 5 ¼ 4 ⋮ ⋮ ⋮ 5 4 c1 5 4 ⋮ 5 x n yn 1 c2 vn x2n + y2n |fflfflfflffl{zfflfflfflffl} |fflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflffl} |ffl{zffl} |fflfflfflfflffl{zfflfflfflfflffl} v1
2
x1
Vn∗1
B n ∗3
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c0 c1 Then xc ¼ , yc ¼ and r ¼ c2 + x2c + y2c 2 2
Δn∗1
(8.12)
Fn ∗1
8.5.2 Rodrigues Rotation Formula To apply the second step of rotating the points to the best fit plane of points, we can utilize the Rodrigues rotation formula to rotate 3D points and get their XY coordinates in the coordinate system of the plane. This is simply a problem of rotating from normal vector n1 to normal n2 as shown in Fig. 8.9. To compute the rotation matrix, we will apply the following steps: (1) Find axis and angle of rotation using cross product and dot product, respectively. The axis of rotation k is a cross product () between plane normal n1 and the normal of the new XY
FIG. 8.9 Rotating from normal vector n1 to normal n2.
coordinates. Thus, n2 ¼ (0, 0, 1)t and k ¼ n1 n2. ! ! 1 n 1:n 2 . Further, θ ¼ cos k!n 1k:k!n 2k (2) Find the rotation matrix M using exponential map: M ¼ I3∗3 + k^sin ðθÞ + k^2 ð1 cos θÞ
(8.13)
259
8.5 FITTING A CIRCLE IN 3D SPACE
3 0 kð3Þ kð2Þ – Skew – symmetric matrix k^ ¼ 4 kð3Þ 0 kð1Þ 5 kð2Þ kð1Þ 0 where
2
– k ¼ (n1 n2), then normalized. The third step after rotation of points is to find the best fit circle Cpoints in 2D space (xc, yc, r). Finally rotate back to 3D space by taking n2 as the plane normal and n1 ¼ (0, 0, 1)t. C ¼ Cpoints ½xc, yc, 0
(8.14)
C ¼ C Mt + Points_mean
(8.15)
A python code to solve this problem is also published in [28].
EXAMPLE 8.4 Given the following set of 3D points (P) in Table 8.4, it is required to find the best fit circle in 3D space.
Solution (1) Find the best fit plane using the methods described in Section 8.2. The cosine direction normal is: A¼ 0.000 B¼0.414 C¼ 1.082 (2) Rotate the plane normal vector into XY plane, which should have a normal vector of [0 0 1]t. – Compute the cross-product vector k (n1 n2) as:
0.41433
-0.00012
0
– Then normalize as w ¼ w/ norm (w):
1 – Skew-symmetric matrix:
-0.00028
0
260
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
2
3 2 3 0 kð3Þ kð2Þ 0 0 :00028 k^ ¼ 4 kð3Þ 0 kð1Þ 5 ¼ 4 0 0 1 5 kð2Þ kð1Þ 0 0:00028 1 0
– Dot product n1, n2 produce θ as 0.36564 rad. – Using Rodrigues’ formula to rotate to XY plane:
M¼
1 1.87E 05 0.000101
1.87E 05 0.9339 0.35755
0.0001 0.35755 0.9339
– Apply the rotation to the points in Table 8.4 as P_rot ¼ P ∗ M as illustrated in Fig. 8.10 in red dots. – Apply the LS adjustment to find the best fit parameters of the XY circle as shown in
TABLE 8.4
3D circle fitting points
261
8.5 FITTING A CIRCLE IN 3D SPACE
1.5
1.8 1.6
1
1.4 1.2
0.5
1 0.8
0 2.5 1.5
2 1.5
2.5 1
2 1.5
0.5
1
0
0.5
–0.5
0
2.5 1
2 1.5
0.5 1
0
0.5 –0.5
–0.5
0 –0.5
FIG. 8.10 Least squares fitting of a 3D circle. xc ¼ 0:98506, yc ¼ 0:96453 and r ¼ 1:5095 Then apply the transformation back to 3D space by: Pcircle ¼ Pcircle 2D ðxc, yc, 0Þt Mt + Pmean where Pmean ¼ [0.99047, 0.5426, 1.3071]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% section 8.4 - fitting 3D circle %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc; clear ; close all P=[0.963 1.904 1.871 1.276 1.901 1.870 1.600
1.819
1.836
2.057
1.491
1.700
2.392
1.266
1.607
2.435
0.937
1.471
2.627
0.380
1.240
2.388
0.056
1.106
2.129 1.897
-0.102 -0.622
1.040 0.825
1.281
-0.851
0.730
0.955
-0.920
0.701
0.734
-0.771
0.763
0.466
-0.761
0.767
-0.165
-0.396
0.918
262
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
-0.394
-0.067
1.054
-0.377
0.166
1.151
-0.496
0.659
1.355
-0.519
0.987
1.491
-0.291
1.250
1.600
-0.041
1.475
1.694
0.087 0.630
1.648 1.962
1.765 1.895];
xo=mean(P(:,1));yo=mean(P(:,2));zo=mean(P(:,3)) ; x=P(:,1);y=P(:,2);z=P(:,3); format short g for i=1:size(x,1) F(i,1)= z(i,1) ; bb(i,3)=1;bb(i,1)=x(i,1);bb(i,2)=y(i,1); end Delta=inv(bb’*bb)*bb’* F; a= Delta(1); b= Delta(2); c= Delta(3); v1=[a;b;c];v2=[0;0;1]; % % rotation vector w=cross(v1,v2); w=w/norm(w); w_hat=[0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; cos_tht=v1’*v2/norm(v1)/norm(v2); tht=acos(cos_tht); %% rotation matrix using Rodrigues formula M=eye(size(v1,1))+w_hat*sin(tht)+w_hat^2*(1-cos(tht)); Prot= P*M X=Prot(:,1);Y=Prot(:,2); for i=1:size(x,1) F(i,1)= X(i,1)^2+Y(i,1)^2; B(i,1)=X(i,1);B(i,2)=Y(i,1);B(i,3)=1; end u=inv(B’*B)*B’*F; xo=u(1,1)/2 yo=u(2,1)/2 r=sqrt(u(3,1)+xo^2+yo^2) angle=linspace(0,2*pi,500); xx=xo+r*cos(angle); yy=yo+r*sin(angle); R_comp=sqrt((X(:,1)-xo).^2+(Y(:,1)-yo).^2) ; resid = R_comp-r; figure(1)
8.6 FITTING A CYLINDER
263
bar( resid) ylabel(’RESIDUALS OF THE OBSERVED CIRCUMFERENCE POINTS [m]’) xlabel(’No. OF OBSERVED CIRCUMFERENCE POINTS ’) grid on axis square %%%%%%%%%%%%%% back rotation figure(2) Pc=[xx’-xo,yy’-yo,zeros(size(xx,2),1)];Pm=mean(P) %% M1=fcn_RotationFromTwoVectors(v1, v2) T= Pm-[xo,yo,0] for i=1:size(Pc,1) PP(i,:)= Pc(i,:)*M1’ +Pm ; end plot3(PP(:,1),PP(:,2),PP(:,3),’-’,’Linewidth’,2);hold on plot3(P(:,1),P(:,2),P(:,3),’r.’, ’markersize’,12);hold on axis image grid on %%%%%%%%%%%%%%%%% max resid, min resid, mean resid %%%%%%%%%%%%%%%%%%5%%%% result=[max(resid),min(resid),mean(resid)] MSE=sqrt(sum(resid.^2)/size(resid,1))%% mean squared errors
8.6 FITTING A CYLINDER There are different methods of least squares fitting of a cylindrical shape either linear- or nonlinear-based solutions [29,30,31]. Normally seven parameters are used to define a cylinder as (Fig. 8.12): • Point of origin lying on the cylinder axis and define as xo, yo, zo. • Cosine direction normal a, b, c of the cylinder axis. • Radius of the cylinder R. In this section, we will introduce the author-modified approach in computing the unknowns parameters of xo, yo, zo, R and normal cosine direction [a, b, c] of the cylinder axis using least squares adjustment that complies with our book context as follows: – The first derivation step is to apply the cross product between the vector derived from the coordinates differences and the cosine direction as: 2 3 2 3 a x xo E ¼ 4 b 5 4 y yo 5 ! d ¼ kEk c z zo
(8.16)
264
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
where refers to cross product and k k refers to norm. Therefore, the objective function is to minimize the difference between the distances from the fitting points to the cylinder axis and its radius R as illustrated in Fig. 8.11.
FIG. 8.11
Or
Fitting of a cylinder defined by seven parameters.
X F ¼ min : ðdi RÞ2
The observation equation v ¼ B Δ F can be formed as follows: 2 3 2 3 Δa ∂F1 ∂F1 ∂F1 ∂F1 ∂F1 ∂F1 ∂F1 6 Δb 7 2 3 2 3 7 v1 F1 6 ∂a ∂b ∂c ∂R ∂xo ∂yo ∂zo 7 6 6 7 7 6 Δc 7 7 6 v2 7 6 6 ΔR 7 ⋮ ⋮ ⋮ 7 ⋮ ⋮ ⋮ 4 ⋮ 5 ¼6 4 F⋮2 5 6 ⋮ 76 6 7 4 ∂Fn ∂Fn ∂Fn ∂Fn ∂Fn ∂Fn ∂Fn 5 6 Δxo 7 vn Fn 4 Δyo 5 |fflffl{zfflffl} |fflffl{zfflffl} ∂a ∂b ∂c ∂R ∂xo ∂yo ∂zo Vn1 Fn1 |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} Δzo |fflfflffl{zfflfflffl} Δ71 Bn7 where the partial derivatives are given as: ∂ jvj signðvÞðz zoÞ jwj signðwÞðy yoÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂a ðw2 + v2 + u2 Þ ∂ juj signðuÞðz zoÞ jwj signðwÞðx xoÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂b ðw2 + v2 + u2 Þ ∂ juj signðuÞðy yoÞ jvj signðvÞðx xoÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂c ð w 2 + v2 + u 2 Þ ∂ c jvj signðvÞ b jwj signðwÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂xo ðw2 + v2 + u2 Þ ∂ c juj signðuÞ a jwj signðwÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂yo ðw2 + v2 + u2 Þ ∂ b juj signðuÞ a jvj signðvÞ pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ¼ ∂zo ð w 2 + v2 + u 2 Þ ∂ ¼ 1 ∂R
(8.17)
(8.18)
8.6 FITTING A CYLINDER
265
and 9 w ¼ bðx xoÞ aðy yoÞ = v ¼ aðz zoÞ cðx xoÞ ; u ¼ cðy yoÞ bðz zoÞ
(8.19)
j j refers to the absolute value. If the value is > 0 then Sign return 1 If the value is < 0 then Sign return 1 Further, to avoid the rank deficiency in the B matrix, two constraint equations (Chapter 6) are added to system as follows: First constraint The cosine direction vector a, b, and c should be normalized as (a2 + b2 + c2 ¼ 1). Therefore, the constraint equation C1 will be:
C1 ¼ ½ 2a 2b 2c 0 0 0 0 , g1 ¼ 1 a2 b2 c2
(8.20)
Second constraint The second constraint equation which should be added is based on the dot product between the cosine direction a, b, and c of the cylinder axis and the origin center point to be 0 or: a:xo + b:yo + c:zo ¼ 0
(8.21)
C2 ¼ ½ xo yo zo a b c 0 , g2 ¼ ½a:xo + b:yo + c:zo
(8.22)
Therefore:
Accordingly, the Helmert system is used to solve the constrained problem as presented in Chapter 6 of Eq. (6.21) as follows:
N Ct C 0
t g Δ C1 ¼ where C ¼ , g ¼ 1 , N ¼ Bt B, and t ¼ Bt F g g2 C2 kc
266
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
EXAMPLE 8.5 Given The following cylindrical distributed points are given in [m] in Table 8.5 as: TABLE 8.5 Cylinder fitting points
• The initial values of the axis normal vector and the radius will be given randomly as: a ¼ 1,b ¼ 1, c ¼ 1,R ¼ 1 • xo, yo, and zo are computed from the average of the point coordinates as: 1.9081, 5.1363, 0.0106.
Required Find the seven parameters of the best fitting cylinder.
Solution The constrained adjustment can be solved by composing the Helmert system of equations as discussed in Chapter 6.
First iteration
8.6 FITTING A CYLINDER
Accordingly, the Helmert system of equations is prepared as:
Second iteration
And directly to the sixth iteration:
The cylinder can then be plotted using the computed adjusted seven parameters. a ¼ 0.0299 b ¼ -0.0070 c ¼ 0.9995 R ¼ 1.5371 xo ¼ 1.9358 yo ¼ 5.1271 zo ¼ -0.0218
267
268
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
Fig. 8.12 illustrates the convergence of corrections to zero value during the least squares adjustment of nine iterations. Fig. 8.13 shows the best fit cylinder of the points listed in Table 8.5. Convergence of cosine direction normal a b c
0.2 Corrections
0
−0.2
−0.4 −0.6 −0.8
Corrections
1
2
3
4
3
4
5 6 Number of iterations Convergence of cylinder radius
7
8
9
0.2 0
−0.2 1
2
5 Number of iterations
6
7
8
9
Convergence of cylinder center 1
xo yo
Corrections
zo
0.5
0
−0.5 1
2
3
4
5
6
7
8
9
Number of iterations
FIG. 8.12
The adjustment convergence to optimal minimum.
6
6
6 4 4
4 2
2
2
6.5
0 0
6
–2
5.5
0 –2
–2
–4
5
–4
4.5
–6
–4
–6 4
6 –6
3 2
5 6 5 4
FIG. 8.13
1
2
3
4
Best fit cylinder.
1
2
3
1 6
5
4
3.5 0.5
1
1.5
2
2.5
3
3.5
8.6 FITTING A CYLINDER
MATLAB code %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%% chapter 8 - example 8.5 %%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc,clear close all %%%%%%%%% given points %%%%%%%%%%%%%%%%%%5 P=[ 1.73 6.41 -6.04 3.38
4.98
-5.94
1.93
3.64
-5.88
0.25
5.69
-6.52
1.26
6.84
-2.19
3.56
4.73
-2.55
1.57
3.88
-1.85
0.07 2.30
5.37 6.54
-1.89 2.19
3.40
4.68
1.74
1.82
3.63
2.06
0.59
5.37
2.34
2.31
6.65
6.71
3.49
4.73
6.13
2.02
3.40
5.58
0.85 5.64 5.94]; x= P(:,1);y= P(:,2);z= P(:,3); P=[x,y,z]; %%%%%%%%%%%%%%%%%%%%5 initial values a=1; b=1;c=1; xo=mean(P(:,1));yo=mean(P(:,2));zo=mean(P(:,3)) ;r=1; format short g ini= [a;b;c;xo;yo;zo;r] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 D=[1;1;1;1;1;1];vv=0; DD=[0 0 0 0 0 0 0]; while vv<7 vv=vv+1; x=P(:,1);y=P(:,2);z=P(:,3); for i=1:size(x,1) u=c*(y(i,1) - yo) - b*(z(i,1) - zo); v=a*(z(i,1) - zo) - c*(x(i,1) - xo); w=b*(x(i,1) - xo) - a*(y(i,1) - yo); d =(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2) - r;
269
270
8. FITTING GEOMETRIC PRIMITIVES WITH LEAST SQUARES
fa(i,1) =-(2*abs(w)*sign(w)*(y(i,1) - yo) + 2*abs(-v)*sign(-v)*(z(i,1) - zo))/(2* (abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fb(i,1) =(2*abs(w)*sign(w)*(x(i,1) - xo) - 2*abs(u)*sign(u)*(z(i,1) - zo))/(2*(abs (w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fc(i,1) =(2*abs(-v)*sign(-v)*(x(i,1) - xo) + 2*abs(u)*sign(u)*(y(i,1) - yo))/(2* (abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fxo(i,1) =-(2*b*abs(w)*sign(w) + 2*c*abs(-v)*sign(-v))/(2*(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fyo(i,1) =(2*a*abs(w)*sign(w) - 2*c*abs(u)*sign(u))/(2*(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fzo(i,1) =(2*a*abs(-v)*sign(-v) + 2*b*abs(u)*sign(u))/(2*(abs(w)^2 + abs(-v)^2 + abs(u)^2)^(1/2)); fr(i,1) =-1; F(i,1)= - d; end B=[ fa(:,1),fb(:,1),fc(:,1),fxo(:,1),fyo(:,1),fzo(:,1),fr(:,1) ]; %%%%%%%%%% 1st constraints %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% C1=[2*a 2*b 2*c 0 0 0 0 ]; g1=[1-a^2-b^2-c^2 ]; %##2nd constraint dot[xo;yo;zo][a,b,c]=0 a1=[xo;yo;zo]; a2=[a;b;c]; C2=[xo,yo,zo,a,b,c,0]; C=[C1;C2]; g2=-(xo*a+yo*b+zo*c); g=[g1;g2]; %%%%%%%%%%%%%%% Helmert method %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% N=[[B’*B; C][C’;zeros(2,2)]]; T=[B’* F;g] ; dt= inv(N)*T; a=a+dt(1); b=b+dt(2); c=c+dt(3); xo=xo+dt(4); yo=yo+dt(5); zo=zo+dt(6); r=r+dt(7); DD=[DD;dt(1:7)’]; end disp(’Adjusted cylinder parameters’) [a;b;c;xo;yo;zo;r] %%%%%%%%%%%%%% statistics dt(end-1:end)=[]; v=B*dt-F; sigma=(v’*v)/(size(B,1)-size(B,2)+1); vcov=sigma*pinv(N);vcov(end-1:end,:)=[];vcov(:,end-1:end)=[]; disp(’std. dev.’) sqrt(diag(vcov)) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
8.6 FITTING A CYLINDER
DD(1,:)=[];D=DD; figure(10) subplot(3,1,1) plot((1:size(D,1)),D(:,1),’-ro’,’Markerfacecolor’,’r’,’LineWidth’,3),hold on plot((1:size(D,1)),D(:,2),’-b^’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on plot((1:size(D,1)),D(:,3),’-ms’,’Markerfacecolor’,’k’,’LineWidth’,3),hold on title(’CONVERGENCE OF COSINE DIRECTION NORMAL’) xlabel(’NUMBER OF ITERATIONS’) ylabel(’CORRECTIONS’) grid on; legend(’a’,’b’,’c’) axis image subplot(3,1,2) plot((1:size(D,1)), D(:,4),’–k’,’Markerfacecolor’,’k’,’LineWidth’,3),hold on title(’CONVERGENCE OF CYLINDER RADIUS’) xlabel(’NUMBER OF ITERATIONS’) ylabel(’CORRECTIONS’) grid on axis image subplot(3,1,3) plot((1:size(D,1)), D(:,5),’-r’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on plot((1:size(D,1)), D(:,6),’-g’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on plot((1:size(D,1)), D(:,7),’-k’,’Markerfacecolor’,’b’,’LineWidth’,3),hold on title(’CONVERGENCE OF CYLINDER CENTER’) xlabel(’NUMBER OF ITERATIONS’) ylabel(’CORRECTIONS’) legend(’xo’,’yo’,’zo’) grid on axis image
271