|
161
|
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 }
|
