1 // This is core/vpgl/vpgl_radial_tangential_distortion.hxx
2 #ifndef vpgl_radial_tangential_distortion_hxx_
3 #define vpgl_radial_tangential_distortion_hxx_
4 //:
5 // \file
6
7 #include <cmath>
8 #include <limits>
9 #include <iostream>
10 #include "vpgl_radial_tangential_distortion.h"
11 #include <vgl/vgl_homg_point_2d.h>
12 #include <vgl/vgl_point_2d.h>
13
14 #ifdef _MSC_VER
15 # include <vcl_msvc_warnings.h>
16 #endif
17 template <class T>
apply_distortion(vgl_vector_2d<T> const & x) const18 vgl_vector_2d<T> vpgl_radial_tangential_distortion<T>::apply_distortion(vgl_vector_2d<T> const& x) const{
19 T rsq = x.length();// radial distance
20 rsq *= rsq;
21 T xi = x.x(), yi = x.y();//projection onto the image plane
22
23 size_t max_k = k_.size();
24 T num = T(1);
25 T den = T(1);
26 T rpow = rsq;
27
28 if(max_k <= 3){
29 for(size_t k = 0; k<max_k; ++k){
30 num += k_[k]*rpow;
31 rpow *= rsq;
32 }
33 }else if(max_k > 3 && max_k<=6){
34 for(size_t k = 0; k<3; ++ k){
35 num += k_[k]*rpow;
36 rpow *= rsq;
37 }
38 rpow = rsq;
39 for(size_t k = 3; k<max_k; ++k){
40 den += k_[k]*rpow;
41 rpow *= rsq;
42 }
43 }else{
44 std::cout << "more than 6 radial distortion coefficients - not valid nk = " << max_k << std::endl;
45 return vgl_vector_2d<T>(T(0), T(0));
46 }
47 T fr = num/den;
48 T x_xp = (xi*fr) + T(2)*p1_*xi*yi + p2_*(rsq + T(2)*xi*xi);
49 T x_yp = (yi*fr) + p1_*(rsq + T(2)*yi*yi) + T(2)*p2_*xi*yi;
50
51 return vgl_vector_2d<T>(x_xp, x_yp);
52 }
53 //: Distort a projected point on the image plane
54 // Calls the pure virtual radial distortion function
55 template <class T>
56 vgl_homg_point_2d<T>
distort(const vgl_homg_point_2d<T> & point) const57 vpgl_radial_tangential_distortion<T>::distort( const vgl_homg_point_2d<T>& point ) const
58 {
59 vgl_vector_2d<T> x = vgl_point_2d<T>(point) - center_;
60 vgl_vector_2d<T> xp = apply_distortion(x);
61 return vgl_homg_point_2d<T>(center_ + xp);
62 }
63
64
65 //: Return the original point that was distorted to this location (inverse of distort)
66 // \param init is an initial guess at the solution for the iterative solver
67 // if \p init is NULL then \p point is used as the initial guess
68 // calls the radial undistortion function
69 template <class T>
70 vgl_homg_point_2d<T>
undistort(const vgl_homg_point_2d<T> & point,const vgl_homg_point_2d<T> * init) const71 vpgl_radial_tangential_distortion<T>::undistort( const vgl_homg_point_2d<T>& point,
72 const vgl_homg_point_2d<T>* init ) const
73 {
74 vgl_point_2d<T> p(point);
75 vgl_vector_2d<T> pr = p-center_;
76 vgl_vector_2d<T> p0 = p-center_;
77 if(init)
78 p0 = vgl_point_2d<T>(*init)-center_;
79
80 // uses the Newton Method with finite differences for root finding
81 T eps = T(100)*std::numeric_limits<T>::epsilon();
82 T delta = 0.0001;
83 vgl_vector_2d<T> dpx(delta, T(0));
84 vgl_vector_2d<T> dpy(T(0), delta);
85 T large = std::numeric_limits<T>::max();
86 vgl_vector_2d<T> del(large, large);
87 unsigned int i = 0;
88 for (; i<100 && fabs(del.x())>eps && fabs(del.y())>eps ; ++i){
89 vgl_vector_2d<T> v0 = apply_distortion(p0);//input point predicted from current solution
90 del = pr-v0; // get the difference vector
91 //compute derivatives for Taylor series
92 vgl_vector_2d<T> dvx = (apply_distortion(p0+dpx)-v0)/delta;
93 vgl_vector_2d<T> dvy = (apply_distortion(p0+dpy)-v0)/delta;
94 // solve the linear system for the update to p0
95 T det = dvx.x()*dvy.y() - dvx.y()*dvy.x();
96 if(fabs(det)<eps){
97 std::cout << "singular system in undistort radial/tangential" << std::endl;
98 return point;
99 }
100 // the delta for new undistorted point relative to the previous solution
101 vgl_vector_2d<T> del_p0((dvy.y()*del.x() - dvx.y()*del.y()),
102 dvx.x()*del.y() - dvy.x()*del.x());
103 #if 0
104 std::cout << "pr (" << pr.x() << ' ' << pr.y() << " )" << std::endl;
105 std::cout << "v0( " << v0.x() << ' ' << v0.y() << " )" << std::endl;
106 std::cout << "pr-v0( " << del.x() << ' ' << del.y() << " )" << std::endl;
107 std::cout << "dx'/dx " << dvx.x() << " dx'/dy " << dvx.y() << std::endl;
108 std::cout << "dy'/dx " << dvy.x() << " dy'/dy " << dvy.y() << std::endl;
109 std::cout << "det " << det << std::endl;
110 #endif
111 del_p0/=det;
112 p0 += del_p0;
113 #if 0
114 std::cout << "p-p0 " << del_p0.x() << ' ' << del_p0.y() << " )" << std::endl;
115 std::cout << "v0+ (" << p0.x() << ' ' << p0.y() << " )" << std::endl;
116 std::cout << "pr (" << pr.x() << ' ' << pr.y() << " )" << std::endl;
117 #endif
118 }
119 if (i >= 100) {
120 std::cout << "Newton's methhod failed to converge in undistort" << std::endl;
121 return vgl_homg_point_2d<T>(0.0, 0.0, 0.0);//ideal point
122 }
123 vgl_point_2d<T> temp = center_+p0;
124 return vgl_homg_point_2d<T>(temp);
125 }
126
127 // Code for easy instantiation.
128 #undef vpgl_RADIAL_TANGENTIAL_DISTORTION_INSTANTIATE
129 #define vpgl_RADIAL_TANGENTIAL_DISTORTION_INSTANTIATE(T) \
130 template class vpgl_radial_tangential_distortion<T>
131
132 #endif // vpgl_radial_tangential_distortion_hxx_
133