Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/FiniteProjectiveCamera.cpp @ 161:197a5ddaf68c wasm
FiniteProjectiveCamera
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Wed, 14 Feb 2018 11:29:26 +0100 |
parents | |
children | 45b03b04a777 |
comparison
equal
deleted
inserted
replaced
160:e9dae7e7bffc | 161:197a5ddaf68c |
---|---|
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 | |
24 #include <Core/Logging.h> | |
25 #include <Core/OrthancException.h> | |
26 | |
27 namespace OrthancStone | |
28 { | |
29 void FiniteProjectiveCamera::ComputeMInverse() | |
30 { | |
31 using namespace boost::numeric::ublas; | |
32 | |
33 // inv(M) = inv(K * R) = inv(R) * inv(K) = R' * inv(K). This | |
34 // matrix is always invertible, by definition of finite | |
35 // projective cameras (page 157). | |
36 Matrix kinv; | |
37 LinearAlgebra::InvertUpperTriangularMatrix(kinv, k_); | |
38 minv_ = prod(trans(r_), kinv); | |
39 } | |
40 | |
41 | |
42 void FiniteProjectiveCamera::Setup(const Matrix& k, | |
43 const Matrix& r, | |
44 const Vector& c) | |
45 { | |
46 using namespace boost::numeric::ublas; | |
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 | |
83 Matrix tmp = identity_matrix<double>(3); | |
84 tmp.resize(3, 4); | |
85 tmp(0, 3) = -c[0]; | |
86 tmp(1, 3) = -c[1]; | |
87 tmp(2, 3) = -c[2]; | |
88 | |
89 tmp = prod(r, tmp); | |
90 p_ = prod(k, tmp); | |
91 } | |
92 | |
93 | |
94 void FiniteProjectiveCamera::Setup(const Matrix& p) | |
95 { | |
96 using namespace boost::numeric::ublas; | |
97 | |
98 if (p.size1() != 3 || | |
99 p.size2() != 4) | |
100 { | |
101 LOG(ERROR) << "Invalid camera matrix"; | |
102 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
103 } | |
104 | |
105 p_ = p; | |
106 | |
107 // M is the left 3x3 submatrix of "P" | |
108 Matrix m = p; | |
109 m.resize(3, 3); | |
110 | |
111 // p4 is the last column of "P" | |
112 Vector p4(3); | |
113 p4[0] = p(0, 3); | |
114 p4[1] = p(1, 3); | |
115 p4[2] = p(2, 3); | |
116 | |
117 // The RQ decomposition is explained on page 157 | |
118 LinearAlgebra::RQDecomposition3x3(k_, r_, m); | |
119 ComputeMInverse(); | |
120 | |
121 c_ = prod(-minv_, p4); | |
122 } | |
123 | |
124 | |
125 FiniteProjectiveCamera::FiniteProjectiveCamera(const double k[9], | |
126 const double r[9], | |
127 const double c[3]) | |
128 { | |
129 Matrix kk, rr; | |
130 Vector cc; | |
131 | |
132 LinearAlgebra::FillMatrix(kk, 3, 3, k); | |
133 LinearAlgebra::FillMatrix(rr, 3, 3, r); | |
134 LinearAlgebra::FillVector(cc, 3, c); | |
135 | |
136 Setup(kk, rr, cc); | |
137 } | |
138 | |
139 | |
140 FiniteProjectiveCamera::FiniteProjectiveCamera(const double p[12]) | |
141 { | |
142 Matrix pp; | |
143 LinearAlgebra::FillMatrix(pp, 3, 4, p); | |
144 Setup(pp); | |
145 } | |
146 | |
147 | |
148 Vector FiniteProjectiveCamera::GetRayDirection(double x, | |
149 double y) const | |
150 { | |
151 // This derives from Equation (6.14) on page 162, taking "mu = | |
152 // 1" and noticing that "-inv(M)*p4" corresponds to the camera | |
153 // center in finite projective cameras | |
154 | |
155 // The (x,y) coordinates on the imaged plane, as an homogeneous vector | |
156 Vector xx(3); | |
157 xx[0] = x; | |
158 xx[1] = y; | |
159 xx[2] = 1.0; | |
160 | |
161 return boost::numeric::ublas::prod(minv_, xx); | |
162 } | |
163 | |
164 | |
165 | |
166 static Vector SetupApply(const Vector& v, | |
167 bool infinityAllowed) | |
168 { | |
169 if (v.size() == 3) | |
170 { | |
171 // Vector "v" in non-homogeneous coordinates, add the homogeneous component | |
172 Vector vv; | |
173 LinearAlgebra::AssignVector(vv, v[0], v[1], v[2], 1.0); | |
174 return vv; | |
175 } | |
176 else if (v.size() == 4) | |
177 { | |
178 // Vector "v" is already in homogeneous coordinates | |
179 | |
180 if (!infinityAllowed && | |
181 LinearAlgebra::IsCloseToZero(v[3])) | |
182 { | |
183 LOG(ERROR) << "Cannot apply a finite projective camera to a " | |
184 << "point at infinity with this method"; | |
185 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
186 } | |
187 | |
188 return v; | |
189 } | |
190 else | |
191 { | |
192 LOG(ERROR) << "The input vector must represent a point in 3D"; | |
193 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
194 } | |
195 } | |
196 | |
197 | |
198 void FiniteProjectiveCamera::ApplyFinite(double& x, | |
199 double& y, | |
200 const Vector& v) const | |
201 { | |
202 Vector p = boost::numeric::ublas::prod(p_, SetupApply(v, false)); | |
203 | |
204 if (LinearAlgebra::IsCloseToZero(p[2])) | |
205 { | |
206 // Point at infinity: Should not happen with a finite input point | |
207 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
208 } | |
209 else | |
210 { | |
211 x = p[0] / p[2]; | |
212 y = p[1] / p[2]; | |
213 } | |
214 } | |
215 | |
216 | |
217 Vector FiniteProjectiveCamera::ApplyGeneral(const Vector& v) const | |
218 { | |
219 return boost::numeric::ublas::prod(p_, SetupApply(v, true)); | |
220 } | |
221 } |