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