Mercurial > hg > orthanc-stone
annotate Framework/Toolbox/FiniteProjectiveCamera.cpp @ 188:45b03b04a777 wasm
calibration of FiniteProjectiveCamera
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Fri, 16 Mar 2018 13:02:17 +0100 |
parents | 197a5ddaf68c |
children | 964118e7e6de |
rev | line source |
---|---|
161 | 1 /** |
2 * Stone of Orthanc | |
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics | |
4 * Department, University Hospital of Liege, Belgium | |
5 * Copyright (C) 2017-2018 Osimis S.A., Belgium | |
6 * | |
7 * This program is free software: you can redistribute it and/or | |
8 * modify it under the terms of the GNU Affero General Public License | |
9 * as published by the Free Software Foundation, either version 3 of | |
10 * the License, or (at your option) any later version. | |
11 * | |
12 * This program is distributed in the hope that it will be useful, but | |
13 * WITHOUT ANY WARRANTY; without even the implied warranty of | |
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
15 * Affero General Public License for more details. | |
16 * | |
17 * You should have received a copy of the GNU Affero General Public License | |
18 * along with this program. If not, see <http://www.gnu.org/licenses/>. | |
19 **/ | |
20 | |
21 | |
22 #include "FiniteProjectiveCamera.h" | |
23 | |
188
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
24 #include "GeometryToolbox.h" |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
25 |
161 | 26 #include <Core/Logging.h> |
27 #include <Core/OrthancException.h> | |
28 | |
29 namespace OrthancStone | |
30 { | |
31 void FiniteProjectiveCamera::ComputeMInverse() | |
32 { | |
33 using namespace boost::numeric::ublas; | |
34 | |
35 // inv(M) = inv(K * R) = inv(R) * inv(K) = R' * inv(K). This | |
36 // matrix is always invertible, by definition of finite | |
37 // projective cameras (page 157). | |
38 Matrix kinv; | |
39 LinearAlgebra::InvertUpperTriangularMatrix(kinv, k_); | |
40 minv_ = prod(trans(r_), kinv); | |
41 } | |
42 | |
43 | |
44 void FiniteProjectiveCamera::Setup(const Matrix& k, | |
45 const Matrix& r, | |
46 const Vector& c) | |
47 { | |
48 if (k.size1() != 3 || | |
49 k.size2() != 3 || | |
50 !LinearAlgebra::IsCloseToZero(k(1, 0)) || | |
51 !LinearAlgebra::IsCloseToZero(k(2, 0)) || | |
52 !LinearAlgebra::IsCloseToZero(k(2, 1))) | |
53 { | |
54 LOG(ERROR) << "Invalid intrinsic parameters"; | |
55 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
56 } | |
57 | |
58 if (r.size1() != 3 || | |
59 r.size2() != 3) | |
60 { | |
61 LOG(ERROR) << "Invalid size for a 3D rotation matrix"; | |
62 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
63 } | |
64 | |
65 if (!LinearAlgebra::IsRotationMatrix(r, 100.0 * std::numeric_limits<float>::epsilon())) | |
66 { | |
67 LOG(ERROR) << "Invalid rotation matrix"; | |
68 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
69 } | |
70 | |
71 if (c.size() != 3) | |
72 { | |
73 LOG(ERROR) << "Invalid camera center"; | |
74 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
75 } | |
76 | |
77 k_ = k; | |
78 r_ = r; | |
79 c_ = c; | |
80 | |
81 ComputeMInverse(); | |
82 | |
188
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
83 Matrix tmp = LinearAlgebra::IdentityMatrix(3); |
161 | 84 tmp.resize(3, 4); |
85 tmp(0, 3) = -c[0]; | |
86 tmp(1, 3) = -c[1]; | |
87 tmp(2, 3) = -c[2]; | |
88 | |
188
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
89 p_ = LinearAlgebra::Product(k, r, tmp); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
90 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
91 assert(p_.size1() == 3 && |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
92 p_.size2() == 4); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
93 |
161 | 94 } |
95 | |
96 | |
97 void FiniteProjectiveCamera::Setup(const Matrix& p) | |
98 { | |
99 if (p.size1() != 3 || | |
100 p.size2() != 4) | |
101 { | |
102 LOG(ERROR) << "Invalid camera matrix"; | |
103 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
104 } | |
105 | |
106 p_ = p; | |
107 | |
108 // M is the left 3x3 submatrix of "P" | |
109 Matrix m = p; | |
110 m.resize(3, 3); | |
111 | |
112 // p4 is the last column of "P" | |
113 Vector p4(3); | |
114 p4[0] = p(0, 3); | |
115 p4[1] = p(1, 3); | |
116 p4[2] = p(2, 3); | |
117 | |
118 // The RQ decomposition is explained on page 157 | |
119 LinearAlgebra::RQDecomposition3x3(k_, r_, m); | |
120 ComputeMInverse(); | |
121 | |
188
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
122 c_ = LinearAlgebra::Product(-minv_, p4); |
161 | 123 } |
124 | |
125 | |
126 FiniteProjectiveCamera::FiniteProjectiveCamera(const double k[9], | |
127 const double r[9], | |
128 const double c[3]) | |
129 { | |
130 Matrix kk, rr; | |
131 Vector cc; | |
132 | |
133 LinearAlgebra::FillMatrix(kk, 3, 3, k); | |
134 LinearAlgebra::FillMatrix(rr, 3, 3, r); | |
135 LinearAlgebra::FillVector(cc, 3, c); | |
136 | |
137 Setup(kk, rr, cc); | |
138 } | |
139 | |
140 | |
141 FiniteProjectiveCamera::FiniteProjectiveCamera(const double p[12]) | |
142 { | |
143 Matrix pp; | |
144 LinearAlgebra::FillMatrix(pp, 3, 4, p); | |
145 Setup(pp); | |
146 } | |
147 | |
148 | |
149 Vector FiniteProjectiveCamera::GetRayDirection(double x, | |
150 double y) const | |
151 { | |
152 // This derives from Equation (6.14) on page 162, taking "mu = | |
153 // 1" and noticing that "-inv(M)*p4" corresponds to the camera | |
154 // center in finite projective cameras | |
155 | |
156 // The (x,y) coordinates on the imaged plane, as an homogeneous vector | |
157 Vector xx(3); | |
158 xx[0] = x; | |
159 xx[1] = y; | |
160 xx[2] = 1.0; | |
161 | |
162 return boost::numeric::ublas::prod(minv_, xx); | |
163 } | |
164 | |
165 | |
166 | |
167 static Vector SetupApply(const Vector& v, | |
168 bool infinityAllowed) | |
169 { | |
170 if (v.size() == 3) | |
171 { | |
172 // Vector "v" in non-homogeneous coordinates, add the homogeneous component | |
173 Vector vv; | |
174 LinearAlgebra::AssignVector(vv, v[0], v[1], v[2], 1.0); | |
175 return vv; | |
176 } | |
177 else if (v.size() == 4) | |
178 { | |
179 // Vector "v" is already in homogeneous coordinates | |
180 | |
181 if (!infinityAllowed && | |
182 LinearAlgebra::IsCloseToZero(v[3])) | |
183 { | |
184 LOG(ERROR) << "Cannot apply a finite projective camera to a " | |
185 << "point at infinity with this method"; | |
186 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
187 } | |
188 | |
189 return v; | |
190 } | |
191 else | |
192 { | |
193 LOG(ERROR) << "The input vector must represent a point in 3D"; | |
194 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
195 } | |
196 } | |
197 | |
198 | |
199 void FiniteProjectiveCamera::ApplyFinite(double& x, | |
200 double& y, | |
201 const Vector& v) const | |
202 { | |
203 Vector p = boost::numeric::ublas::prod(p_, SetupApply(v, false)); | |
204 | |
205 if (LinearAlgebra::IsCloseToZero(p[2])) | |
206 { | |
207 // Point at infinity: Should not happen with a finite input point | |
208 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
209 } | |
210 else | |
211 { | |
212 x = p[0] / p[2]; | |
213 y = p[1] / p[2]; | |
214 } | |
215 } | |
216 | |
217 | |
218 Vector FiniteProjectiveCamera::ApplyGeneral(const Vector& v) const | |
219 { | |
220 return boost::numeric::ublas::prod(p_, SetupApply(v, true)); | |
221 } | |
188
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
222 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
223 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
224 static Vector AddHomogeneousCoordinate(const Vector& p) |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
225 { |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
226 assert(p.size() == 3); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
227 return LinearAlgebra::CreateVector(p[0], p[1], p[2], 1); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
228 } |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
229 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
230 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
231 FiniteProjectiveCamera::FiniteProjectiveCamera(const Vector& camera, |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
232 const Vector& principalPoint, |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
233 double angle, |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
234 unsigned int imageWidth, |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
235 unsigned int imageHeight, |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
236 double pixelSpacingX, |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
237 double pixelSpacingY) |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
238 { |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
239 if (camera.size() != 3 || |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
240 principalPoint.size() != 3 || |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
241 LinearAlgebra::IsCloseToZero(pixelSpacingX) || |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
242 LinearAlgebra::IsCloseToZero(pixelSpacingY)) |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
243 { |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
244 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
245 } |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
246 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
247 const double focal = boost::numeric::ublas::norm_2(camera - principalPoint); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
248 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
249 if (LinearAlgebra::IsCloseToZero(focal)) |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
250 { |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
251 LOG(ERROR) << "Camera lies on the image plane"; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
252 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
253 } |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
254 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
255 Matrix a; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
256 LinearAlgebra::AlignVectorsWithRotation(a, camera - principalPoint, |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
257 LinearAlgebra::CreateVector(0, 0, -1)); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
258 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
259 Matrix r = LinearAlgebra::Product(GeometryToolbox::CreateRotationMatrixAlongZ(angle), a); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
260 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
261 Matrix k = LinearAlgebra::ZeroMatrix(3, 3); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
262 k(0,0) = focal / pixelSpacingX; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
263 k(1,1) = focal / pixelSpacingY; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
264 k(0,2) = static_cast<double>(imageWidth) / 2.0; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
265 k(1,2) = static_cast<double>(imageHeight) / 2.0; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
266 k(2,2) = 1; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
267 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
268 Setup(k, r, camera); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
269 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
270 { |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
271 // Sanity checks |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
272 Vector v1 = LinearAlgebra::Product(p_, AddHomogeneousCoordinate(camera)); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
273 Vector v2 = LinearAlgebra::Product(p_, AddHomogeneousCoordinate(principalPoint)); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
274 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
275 if (!LinearAlgebra::IsCloseToZero(v1[2]) || // Camera is mapped to singularity |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
276 LinearAlgebra::IsCloseToZero(v2[2])) |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
277 { |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
278 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
279 } |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
280 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
281 // The principal point must be mapped to the center of the image |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
282 v2 /= v2[2]; |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
283 |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
284 if (!LinearAlgebra::IsNear(v2[0], static_cast<double>(imageWidth) / 2.0) || |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
285 !LinearAlgebra::IsNear(v2[1], static_cast<double>(imageHeight) / 2.0)) |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
286 { |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
287 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
288 } |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
289 } |
45b03b04a777
calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
161
diff
changeset
|
290 } |
161 | 291 } |