35 if(m_tempImage !=
nullptr)
36 cvReleaseImage(&m_tempImage);
47 float k1 = m_distortionMatrixData[0];
48 float k2 = m_distortionMatrixData[1];
49 float p1 = m_distortionMatrixData[2];
50 float p2 = m_distortionMatrixData[3];
54 dstd[0] = (src[0] - m_ccX) / m_fcX;
55 dstd[1] = (src[1] - m_ccY) / m_fcY;
62 for (
int iter = 0; iter < 5; iter++)
65 const float k_radial = 1 + k1 * r_2 + k2 * r_2 * r_2;
66 const float delta_x = 2 * p1*x*y + p2 * (r_2 + 2*x*x);
67 const float delta_y = 2 * p2*x*y + p1 * (r_2 + 2*y*y);
68 x = (desPnt[0] - delta_x) / k_radial;
69 y = (desPnt[1] - delta_y) / k_radial;
88 float distx = x + x*(k1*r_2 + k2*r_2*r_2) + (2*p1*x*y + p2*(r_2 + 2*x*x));
89 float disty = y + y*(k1*r_2 + k2*r_2*r_2) + (2*p2*x*y + p1*(r_2 + 2*y*y));
96 const float diffx = old_src[0] - distx;
97 const float diffy = old_src[1] - disty;
98 if (fabs(diffx) > .1 || fabs(diffy) > .1)
100 std::cout <<
"undistort sanity check error: diffx =" << diffx <<
" , diffy = " << diffy;
108 m_intrinsicMatrixData[0] = (double)m_fcX;
109 m_intrinsicMatrixData[1] = 0.0;
110 m_intrinsicMatrixData[2] = (double)m_ccX;
111 m_intrinsicMatrixData[3] = 0.0;
112 m_intrinsicMatrixData[4] = (double)m_fcY;
113 m_intrinsicMatrixData[5] = (double)m_ccY;
114 m_intrinsicMatrixData[6] = 0.0;
115 m_intrinsicMatrixData[7] = 0.0;
116 m_intrinsicMatrixData[8] = 1.0;
117 m_intrinsicMatrix = cvMat(3, 3, CV_32FC1, m_intrinsicMatrixData);
120 m_distortionMatrix = cvMat(1, 4, CV_32F, m_distortionMatrixData);
123 cvUndistort2(src,dst, &m_intrinsicMatrix, &m_distortionMatrix);
165 m_tempImage = cvCloneImage( src );
166 cvRemap(m_tempImage, src, m_mapX, m_mapY, CV_INTER_CUBIC);
167 cvReleaseImage( &m_tempImage );
168 m_tempImage =
nullptr;
174 cvRemap(src, dst, m_mapX, m_mapY, CV_INTER_CUBIC);
191 float in_dPrincipalX,
float in_dPrincipalY,
192 float in_Dist[4],
float ImageSizeX,
float ImageSizeY)
195 m_DistortionCoeffs = cvCreateMat(4, 1, CV_64FC1);
196 m_CameraMatrix = cvCreateMat(3, 3, CV_64FC1);
200 cvSetReal2D(m_CameraMatrix, 0, 0, in_dF1);
201 cvSetReal2D(m_CameraMatrix, 0, 1, 0.0);
202 cvSetReal2D(m_CameraMatrix, 0, 2, in_dPrincipalX);
204 cvSetReal2D(m_CameraMatrix, 1, 0, 0.0);
205 cvSetReal2D(m_CameraMatrix, 1, 1, in_dF2);
206 cvSetReal2D(m_CameraMatrix, 1, 2, in_dPrincipalY);
208 cvSetReal2D(m_CameraMatrix, 2, 0, 0.0);
209 cvSetReal2D(m_CameraMatrix, 2, 1, 0.0);
210 cvSetReal2D(m_CameraMatrix, 2, 2, 1.0);
213 cvSetReal1D(m_DistortionCoeffs, 0, in_Dist[0]);
214 cvSetReal1D(m_DistortionCoeffs, 1, in_Dist[1]);
215 cvSetReal1D(m_DistortionCoeffs, 2, in_Dist[2]);
216 cvSetReal1D(m_DistortionCoeffs, 3, in_Dist[3]);
218 m_mapX = cvCreateMat(ImageSizeY, ImageSizeX, CV_32FC1);
219 m_mapY = cvCreateMat(ImageSizeY, ImageSizeX, CV_32FC1);
222 cvInitUndistortMap(m_CameraMatrix, m_DistortionCoeffs, m_mapX, m_mapY);
virtual ~UndistortCameraImage()
void UndistortImage(IplImage *src, IplImage *dst)
void UndistortImageFast(IplImage *src, IplImage *dst=nullptr)
mitk::Point2D UndistortPixel(const mitk::Point2D &src)
USAGE ///.
void SetUndistortImageFastInfo(float in_dF1, float in_dF2, float in_dPrincipalX, float in_dPrincipalY, float in_Dist[4], float ImageSizeX, float ImageSizeY)