43 ApolloMetricDistortionMap::ApolloMetricDistortionMap(
Camera *parent,
47 double j2,
double t0) :
80 double rr = x * x + y * y;
81 double rrrr = rr * rr;
86 double dr = 1 +
p_k1 * rr +
p_k2 * rrrr +
p_k3 * rr * rrrr;
90 p_undistortedFocalPlaneX = dr * x - dt * sin(
p_t0);
91 p_undistortedFocalPlaneY = dr * y + dt * cos(
p_t0);
111 p_undistortedFocalPlaneX = ux;
112 p_undistortedFocalPlaneY = uy;
117 double xx, yy, rr, rrrr, dr;
118 double xdistortion, ydistortion;
119 double xdistorted, ydistorted;
120 double xprevious, yprevious;
126 xprevious = 1000000.0;
127 yprevious = 1000000.0;
129 double tolerance = 0.000001;
131 bool bConverged =
false;
136 for(
int i = 0; i < 50; i++) {
151 xdistortion = xt * dr - dt * sin(
p_t0);
152 ydistortion = yt * dr + dt * cos(
p_t0);
155 xt = ux - xdistortion;
156 yt = uy - ydistortion;
159 xdistorted = xt +
p_xp;
160 ydistorted = yt +
p_yp;
163 if((fabs(xt - xprevious) <= tolerance) && (fabs(yt - yprevious) <= tolerance)) {
173 p_focalPlaneX = xdistorted;
174 p_focalPlaneY = ydistorted;