Mercurial > hg > orthanc-stone

comparison Framework/Toolbox/FiniteProjectiveCamera.cpp @ 188:45b03b04a777 wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 }