1 /*
2 * Medical Image Registration ToolKit (MIRTK)
3 *
4 * Copyright 2016 Imperial College London
5 * Copyright 2016 Andreas Schuh
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 */
19
20 #include "mirtk/Plane.h"
21
22 #include "mirtk/Vector.h"
23 #include "mirtk/Matrix.h"
24
25
26 namespace mirtk {
27
28
29 // -----------------------------------------------------------------------------
UpdateOrigin()30 void Plane::UpdateOrigin()
31 {
32 _Origin._x = _Offset * _Normal[0];
33 _Origin._y = _Offset * _Normal[1];
34 _Origin._z = _Offset * _Normal[2];
35 }
36
37 // -----------------------------------------------------------------------------
UpdateTangents()38 void Plane::UpdateTangents()
39 {
40 Vector3 v(_Normal[1], _Normal[2], _Normal[0]);
41 _Tangent1 = _Normal.Cross(v);
42 if (_Tangent1.SquaredLength() < 1e-6) {
43 v[1] *= -1.0;
44 _Tangent1 = _Normal.Cross(v);
45 if (_Tangent1.SquaredLength() < 1e-6) {
46 cerr << "Plane::UpdateTangents: Degenerate normal vector!" << endl;
47 exit(1);
48 }
49 }
50 _Tangent2 = _Normal.Cross(_Tangent1);
51 _Tangent1.Normalize();
52 _Tangent2.Normalize();
53 }
54
55 // -----------------------------------------------------------------------------
Fit(const PointSet & points)56 double Plane::Fit(const PointSet &points)
57 {
58 if (points.Size() == 1) {
59 _Normal = Vector3(.0, .0, 1.0);
60 _Offset = _Normal.Dot(Vector3(points(0)._x, points(0)._y, points(0)._z));
61 UpdateTangents();
62 return .0;
63 }
64
65 // Build design matrix
66 Matrix x(points.Size(), 3);
67 for (int i = 0; i < x.Rows(); ++i) {
68 const Point &p = points(i);
69 x(i, 0) = p._x;
70 x(i, 1) = p._y;
71 x(i, 2) = p._z;
72 }
73
74 // Normalize design matrix
75 _Origin._x = x.ColMean(0);
76 _Origin._y = x.ColMean(1);
77 _Origin._z = x.ColMean(2);
78 x.AddToCol(0, -_Origin._x);
79 x.AddToCol(1, -_Origin._y);
80 x.AddToCol(2, -_Origin._z);
81 x /= sqrt(x.Rows() - 1);
82
83 // Perform PCA using SVD
84 Matrix u, v;
85 Vector s;
86
87 x.SVD(u, s, v);
88
89 // Get plane normal and orthonormal tangent vectors
90 int c = 2;
91 if (s(0) < s(c)) c = 0;
92 if (s(1) < s(c)) c = 1;
93
94 _Normal[0] = v(0, c);
95 _Normal[1] = v(1, c);
96 _Normal[2] = v(2, c);
97
98 c = (c == 2 ? 0 : c + 1);
99 _Tangent1[0] = v(0, c);
100 _Tangent1[1] = v(1, c);
101 _Tangent1[2] = v(2, c);
102
103 c = (c == 2 ? 0 : c + 1);
104 _Tangent2[0] = v(0, c);
105 _Tangent2[1] = v(1, c);
106 _Tangent2[2] = v(2, c);
107
108 Vector3 cross = _Tangent2.Cross(_Normal);
109 if (_Tangent1.Dot(cross) < .0) _Normal *= -1.0;
110
111 // Compute distance of plane to world origin
112 _Offset = (_Origin._x * _Normal[0] + _Origin._y * _Normal[1] + _Origin._z * _Normal[2]);
113
114 // Evaluate RMS error
115 double d, error = .0;
116 for (int i = 0; i < points.Size(); ++i) {
117 d = Distance(points(i));
118 error += d * d;
119 }
120 return sqrt(error / points.Size());
121 }
122
123 // -----------------------------------------------------------------------------
Print(ostream & os,Indent indent) const124 ostream &Plane::Print(ostream &os, Indent indent) const
125 {
126 os << indent << "<n, x> = b, where n = ["
127 << _Normal[0] << ", " << _Normal[1] << ", " << _Normal[2]
128 << "] and b = " << _Offset << " with local origin at x0 = ["
129 << _Origin._x << ", " << _Origin._y << ", " << _Origin._z << "]\n";
130 return os;
131 }
132
133
134 } // namespace mirtk
135