1 // Copyright (c) 2011 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include "libmv/simple_pipeline/camera_intrinsics.h"
22
23 #include "libmv/logging/logging.h"
24 #include "libmv/simple_pipeline/distortion_models.h"
25 #include "libmv/simple_pipeline/packed_intrinsics.h"
26
27 namespace libmv {
28
29 namespace internal {
30
LookupWarpGrid()31 LookupWarpGrid::LookupWarpGrid()
32 : offset_(NULL),
33 width_(0),
34 height_(0),
35 overscan_(0.0),
36 threads_(1) {}
37
LookupWarpGrid(const LookupWarpGrid & from)38 LookupWarpGrid::LookupWarpGrid(const LookupWarpGrid &from)
39 : offset_(NULL),
40 width_(from.width_),
41 height_(from.height_),
42 overscan_(from.overscan_),
43 threads_(from.threads_) {
44 if (from.offset_) {
45 offset_ = new Offset[width_ * height_];
46 memcpy(offset_, from.offset_, sizeof(Offset) * width_ * height_);
47 }
48 }
49
~LookupWarpGrid()50 LookupWarpGrid::~LookupWarpGrid() {
51 delete [] offset_;
52 }
53
Reset()54 void LookupWarpGrid::Reset() {
55 delete [] offset_;
56 offset_ = NULL;
57 }
58
59 // Set number of threads used for threaded buffer distortion/undistortion.
SetThreads(int threads)60 void LookupWarpGrid::SetThreads(int threads) {
61 threads_ = threads;
62 }
63
64 } // namespace internal
65
CameraIntrinsics()66 CameraIntrinsics::CameraIntrinsics()
67 : image_width_(0),
68 image_height_(0),
69 K_(Mat3::Identity()) {}
70
CameraIntrinsics(const CameraIntrinsics & from)71 CameraIntrinsics::CameraIntrinsics(const CameraIntrinsics &from)
72 : image_width_(from.image_width_),
73 image_height_(from.image_height_),
74 K_(from.K_),
75 distort_(from.distort_),
76 undistort_(from.undistort_) {}
77
78 // Set the image size in pixels.
SetImageSize(int width,int height)79 void CameraIntrinsics::SetImageSize(int width, int height) {
80 image_width_ = width;
81 image_height_ = height;
82 ResetLookupGrids();
83 }
84
85 // Set the entire calibration matrix at once.
SetK(const Mat3 new_k)86 void CameraIntrinsics::SetK(const Mat3 new_k) {
87 K_ = new_k;
88 ResetLookupGrids();
89 }
90
91 // Set both x and y focal length in pixels.
SetFocalLength(double focal_x,double focal_y)92 void CameraIntrinsics::SetFocalLength(double focal_x,
93 double focal_y) {
94 K_(0, 0) = focal_x;
95 K_(1, 1) = focal_y;
96 ResetLookupGrids();
97 }
98
99 // Set principal point in pixels.
SetPrincipalPoint(double cx,double cy)100 void CameraIntrinsics::SetPrincipalPoint(double cx,
101 double cy) {
102 K_(0, 2) = cx;
103 K_(1, 2) = cy;
104 ResetLookupGrids();
105 }
106
107 // Set number of threads used for threaded buffer distortion/undistortion.
SetThreads(int threads)108 void CameraIntrinsics::SetThreads(int threads) {
109 distort_.SetThreads(threads);
110 undistort_.SetThreads(threads);
111 }
112
ImageSpaceToNormalized(double image_x,double image_y,double * normalized_x,double * normalized_y) const113 void CameraIntrinsics::ImageSpaceToNormalized(double image_x,
114 double image_y,
115 double *normalized_x,
116 double *normalized_y) const {
117 *normalized_x = (image_x - principal_point_x()) / focal_length_x();
118 *normalized_y = (image_y - principal_point_y()) / focal_length_y();
119 }
120
NormalizedToImageSpace(double normalized_x,double normalized_y,double * image_x,double * image_y) const121 void CameraIntrinsics::NormalizedToImageSpace(double normalized_x,
122 double normalized_y,
123 double *image_x,
124 double *image_y) const {
125 *image_x = normalized_x * focal_length_x() + principal_point_x();
126 *image_y = normalized_y * focal_length_y() + principal_point_y();
127 }
128
129 // Reset lookup grids after changing the distortion model.
ResetLookupGrids()130 void CameraIntrinsics::ResetLookupGrids() {
131 distort_.Reset();
132 undistort_.Reset();
133 }
134
Pack(PackedIntrinsics * packed_intrinsics) const135 void CameraIntrinsics::Pack(PackedIntrinsics* packed_intrinsics) const {
136 packed_intrinsics->SetFocalLength(focal_length());
137 packed_intrinsics->SetPrincipalPoint(principal_point_x(),
138 principal_point_y());
139 }
140
Unpack(const PackedIntrinsics & packed_intrinsics)141 void CameraIntrinsics::Unpack(const PackedIntrinsics& packed_intrinsics) {
142 SetFocalLength(packed_intrinsics.GetFocalLength(),
143 packed_intrinsics.GetFocalLength());
144
145 SetPrincipalPoint(packed_intrinsics.GetPrincipalPointX(),
146 packed_intrinsics.GetPrincipalPointY());
147 }
148
149 // Polynomial model.
150
PolynomialCameraIntrinsics()151 PolynomialCameraIntrinsics::PolynomialCameraIntrinsics()
152 : CameraIntrinsics() {
153 SetRadialDistortion(0.0, 0.0, 0.0);
154 SetTangentialDistortion(0.0, 0.0);
155 }
156
PolynomialCameraIntrinsics(const PolynomialCameraIntrinsics & from)157 PolynomialCameraIntrinsics::PolynomialCameraIntrinsics(
158 const PolynomialCameraIntrinsics &from)
159 : CameraIntrinsics(from) {
160 SetRadialDistortion(from.k1(), from.k2(), from.k3());
161 SetTangentialDistortion(from.p1(), from.p2());
162 }
163
SetRadialDistortion(double k1,double k2,double k3)164 void PolynomialCameraIntrinsics::SetRadialDistortion(double k1,
165 double k2,
166 double k3) {
167 parameters_[OFFSET_K1] = k1;
168 parameters_[OFFSET_K2] = k2;
169 parameters_[OFFSET_K3] = k3;
170 ResetLookupGrids();
171 }
172
SetTangentialDistortion(double p1,double p2)173 void PolynomialCameraIntrinsics::SetTangentialDistortion(double p1,
174 double p2) {
175 parameters_[OFFSET_P1] = p1;
176 parameters_[OFFSET_P2] = p2;
177 ResetLookupGrids();
178 }
179
ApplyIntrinsics(double normalized_x,double normalized_y,double * image_x,double * image_y) const180 void PolynomialCameraIntrinsics::ApplyIntrinsics(double normalized_x,
181 double normalized_y,
182 double *image_x,
183 double *image_y) const {
184 ApplyPolynomialDistortionModel(focal_length_x(),
185 focal_length_y(),
186 principal_point_x(),
187 principal_point_y(),
188 k1(), k2(), k3(),
189 p1(), p2(),
190 normalized_x,
191 normalized_y,
192 image_x,
193 image_y);
194 }
195
InvertIntrinsics(double image_x,double image_y,double * normalized_x,double * normalized_y) const196 void PolynomialCameraIntrinsics::InvertIntrinsics(
197 double image_x,
198 double image_y,
199 double *normalized_x,
200 double *normalized_y) const {
201 InvertPolynomialDistortionModel(focal_length_x(),
202 focal_length_y(),
203 principal_point_x(),
204 principal_point_y(),
205 k1(), k2(), k3(),
206 p1(), p2(),
207 image_x,
208 image_y,
209 normalized_x,
210 normalized_y);
211 }
212
Pack(PackedIntrinsics * packed_intrinsics) const213 void PolynomialCameraIntrinsics::Pack(
214 PackedIntrinsics* packed_intrinsics) const {
215 CameraIntrinsics::Pack(packed_intrinsics);
216
217 packed_intrinsics->SetK1(k1());
218 packed_intrinsics->SetK2(k2());
219 packed_intrinsics->SetK3(k3());
220
221 packed_intrinsics->SetP1(p1());
222 packed_intrinsics->SetP2(p2());
223 }
224
Unpack(const PackedIntrinsics & packed_intrinsics)225 void PolynomialCameraIntrinsics::Unpack(
226 const PackedIntrinsics& packed_intrinsics) {
227 CameraIntrinsics::Unpack(packed_intrinsics);
228
229 SetRadialDistortion(packed_intrinsics.GetK1(),
230 packed_intrinsics.GetK2(),
231 packed_intrinsics.GetK3());
232
233 SetTangentialDistortion(packed_intrinsics.GetP1(),
234 packed_intrinsics.GetP2());
235 }
236
237 // Division model.
238
DivisionCameraIntrinsics()239 DivisionCameraIntrinsics::DivisionCameraIntrinsics()
240 : CameraIntrinsics() {
241 SetDistortion(0.0, 0.0);
242 }
243
DivisionCameraIntrinsics(const DivisionCameraIntrinsics & from)244 DivisionCameraIntrinsics::DivisionCameraIntrinsics(
245 const DivisionCameraIntrinsics &from)
246 : CameraIntrinsics(from) {
247 SetDistortion(from.k1(), from.k1());
248 }
249
SetDistortion(double k1,double k2)250 void DivisionCameraIntrinsics::SetDistortion(double k1,
251 double k2) {
252 parameters_[OFFSET_K1] = k1;
253 parameters_[OFFSET_K2] = k2;
254 ResetLookupGrids();
255 }
256
ApplyIntrinsics(double normalized_x,double normalized_y,double * image_x,double * image_y) const257 void DivisionCameraIntrinsics::ApplyIntrinsics(double normalized_x,
258 double normalized_y,
259 double *image_x,
260 double *image_y) const {
261 ApplyDivisionDistortionModel(focal_length_x(),
262 focal_length_y(),
263 principal_point_x(),
264 principal_point_y(),
265 k1(), k2(),
266 normalized_x,
267 normalized_y,
268 image_x,
269 image_y);
270 }
271
InvertIntrinsics(double image_x,double image_y,double * normalized_x,double * normalized_y) const272 void DivisionCameraIntrinsics::InvertIntrinsics(double image_x,
273 double image_y,
274 double *normalized_x,
275 double *normalized_y) const {
276 InvertDivisionDistortionModel(focal_length_x(),
277 focal_length_y(),
278 principal_point_x(),
279 principal_point_y(),
280 k1(), k2(),
281 image_x,
282 image_y,
283 normalized_x,
284 normalized_y);
285 }
286
Pack(PackedIntrinsics * packed_intrinsics) const287 void DivisionCameraIntrinsics::Pack(
288 PackedIntrinsics* packed_intrinsics) const {
289 CameraIntrinsics::Pack(packed_intrinsics);
290
291 packed_intrinsics->SetK1(k1());
292 packed_intrinsics->SetK2(k2());
293 }
294
Unpack(const PackedIntrinsics & packed_intrinsics)295 void DivisionCameraIntrinsics::Unpack(
296 const PackedIntrinsics& packed_intrinsics) {
297 CameraIntrinsics::Unpack(packed_intrinsics);
298
299 SetDistortion(packed_intrinsics.GetK1(), packed_intrinsics.GetK2());
300 }
301
302 // Nuke model.
303
NukeCameraIntrinsics()304 NukeCameraIntrinsics::NukeCameraIntrinsics()
305 : CameraIntrinsics() {
306 SetDistortion(0.0, 0.0);
307 }
308
NukeCameraIntrinsics(const NukeCameraIntrinsics & from)309 NukeCameraIntrinsics::NukeCameraIntrinsics(
310 const NukeCameraIntrinsics &from)
311 : CameraIntrinsics(from) {
312 SetDistortion(from.k1(), from.k2());
313 }
314
SetDistortion(double k1,double k2)315 void NukeCameraIntrinsics::SetDistortion(double k1, double k2) {
316 parameters_[OFFSET_K1] = k1;
317 parameters_[OFFSET_K2] = k2;
318 ResetLookupGrids();
319 }
320
ApplyIntrinsics(double normalized_x,double normalized_y,double * image_x,double * image_y) const321 void NukeCameraIntrinsics::ApplyIntrinsics(double normalized_x,
322 double normalized_y,
323 double *image_x,
324 double *image_y) const {
325 ApplyNukeDistortionModel(focal_length_x(),
326 focal_length_y(),
327 principal_point_x(),
328 principal_point_y(),
329 image_width(), image_height(),
330 k1(), k2(),
331 normalized_x,
332 normalized_y,
333 image_x,
334 image_y);
335 }
336
InvertIntrinsics(double image_x,double image_y,double * normalized_x,double * normalized_y) const337 void NukeCameraIntrinsics::InvertIntrinsics(double image_x,
338 double image_y,
339 double *normalized_x,
340 double *normalized_y) const {
341 InvertNukeDistortionModel(focal_length_x(),
342 focal_length_y(),
343 principal_point_x(),
344 principal_point_y(),
345 image_width(), image_height(),
346 k1(), k2(),
347 image_x,
348 image_y,
349 normalized_x,
350 normalized_y);
351 }
352
Pack(PackedIntrinsics * packed_intrinsics) const353 void NukeCameraIntrinsics::Pack(
354 PackedIntrinsics* packed_intrinsics) const {
355 CameraIntrinsics::Pack(packed_intrinsics);
356
357 packed_intrinsics->SetK1(k1());
358 packed_intrinsics->SetK2(k2());
359 }
360
Unpack(const PackedIntrinsics & packed_intrinsics)361 void NukeCameraIntrinsics::Unpack(
362 const PackedIntrinsics& packed_intrinsics) {
363 CameraIntrinsics::Unpack(packed_intrinsics);
364
365 SetDistortion(packed_intrinsics.GetK1(), packed_intrinsics.GetK2());
366 }
367
368 // Brown model.
369
BrownCameraIntrinsics()370 BrownCameraIntrinsics::BrownCameraIntrinsics()
371 : CameraIntrinsics() {
372 SetRadialDistortion(0.0, 0.0, 0.0, 0.0);
373 SetTangentialDistortion(0.0, 0.0);
374 }
375
BrownCameraIntrinsics(const BrownCameraIntrinsics & from)376 BrownCameraIntrinsics::BrownCameraIntrinsics(
377 const BrownCameraIntrinsics &from)
378 : CameraIntrinsics(from) {
379 SetRadialDistortion(from.k1(), from.k2(), from.k3(), from.k4());
380 SetTangentialDistortion(from.p1(), from.p2());
381 }
382
SetRadialDistortion(double k1,double k2,double k3,double k4)383 void BrownCameraIntrinsics::SetRadialDistortion(double k1,
384 double k2,
385 double k3,
386 double k4) {
387 parameters_[OFFSET_K1] = k1;
388 parameters_[OFFSET_K2] = k2;
389 parameters_[OFFSET_K3] = k3;
390 parameters_[OFFSET_K4] = k4;
391 ResetLookupGrids();
392 }
393
SetTangentialDistortion(double p1,double p2)394 void BrownCameraIntrinsics::SetTangentialDistortion(double p1,
395 double p2) {
396 parameters_[OFFSET_P1] = p1;
397 parameters_[OFFSET_P2] = p2;
398 ResetLookupGrids();
399 }
400
ApplyIntrinsics(double normalized_x,double normalized_y,double * image_x,double * image_y) const401 void BrownCameraIntrinsics::ApplyIntrinsics(double normalized_x,
402 double normalized_y,
403 double *image_x,
404 double *image_y) const {
405 ApplyBrownDistortionModel(focal_length_x(),
406 focal_length_y(),
407 principal_point_x(),
408 principal_point_y(),
409 k1(), k2(), k3(), k4(),
410 p1(), p2(),
411 normalized_x,
412 normalized_y,
413 image_x,
414 image_y);
415 }
416
InvertIntrinsics(double image_x,double image_y,double * normalized_x,double * normalized_y) const417 void BrownCameraIntrinsics::InvertIntrinsics(
418 double image_x,
419 double image_y,
420 double *normalized_x,
421 double *normalized_y) const {
422 InvertBrownDistortionModel(focal_length_x(),
423 focal_length_y(),
424 principal_point_x(),
425 principal_point_y(),
426 k1(), k2(), k3(), k4(),
427 p1(), p2(),
428 image_x,
429 image_y,
430 normalized_x,
431 normalized_y);
432 }
433
Pack(PackedIntrinsics * packed_intrinsics) const434 void BrownCameraIntrinsics::Pack(
435 PackedIntrinsics* packed_intrinsics) const {
436 CameraIntrinsics::Pack(packed_intrinsics);
437
438 packed_intrinsics->SetK1(k1());
439 packed_intrinsics->SetK2(k2());
440 packed_intrinsics->SetK3(k3());
441 packed_intrinsics->SetK4(k4());
442
443 packed_intrinsics->SetP1(p1());
444 packed_intrinsics->SetP2(p2());
445 }
446
Unpack(const PackedIntrinsics & packed_intrinsics)447 void BrownCameraIntrinsics::Unpack(
448 const PackedIntrinsics& packed_intrinsics) {
449 CameraIntrinsics::Unpack(packed_intrinsics);
450
451 SetRadialDistortion(packed_intrinsics.GetK1(),
452 packed_intrinsics.GetK2(),
453 packed_intrinsics.GetK3(),
454 packed_intrinsics.GetK4());
455
456 SetTangentialDistortion(packed_intrinsics.GetP1(),
457 packed_intrinsics.GetP2());
458 }
459
operator <<(std::ostream & os,const CameraIntrinsics & intrinsics)460 std::ostream& operator <<(std::ostream &os,
461 const CameraIntrinsics &intrinsics) {
462 if (intrinsics.focal_length_x() == intrinsics.focal_length_x()) {
463 os << "f=" << intrinsics.focal_length();
464 } else {
465 os << "fx=" << intrinsics.focal_length_x()
466 << " fy=" << intrinsics.focal_length_y();
467 }
468 os << " cx=" << intrinsics.principal_point_x()
469 << " cy=" << intrinsics.principal_point_y()
470 << " w=" << intrinsics.image_width()
471 << " h=" << intrinsics.image_height();
472
473 #define PRINT_NONZERO_COEFFICIENT(intrinsics, coeff) \
474 { \
475 if (intrinsics->coeff() != 0.0) { \
476 os << " " #coeff "=" << intrinsics->coeff(); \
477 } \
478 } (void) 0
479
480 switch (intrinsics.GetDistortionModelType()) {
481 case DISTORTION_MODEL_POLYNOMIAL:
482 {
483 const PolynomialCameraIntrinsics *polynomial_intrinsics =
484 static_cast<const PolynomialCameraIntrinsics *>(&intrinsics);
485 PRINT_NONZERO_COEFFICIENT(polynomial_intrinsics, k1);
486 PRINT_NONZERO_COEFFICIENT(polynomial_intrinsics, k2);
487 PRINT_NONZERO_COEFFICIENT(polynomial_intrinsics, k3);
488 PRINT_NONZERO_COEFFICIENT(polynomial_intrinsics, p1);
489 PRINT_NONZERO_COEFFICIENT(polynomial_intrinsics, p2);
490 break;
491 }
492 case DISTORTION_MODEL_DIVISION:
493 {
494 const DivisionCameraIntrinsics *division_intrinsics =
495 static_cast<const DivisionCameraIntrinsics *>(&intrinsics);
496 PRINT_NONZERO_COEFFICIENT(division_intrinsics, k1);
497 PRINT_NONZERO_COEFFICIENT(division_intrinsics, k2);
498 break;
499 }
500 case DISTORTION_MODEL_NUKE:
501 {
502 const NukeCameraIntrinsics *nuke_intrinsics =
503 static_cast<const NukeCameraIntrinsics *>(&intrinsics);
504 PRINT_NONZERO_COEFFICIENT(nuke_intrinsics, k1);
505 PRINT_NONZERO_COEFFICIENT(nuke_intrinsics, k2);
506 break;
507 }
508 case DISTORTION_MODEL_BROWN:
509 {
510 const BrownCameraIntrinsics *brown_intrinsics =
511 static_cast<const BrownCameraIntrinsics *>(&intrinsics);
512 PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k1);
513 PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k2);
514 PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k3);
515 PRINT_NONZERO_COEFFICIENT(brown_intrinsics, k4);
516 PRINT_NONZERO_COEFFICIENT(brown_intrinsics, p1);
517 PRINT_NONZERO_COEFFICIENT(brown_intrinsics, p2);
518 break;
519 }
520 default:
521 LOG(FATAL) << "Unknown distortion model.";
522 }
523
524 #undef PRINT_NONZERO_COEFFICIENT
525
526 return os;
527 }
528
529 } // namespace libmv
530