Mercurial > hg > orthanc-stone
comparison 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 |
comparison
equal
deleted
inserted
replaced
187:a12ca0f4aaaa | 188:45b03b04a777 |
---|---|
19 **/ | 19 **/ |
20 | 20 |
21 | 21 |
22 #include "FiniteProjectiveCamera.h" | 22 #include "FiniteProjectiveCamera.h" |
23 | 23 |
24 #include "GeometryToolbox.h" | |
25 | |
24 #include <Core/Logging.h> | 26 #include <Core/Logging.h> |
25 #include <Core/OrthancException.h> | 27 #include <Core/OrthancException.h> |
26 | 28 |
27 namespace OrthancStone | 29 namespace OrthancStone |
28 { | 30 { |
41 | 43 |
42 void FiniteProjectiveCamera::Setup(const Matrix& k, | 44 void FiniteProjectiveCamera::Setup(const Matrix& k, |
43 const Matrix& r, | 45 const Matrix& r, |
44 const Vector& c) | 46 const Vector& c) |
45 { | 47 { |
46 using namespace boost::numeric::ublas; | |
47 | |
48 if (k.size1() != 3 || | 48 if (k.size1() != 3 || |
49 k.size2() != 3 || | 49 k.size2() != 3 || |
50 !LinearAlgebra::IsCloseToZero(k(1, 0)) || | 50 !LinearAlgebra::IsCloseToZero(k(1, 0)) || |
51 !LinearAlgebra::IsCloseToZero(k(2, 0)) || | 51 !LinearAlgebra::IsCloseToZero(k(2, 0)) || |
52 !LinearAlgebra::IsCloseToZero(k(2, 1))) | 52 !LinearAlgebra::IsCloseToZero(k(2, 1))) |
78 r_ = r; | 78 r_ = r; |
79 c_ = c; | 79 c_ = c; |
80 | 80 |
81 ComputeMInverse(); | 81 ComputeMInverse(); |
82 | 82 |
83 Matrix tmp = identity_matrix<double>(3); | 83 Matrix tmp = LinearAlgebra::IdentityMatrix(3); |
84 tmp.resize(3, 4); | 84 tmp.resize(3, 4); |
85 tmp(0, 3) = -c[0]; | 85 tmp(0, 3) = -c[0]; |
86 tmp(1, 3) = -c[1]; | 86 tmp(1, 3) = -c[1]; |
87 tmp(2, 3) = -c[2]; | 87 tmp(2, 3) = -c[2]; |
88 | 88 |
89 tmp = prod(r, tmp); | 89 p_ = LinearAlgebra::Product(k, r, tmp); |
90 p_ = prod(k, tmp); | 90 |
91 assert(p_.size1() == 3 && | |
92 p_.size2() == 4); | |
93 | |
91 } | 94 } |
92 | 95 |
93 | 96 |
94 void FiniteProjectiveCamera::Setup(const Matrix& p) | 97 void FiniteProjectiveCamera::Setup(const Matrix& p) |
95 { | 98 { |
96 using namespace boost::numeric::ublas; | |
97 | |
98 if (p.size1() != 3 || | 99 if (p.size1() != 3 || |
99 p.size2() != 4) | 100 p.size2() != 4) |
100 { | 101 { |
101 LOG(ERROR) << "Invalid camera matrix"; | 102 LOG(ERROR) << "Invalid camera matrix"; |
102 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | 103 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
116 | 117 |
117 // The RQ decomposition is explained on page 157 | 118 // The RQ decomposition is explained on page 157 |
118 LinearAlgebra::RQDecomposition3x3(k_, r_, m); | 119 LinearAlgebra::RQDecomposition3x3(k_, r_, m); |
119 ComputeMInverse(); | 120 ComputeMInverse(); |
120 | 121 |
121 c_ = prod(-minv_, p4); | 122 c_ = LinearAlgebra::Product(-minv_, p4); |
122 } | 123 } |
123 | 124 |
124 | 125 |
125 FiniteProjectiveCamera::FiniteProjectiveCamera(const double k[9], | 126 FiniteProjectiveCamera::FiniteProjectiveCamera(const double k[9], |
126 const double r[9], | 127 const double r[9], |
216 | 217 |
217 Vector FiniteProjectiveCamera::ApplyGeneral(const Vector& v) const | 218 Vector FiniteProjectiveCamera::ApplyGeneral(const Vector& v) const |
218 { | 219 { |
219 return boost::numeric::ublas::prod(p_, SetupApply(v, true)); | 220 return boost::numeric::ublas::prod(p_, SetupApply(v, true)); |
220 } | 221 } |
222 | |
223 | |
224 static Vector AddHomogeneousCoordinate(const Vector& p) | |
225 { | |
226 assert(p.size() == 3); | |
227 return LinearAlgebra::CreateVector(p[0], p[1], p[2], 1); | |
228 } | |
229 | |
230 | |
231 FiniteProjectiveCamera::FiniteProjectiveCamera(const Vector& camera, | |
232 const Vector& principalPoint, | |
233 double angle, | |
234 unsigned int imageWidth, | |
235 unsigned int imageHeight, | |
236 double pixelSpacingX, | |
237 double pixelSpacingY) | |
238 { | |
239 if (camera.size() != 3 || | |
240 principalPoint.size() != 3 || | |
241 LinearAlgebra::IsCloseToZero(pixelSpacingX) || | |
242 LinearAlgebra::IsCloseToZero(pixelSpacingY)) | |
243 { | |
244 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
245 } | |
246 | |
247 const double focal = boost::numeric::ublas::norm_2(camera - principalPoint); | |
248 | |
249 if (LinearAlgebra::IsCloseToZero(focal)) | |
250 { | |
251 LOG(ERROR) << "Camera lies on the image plane"; | |
252 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
253 } | |
254 | |
255 Matrix a; | |
256 LinearAlgebra::AlignVectorsWithRotation(a, camera - principalPoint, | |
257 LinearAlgebra::CreateVector(0, 0, -1)); | |
258 | |
259 Matrix r = LinearAlgebra::Product(GeometryToolbox::CreateRotationMatrixAlongZ(angle), a); | |
260 | |
261 Matrix k = LinearAlgebra::ZeroMatrix(3, 3); | |
262 k(0,0) = focal / pixelSpacingX; | |
263 k(1,1) = focal / pixelSpacingY; | |
264 k(0,2) = static_cast<double>(imageWidth) / 2.0; | |
265 k(1,2) = static_cast<double>(imageHeight) / 2.0; | |
266 k(2,2) = 1; | |
267 | |
268 Setup(k, r, camera); | |
269 | |
270 { | |
271 // Sanity checks | |
272 Vector v1 = LinearAlgebra::Product(p_, AddHomogeneousCoordinate(camera)); | |
273 Vector v2 = LinearAlgebra::Product(p_, AddHomogeneousCoordinate(principalPoint)); | |
274 | |
275 if (!LinearAlgebra::IsCloseToZero(v1[2]) || // Camera is mapped to singularity | |
276 LinearAlgebra::IsCloseToZero(v2[2])) | |
277 { | |
278 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
279 } | |
280 | |
281 // The principal point must be mapped to the center of the image | |
282 v2 /= v2[2]; | |
283 | |
284 if (!LinearAlgebra::IsNear(v2[0], static_cast<double>(imageWidth) / 2.0) || | |
285 !LinearAlgebra::IsNear(v2[1], static_cast<double>(imageHeight) / 2.0)) | |
286 { | |
287 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
288 } | |
289 } | |
290 } | |
221 } | 291 } |