Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
mitkUndistortCameraImage.cpp
Go to the documentation of this file.
1 /*===================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center,
6 Division of Medical and Biological Informatics.
7 All rights reserved.
8 
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
11 A PARTICULAR PURPOSE.
12 
13 See LICENSE.txt or http://www.mitk.org for details.
14 
15 ===================================================================*/
16 
17 
23 
24 
26 
27 
29 {
30  m_tempImage = nullptr;
31 
32 }
34 {
35  if(m_tempImage != nullptr)
36  cvReleaseImage(&m_tempImage);
37 }
38 
39 
42 {
43  float r_2 = 0; // radial distance squared
44  const mitk::Point2D old_src = src; // copy of the original distorted point
45 
46  // distortion coefficients
47  float k1 = m_distortionMatrixData[0];
48  float k2 = m_distortionMatrixData[1];
49  float p1 = m_distortionMatrixData[2];
50  float p2 = m_distortionMatrixData[3];
51 
52  // Shift points to principal point and use focal length
53  mitk::Point2D dstd;
54  dstd[0] = (src[0] - m_ccX) / m_fcX;
55  dstd[1] = (src[1] - m_ccY) / m_fcY;
56  mitk::Point2D desPnt = dstd;
57 
58  // Compensate lens distortion
59  float x = dstd[0];
60  float y = dstd[1];
61 
62  for (int iter = 0; iter < 5; iter++)
63  {
64  r_2 = x*x + y*y;
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;
70  }
71  dstd[0] = x;
72  dstd[1] = y;
73  dstd[0] *= m_fcX;
74  dstd[1] *= m_fcY;
75  dstd[0] += m_ccX;
76  dstd[1] += m_ccY;
77 
78  // ready
79  // const mitk::Point2D dst = dstd;
80 
81  // do a sanity check to make sure this ideal point translates properly to the distorted point
82  // this does the reverse of the above. It maps ideal undistorted to distorted image coordinates
83  x = dstd[0] - m_ccX;
84  y = dstd[1] - m_ccY;
85  x /= m_fcX;
86  y /= m_fcY;
87  r_2 = x*x + y*y;
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));
90  distx *= m_fcX;
91  disty *= m_fcY;
92  distx += m_ccX;
93  disty += m_ccY;
94 
95  // this should never be more than .2 pixels...
96  const float diffx = old_src[0] - distx;
97  const float diffy = old_src[1] - disty;
98  if (fabs(diffx) > .1 || fabs(diffy) > .1)
99  {
100  std::cout << "undistort sanity check error: diffx =" << diffx << " , diffy = " << diffy;
101  }
102  return dstd;
103 }
104 
105 void mitk::UndistortCameraImage::UndistortImage(IplImage *src, IplImage *dst)
106 {
107  // init intrinsic camera matrix [fx 0 cx; 0 fy cy; 0 0 1].
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);
118 
119  // init distortion matrix
120  m_distortionMatrix = cvMat(1, 4, CV_32F, m_distortionMatrixData);
121 
122  // undistort
123  cvUndistort2(src,dst, &m_intrinsicMatrix, &m_distortionMatrix);
124 }
125 
126 
127 // FAST METHODS FOR UNDISTORTION IN REALTIME //
128 
129 void mitk::UndistortCameraImage::UndistortImageFast(IplImage * src, IplImage* dst)
130 {
131  if(!src)
132  return;
133 
134  /*if(dst == NULL)
135  dst = src;
136 
137  if(src->nChannels == 3)
138  {
139  IplImage *r = cvCreateImage(cvGetSize(src),src->depth,1);//subpixel
140  IplImage *g = cvCreateImage(cvGetSize(src),src->depth,1);//subpixel
141  IplImage *b = cvCreateImage(cvGetSize(src),src->depth,1);//subpixel
142 
143  cvSplit(src, r,g,b, NULL);
144 
145  cvRemap( r, r, m_mapX, m_mapY ); // Undistort image
146  cvRemap( g, g, m_mapX, m_mapY ); // Undistort image
147  cvRemap( b, b, m_mapX, m_mapY ); // Undistort image
148 
149  cvMerge(r,g,b, NULL, dst);
150  }
151  else
152  {
153  cvRemap(src, dst, m_mapX, m_mapY);
154  }*/
155 
156 
157  /*if(m_tempImage == NULL)
158  m_tempImage = cvCreateImage(cvSize(src->width,src->height),src->depth,src->nChannels);*/
159 
160  /*if(dst == NULL)
161  dst = cvCreateImage(cvSize(src->width,src->height),src->depth,src->nChannels);*/
162 
163  if(!dst)
164  {
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;
169  /*memcpy( src->imageData, m_tempImage->imageData, m_tempImage->imageSize );
170  cvReleaseImage( &m_tempImage );*/
171  }
172  else
173  {
174  cvRemap(src, dst, m_mapX, m_mapY, CV_INTER_CUBIC);
175  }
176 
177  /*m_tempImage->origin = src->origin;
178 
179  if(dst == NULL)
180  memcpy( src->imageData, m_tempImage->imageData, m_tempImage->imageSize );
181  else
182  memcpy( dst->imageData, m_tempImage->imageData, m_tempImage->imageSize );
183 
184  //cvUnDistort(m_srcImg, m_dstImg, m_undistMap,m_interpolationMode);
185  //cvUndistort2(m_srcImg, m_dstImg, &m_intrinsicMatrix,&m_distortionMatrixDataCoefficients);*/
186 }
187 
188 
189 
191  float in_dPrincipalX, float in_dPrincipalY,
192  float in_Dist[4], float ImageSizeX, float ImageSizeY)
193 {
194  //create new matrix
195  m_DistortionCoeffs = cvCreateMat(4, 1, CV_64FC1);
196  m_CameraMatrix = cvCreateMat(3, 3, CV_64FC1);
197 
198 
199  //set the camera matrix [fx 0 cx; 0 fy cy; 0 0 1].
200  cvSetReal2D(m_CameraMatrix, 0, 0, in_dF1);
201  cvSetReal2D(m_CameraMatrix, 0, 1, 0.0);
202  cvSetReal2D(m_CameraMatrix, 0, 2, in_dPrincipalX);
203 
204  cvSetReal2D(m_CameraMatrix, 1, 0, 0.0);
205  cvSetReal2D(m_CameraMatrix, 1, 1, in_dF2);
206  cvSetReal2D(m_CameraMatrix, 1, 2, in_dPrincipalY);
207 
208  cvSetReal2D(m_CameraMatrix, 2, 0, 0.0);
209  cvSetReal2D(m_CameraMatrix, 2, 1, 0.0);
210  cvSetReal2D(m_CameraMatrix, 2, 2, 1.0);
211 
212  //set distortions coefficients
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]);
217 
218  m_mapX = cvCreateMat(ImageSizeY, ImageSizeX, CV_32FC1);
219  m_mapY = cvCreateMat(ImageSizeY, ImageSizeX, CV_32FC1);
220 
221  //cv::initUndistortRectifyMap(m_CameraMatrix, m_DistortionCoeffs, m_mapX, m_mapY);
222  cvInitUndistortMap(m_CameraMatrix, m_DistortionCoeffs, m_mapX, m_mapY);
223 }
224 
225 
226 
227 
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)