| 155 | } |
| 156 | |
| 157 | Imath::V2d StandardRadialLensModel::distort( Imath::V2d p ) |
| 158 | { |
| 159 | Imath::V2d dn( UVtoDN( p ) ); |
| 160 | const Imath::V2d dnl( dn ); |
| 161 | |
| 162 | // Use Newtons method to derive the value of the undistorted point... |
| 163 | for (unsigned int i = 0; i < 15; i++) |
| 164 | { |
| 165 | // Calculate the first derivative matrix. |
| 166 | Imath::M33d fd; |
| 167 | const double dnx2( dn.x*dn.x ), dny2( dn.y*dn.y ); |
| 168 | const double dnx4( dnx2*dnx2 ), dny4( dny2*dny2 ); |
| 169 | |
| 170 | fd[0][0] = 1.0 + 3.0*m_cxx*dnx2 + m_cxy*dn.y*dn.y + 5.*m_cxxx*dnx4 + 3.*m_cxxy*dnx2*dny2 + m_cxyy*dny4; |
| 171 | fd[1][0] = 2.0*m_cxy*dn.x*dn.y + 2.*m_cxxy*dnx2*dn.x*dn.y + 4.*m_cxyy*dny2*dn.y*dn.x; |
| 172 | fd[0][1] = 2.0*m_cyx*dn.x*dn.y + 2.*m_cyyx*dny2*dn.y*dn.x + 4.*m_cyxx*dnx2*dn.x*dn.y; |
| 173 | fd[1][1] = 1.0 + 3.0*m_cyy*dny2 + m_cyx*dnx2 + 5.*m_cyyy*dny4 + 3.*m_cyyx*dnx2*dny2 + m_cyxx*dnx4; |
| 174 | |
| 175 | Imath::V2d fDist; |
| 176 | fDist.x = dn.x * (1. + m_cxx*dnx2 + m_cxy*dny2 + m_cxxx*dnx4 + m_cxxy*dnx2*dny2 + m_cxyy*dny4); |
| 177 | fDist.y = dn.y * (1. + m_cyx*dnx2 + m_cyy*dny2 + m_cyxx*dnx4 + m_cyyx*dnx2*dny2 + m_cyyy*dny4); |
| 178 | |
| 179 | dn -= (fDist-dnl)*fd.gjInverse(); |
| 180 | } |
| 181 | |
| 182 | return DNtoUV( dn ); |
| 183 | } |
| 184 | |
| 185 | // Transforms UV coordinates in the range 0-1 to the dimesionless |
| 186 | // coordinates which are used by the distortion algorithm. |