Mercurial > hg > orthanc-stone

comparison Framework/Toolbox/FiniteProjectiveCamera.cpp @ 161:197a5ddaf68c wasm

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