From 3609343acfcba1393632266aaf29582779d746bc Mon Sep 17 00:00:00 2001
From: Pavel Rojtberg <pavel.rojtberg@igd.fraunhofer.de>
Date: Wed, 9 Nov 2016 14:19:23 +0100
Subject: [PATCH] undistortPoints: only consider distCoeffs if present

iters should be 0 if we have no distortion. Also skip tilt distortion in
that case.
Furthermore move variable declarations to usage sites.
---
 modules/imgproc/src/undistort.cpp | 60 +++++++++++++++----------------
 1 file changed, 28 insertions(+), 32 deletions(-)

diff --git a/modules/imgproc/src/undistort.cpp b/modules/imgproc/src/undistort.cpp
index 198b0cbf9b..127481fe0b 100644
--- a/modules/imgproc/src/undistort.cpp
+++ b/modules/imgproc/src/undistort.cpp
@@ -280,16 +280,9 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
                    const CvMat* _distCoeffs,
                    const CvMat* matR, const CvMat* matP )
 {
-    double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
+    double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
     CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
     CvMat _RR=cvMat(3, 3, CV_64F, RR);
-    const CvPoint2D32f* srcf;
-    const CvPoint2D64f* srcd;
-    CvPoint2D32f* dstf;
-    CvPoint2D64f* dstd;
-    int stype, dtype;
-    int sstep, dstep;
-    int i, j, n, iters = 1;
     cv::Matx33d invMatTilt = cv::Matx33d::eye();
 
     CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
@@ -304,6 +297,8 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
 
     cvConvert( _cameraMatrix, &matA );
 
+    int iters = 0;
+
     if( _distCoeffs )
     {
         CV_Assert( CV_IS_MAT(_distCoeffs) &&
@@ -340,27 +335,26 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
         cvMatMul( &_PP, &_RR, &_RR );
     }
 
-    srcf = (const CvPoint2D32f*)_src->data.ptr;
-    srcd = (const CvPoint2D64f*)_src->data.ptr;
-    dstf = (CvPoint2D32f*)_dst->data.ptr;
-    dstd = (CvPoint2D64f*)_dst->data.ptr;
-    stype = CV_MAT_TYPE(_src->type);
-    dtype = CV_MAT_TYPE(_dst->type);
-    sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
-    dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
+    const CvPoint2D32f* srcf = (const CvPoint2D32f*)_src->data.ptr;
+    const CvPoint2D64f* srcd = (const CvPoint2D64f*)_src->data.ptr;
+    CvPoint2D32f* dstf = (CvPoint2D32f*)_dst->data.ptr;
+    CvPoint2D64f* dstd = (CvPoint2D64f*)_dst->data.ptr;
+    int stype = CV_MAT_TYPE(_src->type);
+    int dtype = CV_MAT_TYPE(_dst->type);
+    int sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
+    int dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
 
-    n = _src->rows + _src->cols - 1;
+    double fx = A[0][0];
+    double fy = A[1][1];
+    double ifx = 1./fx;
+    double ify = 1./fy;
+    double cx = A[0][2];
+    double cy = A[1][2];
 
-    fx = A[0][0];
-    fy = A[1][1];
-    ifx = 1./fx;
-    ify = 1./fy;
-    cx = A[0][2];
-    cy = A[1][2];
-
-    for( i = 0; i < n; i++ )
+    int n = _src->rows + _src->cols - 1;
+    for( int i = 0; i < n; i++ )
     {
-        double x, y, x0, y0;
+        double x, y, x0 = 0, y0 = 0;
         if( stype == CV_32FC2 )
         {
             x = srcf[i*sstep].x;
@@ -375,14 +369,16 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
         x = (x - cx)*ifx;
         y = (y - cy)*ify;
 
-        // compensate tilt distortion
-        cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
-        double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
-        x0 = x = invProj * vecUntilt(0);
-        y0 = y = invProj * vecUntilt(1);
+        if( iters ) {
+            // compensate tilt distortion
+            cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
+            double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
+            x0 = x = invProj * vecUntilt(0);
+            y0 = y = invProj * vecUntilt(1);
+        }
 
         // compensate distortion iteratively
-        for( j = 0; j < iters; j++ )
+        for( int j = 0; j < iters; j++ )
         {
             double r2 = x*x + y*y;
             double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);