1 // This is core/vpgl/vpgl_lens_distortion.hxx
2 #ifndef vpgl_lens_distortion_hxx_
3 #define vpgl_lens_distortion_hxx_
4 //:
5 // \file
6
7 #include "vpgl_lens_distortion.h"
8 #include <vgl/vgl_homg_point_2d.h>
9 #include <vgl/vgl_point_2d.h>
10 #if 0
11 #include <vgl/vgl_vector_2d.h>
12 #endif
13 #include <cassert>
14 #ifdef _MSC_VER
15 # include <vcl_msvc_warnings.h>
16 #endif
17
18
19 template <class T>
distort_pixel(const vgl_homg_point_2d<T> & pixel,const vpgl_calibration_matrix<T> & K) const20 vgl_homg_point_2d<T> vpgl_lens_distortion<T>::distort_pixel(const vgl_homg_point_2d<T>& pixel, const vpgl_calibration_matrix<T>& K) const{
21 // convert image coordinates to focal plane coordinates
22 T f = K.focal_length();
23 T ax = K.x_scale(), ay = K.y_scale();
24 vgl_point_2d<T> pp = K.principal_point();
25 T sk = K.skew();
26 vgl_point_2d<T> c(pp.x(), pp.y());
27 vgl_point_2d<T> pix(pixel), pix_d;
28 // apply the inverse of the calibration matrix
29 vgl_vector_2d<T> xvu = pix - c, xv;
30 vgl_homg_point_2d<T> hux, hx;
31 if(sk == T(0))
32 hux.set(xvu.x()/(ax*f), xvu.y()/(ay*f));
33 else
34 hux.set((xvu.x()/(ax*f) - sk*xvu.y()/(ax*ay*f*f)), xvu.y()/(ay*f));
35
36 // hux is now in focal plane coordinates so distortion can be applied
37 hx = this->distort(hux);
38
39 // convert back to pixel coordinates
40 if(sk == T(0))
41 xv.set(hx.x()*(ax*f), hx.y()*(ay*f));
42 else
43 xv.set((hx.x()*(ax*f)+sk*hx.y()), hx.y()*(ay*f));
44 pix_d = c + xv;
45 return vgl_homg_point_2d<T>(pix_d.x(), pix_d.y());
46 }
47
48
49 template <class T>
50 vgl_homg_point_2d<T>
undistort_pixel(const vgl_homg_point_2d<T> & distorted_pixel,const vpgl_calibration_matrix<T> & K) const51 vpgl_lens_distortion<T>::undistort_pixel(const vgl_homg_point_2d<T>& distorted_pixel, const vpgl_calibration_matrix<T>& K) const{
52 // convert image coordinates to focal plane coordinates
53 T f = K.focal_length();
54 T ax = K.x_scale(), ay = K.y_scale();
55 vgl_point_2d<T> pp = K.principal_point();
56 T sk = K.skew();
57 vgl_point_2d<T> c(pp.x(), pp.y());
58 vgl_point_2d<T> pix_d(distorted_pixel), pix_ud;
59 // apply the inverse of the calibration matrix
60 vgl_vector_2d<T> xv = pix_d - c, xvu;
61 vgl_homg_point_2d<T> hx, hux;
62 if(sk == T(0))
63 hx.set(xv.x()/(ax*f), xv.y()/(ay*f));
64 else
65 hx.set((xv.x()/(ax*f) - sk*xv.y()/(ax*ay*f*f)), xv.y()/(ay*f));
66
67 // hx is now in focal plane coordinates so undistort
68 hux = this->undistort(hx);
69
70 // convert back to pixel coordinates
71 if(sk == T(0))
72 xvu.set(hux.x()*(ax*f), hux.y()*(ay*f));
73 else
74 xvu.set((hux.x()*(ax*f)+sk*hux.y()), hux.y()*(ay*f));
75 pix_ud = c + xvu;
76 return vgl_homg_point_2d<T>(pix_ud.x(), pix_ud.y());
77 }
78
79 // Code for easy instantiation.
80 #undef vpgl_LENS_DISTORTION_INSTANTIATE
81 #define vpgl_LENS_DISTORTION_INSTANTIATE(T) \
82 template class vpgl_lens_distortion<T >
83
84 #endif // vpgl_lens_distortion_hxx_
85