1 // This is core/vpgl/vpgl_radial_tangential_distortion.h
2 #ifndef vpgl_radial_tangential_distortion_h_
3 #define vpgl_radial_tangential_distortion_h_
4 //:
5 // \file
6 // \brief The standard distortion model including radial and tangential distortion
7 // \author J.L. Mundy
8 // \date March 2, 2019
9 //
10 //   It is typical to account for both radial and tangential distortion in camera calibration
11 //   The general form is as follows
12 // (see opencv https://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=projectpoints)
13 //
14 //          (1 + k1*r^2 + k2*r^4 + k3*r^6)
15 //   x' = x ------------------------------  + 2p1(x*y) + p2(r^2 + 2x^2)
16 //          (1 + k4*r^2 + k5*r*6 + k6*r*6)
17 //
18 //
19 //          (1 + k1*r^2 + k2*r^4 + k3*r^6)
20 //   y' = y ------------------------------  + p1(r^2 + 2y^2) 2p2(x*y) +
21 //          (1 + k4*r^2 + k5*r*6 + k6*r*6)
22 //
23 //  note that (x, y) are focal plane coordinates and (x', y') are distorted focal plane coordinates.
24 //
25 //     u        x'
26 //     v   = K  y' , where K is the calibration matrix
27 //     1        1
28 //
29 //                                      wx             X
30 // To apply lens distortion,    compute wy = [ R | t ] Y
31 //                                      w              Z
32 //                                                     1
33 // compute x'(x, y)  and y'(x, y) from above, and then apply the calibration matrix to determine u, v.
34 //
35 // To undistort an image, apply the inverse of the calibration matrix to compute x' and y'
36 // non-linearly solve the distortion equations for x and y (the undistorted projection onto the image plane)
37 // Determine undistorted (u, v) from (x, y) as,
38 //
39 //     u        x
40 //     v   = K  y , where K is the calibration matrix
41 //     1        1
42 // Note that in other vpgl lens distortion implementations, e.g. vpgl_poly_radial_distortion only k coeficients are specified and in terms of image
43 // coordinates not focal plane coordinates, as assumed in the standard definition. However, by
44 // by specifying a center of distortion the correction can work similarly, but with modified coefficients.
45 //
46 #include "vpgl_lens_distortion.h"
47 #include <vgl/vgl_point_2d.h>
48 #include <vgl/vgl_vector_2d.h>
49 #include <vgl/vgl_homg_point_2d.h>
50 #include "vpgl_calibration_matrix.h"
51 
52 template <class T>
53 class vpgl_radial_tangential_distortion : public vpgl_lens_distortion<T>
54 {
55  public:
56   //: Constructor
vpgl_radial_tangential_distortion(const std::vector<T> k,T p1,T p2)57  vpgl_radial_tangential_distortion(const std::vector<T> k, T p1, T p2)
58    : center_(vgl_point_2d<T>(T(0),T(0))), k_(k), p1_(p1), p2_(p2) {}
59 
vpgl_radial_tangential_distortion(vgl_point_2d<T> const & center,std::vector<T> k,T p1,T p2)60  vpgl_radial_tangential_distortion(vgl_point_2d<T> const& center, std::vector<T> k, T p1, T p2)
61    : center_(center), k_(k), p1_(p1), p2_(p2){}
62 
63   //: Distort a projected point on the image plane
64   //  Calls the pure virtual radial distortion function
65   vgl_homg_point_2d<T> distort( const vgl_homg_point_2d<T>& point ) const override;
66 
67   //: Return the original point that was distorted to this location (inverse of distort)
68   // \param init is an initial guess at the solution for the iterative solver
69   // if \p init is NULL then \p point is used as the initial guess
70   // calls the radial undistortion function
71   vgl_homg_point_2d<T> undistort( const vgl_homg_point_2d<T>& point,
72                                           const vgl_homg_point_2d<T>* init=nullptr) const override;
73 
74   //: implementation of distortion in the pixel coordinate system
distort_pixel(const vgl_homg_point_2d<T> & pixel,const vpgl_calibration_matrix<T> & K)75   vgl_homg_point_2d<T> distort_pixel(const vgl_homg_point_2d<T>& pixel, const vpgl_calibration_matrix<T>& K) const override {
76     vgl_homg_point_2d<T> cen_pixel(pixel.x()-center_.x(), pixel.y()-center_.y());
77     vgl_homg_point_2d<T> ret = vpgl_lens_distortion<T>::distort_pixel(cen_pixel, K);
78     return ret;
79   }
80 
81   //: implementation of undistortion in the pixel coordinate system
undistort_pixel(const vgl_homg_point_2d<T> & distorted_pixel,const vpgl_calibration_matrix<T> & K)82   vgl_homg_point_2d<T> undistort_pixel(const vgl_homg_point_2d<T>& distorted_pixel, const vpgl_calibration_matrix<T>& K) const override {
83     vgl_homg_point_2d<T> ret = vpgl_lens_distortion<T>::undistort_pixel(distorted_pixel, K);
84     ret.set(ret.x()+center_.x(), ret.y() + center_.y());
85     return ret;
86   }
87 
88   //: do nothing
89   //: Set a translation to apply before of after distortion
90   // This is needed when distorting an image to translate the resulting image
91   // such that all points have positive indices
92   void set_translation(const vgl_vector_2d<T> &offset,
93                        bool /*after*/ = true) override {
94     center_ += offset;
95   }
96 
97  protected:
98   vgl_vector_2d<T> apply_distortion(vgl_vector_2d<T> const& x) const;
99   //: The center of distortion
100   vgl_point_2d<T> center_;
101 
102   //: the k coefficients (radial distortion)
103   std::vector<T> k_;
104 
105   //: the p1, p2 coefficients (tangential distortion)
106   T p1_;
107   T p2_;
108 
109 };
110 
111 #endif // vpgl_radial_tangential_distortion_h_
112