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
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
161
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
1 /**
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
2 * Stone of Orthanc
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
4 * Department, University Hospital of Liege, Belgium
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
5 * Copyright (C) 2017-2018 Osimis S.A., Belgium
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
6 *
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
7 * This program is free software: you can redistribute it and/or
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
8 * modify it under the terms of the GNU Affero General Public License
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
9 * as published by the Free Software Foundation, either version 3 of
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
10 * the License, or (at your option) any later version.
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
11 *
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
12 * This program is distributed in the hope that it will be useful, but
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
15 * Affero General Public License for more details.
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
16 *
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
17 * You should have received a copy of the GNU Affero General Public License
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
18 * along with this program. If not, see <http://www.gnu.org/licenses/>.
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
19 **/
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
20
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
21
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
22 #include "FiniteProjectiveCamera.h"
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
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
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
26 #include <Core/Logging.h>
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
27 #include <Core/OrthancException.h>
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
28
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
29 namespace OrthancStone
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
30 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
31 void FiniteProjectiveCamera::ComputeMInverse()
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
32 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
33 using namespace boost::numeric::ublas;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35 // inv(M) = inv(K * R) = inv(R) * inv(K) = R' * inv(K). This
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 // matrix is always invertible, by definition of finite
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37 // projective cameras (page 157).
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 Matrix kinv;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
39 LinearAlgebra::InvertUpperTriangularMatrix(kinv, k_);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
40 minv_ = prod(trans(r_), kinv);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
41 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
42
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
43
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
44 void FiniteProjectiveCamera::Setup(const Matrix& k,
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
45 const Matrix& r,
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
46 const Vector& c)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
47 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
48 if (k.size1() != 3 ||
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
49 k.size2() != 3 ||
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
50 !LinearAlgebra::IsCloseToZero(k(1, 0)) ||
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
51 !LinearAlgebra::IsCloseToZero(k(2, 0)) ||
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
52 !LinearAlgebra::IsCloseToZero(k(2, 1)))
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
53 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
54 LOG(ERROR) << "Invalid intrinsic parameters";
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
56 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
57
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
58 if (r.size1() != 3 ||
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
59 r.size2() != 3)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
60 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61 LOG(ERROR) << "Invalid size for a 3D rotation matrix";
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
62 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65 if (!LinearAlgebra::IsRotationMatrix(r, 100.0 * std::numeric_limits<float>::epsilon()))
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67 LOG(ERROR) << "Invalid rotation matrix";
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
68 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
69 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
71 if (c.size() != 3)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
72 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
73 LOG(ERROR) << "Invalid camera center";
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
77 k_ = k;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78 r_ = r;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
79 c_ = c;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
80
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
81 ComputeMInverse();
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
82
188
45b03b04a777 calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
83 Matrix tmp = LinearAlgebra::IdentityMatrix(3);
161
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
84 tmp.resize(3, 4);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
85 tmp(0, 3) = -c[0];
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
86 tmp(1, 3) = -c[1];
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
87 tmp(2, 3) = -c[2];
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
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
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
94 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
95
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
97 void FiniteProjectiveCamera::Setup(const Matrix& p)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
98 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
99 if (p.size1() != 3 ||
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
100 p.size2() != 4)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
101 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
102 LOG(ERROR) << "Invalid camera matrix";
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
103 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
104 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
105
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
106 p_ = p;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
107
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
108 // M is the left 3x3 submatrix of "P"
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
109 Matrix m = p;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
110 m.resize(3, 3);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112 // p4 is the last column of "P"
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
113 Vector p4(3);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
114 p4[0] = p(0, 3);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
115 p4[1] = p(1, 3);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
116 p4[2] = p(2, 3);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
117
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
118 // The RQ decomposition is explained on page 157
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
119 LinearAlgebra::RQDecomposition3x3(k_, r_, m);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
120 ComputeMInverse();
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
121
188
45b03b04a777 calibration of FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 161
diff changeset
122 c_ = LinearAlgebra::Product(-minv_, p4);
161
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
123 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
124
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
125
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
126 FiniteProjectiveCamera::FiniteProjectiveCamera(const double k[9],
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
127 const double r[9],
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128 const double c[3])
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
129 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
130 Matrix kk, rr;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
131 Vector cc;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
132
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
133 LinearAlgebra::FillMatrix(kk, 3, 3, k);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
134 LinearAlgebra::FillMatrix(rr, 3, 3, r);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
135 LinearAlgebra::FillVector(cc, 3, c);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
136
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
137 Setup(kk, rr, cc);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
138 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
139
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
140
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
141 FiniteProjectiveCamera::FiniteProjectiveCamera(const double p[12])
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
142 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
143 Matrix pp;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 LinearAlgebra::FillMatrix(pp, 3, 4, p);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
145 Setup(pp);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
146 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
147
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
148
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
149 Vector FiniteProjectiveCamera::GetRayDirection(double x,
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
150 double y) const
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
151 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
152 // This derives from Equation (6.14) on page 162, taking "mu =
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
153 // 1" and noticing that "-inv(M)*p4" corresponds to the camera
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
154 // center in finite projective cameras
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
155
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
156 // The (x,y) coordinates on the imaged plane, as an homogeneous vector
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
157 Vector xx(3);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
158 xx[0] = x;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
159 xx[1] = y;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
160 xx[2] = 1.0;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
161
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
162 return boost::numeric::ublas::prod(minv_, xx);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
163 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
164
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
165
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
166
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
167 static Vector SetupApply(const Vector& v,
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
168 bool infinityAllowed)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
169 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
170 if (v.size() == 3)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
171 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
172 // Vector "v" in non-homogeneous coordinates, add the homogeneous component
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
173 Vector vv;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
174 LinearAlgebra::AssignVector(vv, v[0], v[1], v[2], 1.0);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
175 return vv;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
176 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
177 else if (v.size() == 4)
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
178 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
179 // Vector "v" is already in homogeneous coordinates
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
180
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
181 if (!infinityAllowed &&
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
182 LinearAlgebra::IsCloseToZero(v[3]))
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
183 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
184 LOG(ERROR) << "Cannot apply a finite projective camera to a "
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
185 << "point at infinity with this method";
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
186 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
187 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
188
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
189 return v;
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
190 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
191 else
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
192 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
193 LOG(ERROR) << "The input vector must represent a point in 3D";
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
194 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
195 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
196 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
197
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
198
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
199 void FiniteProjectiveCamera::ApplyFinite(double& x,
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
200 double& y,
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
201 const Vector& v) const
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
202 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
203 Vector p = boost::numeric::ublas::prod(p_, SetupApply(v, false));
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
204
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
205 if (LinearAlgebra::IsCloseToZero(p[2]))
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
206 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
207 // Point at infinity: Should not happen with a finite input point
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
208 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
209 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
210 else
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
211 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
212 x = p[0] / p[2];
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
213 y = p[1] / p[2];
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
214 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
215 }
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
216
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
217
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
218 Vector FiniteProjectiveCamera::ApplyGeneral(const Vector& v) const
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
219 {
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
220 return boost::numeric::ublas::prod(p_, SetupApply(v, true));
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
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
197a5ddaf68c FiniteProjectiveCamera
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
291 }