diff python/cvutils.py @ 930:7db0f2853bfd

corrected projection back to image space
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 13 Jul 2017 00:31:54 -0400
parents be28a3538dc9
children 8148991b1dab
line wrap: on
line diff
--- a/python/cvutils.py	Wed Jul 12 18:00:53 2017 -0400
+++ b/python/cvutils.py	Thu Jul 13 00:31:54 2017 -0400
@@ -22,6 +22,7 @@
 from math import floor, log10, ceil
 
 from numpy import dot, array, append, float32, loadtxt, savetxt, append, zeros, ones, identity, abs as npabs, logical_and, unravel_index, sum as npsum, isnan, mgrid, median, floor as npfloor, ceil as npceil
+from numpy.linalg import inv
 from matplotlib.mlab import find
 from matplotlib.pyplot import imread, imsave
 
@@ -518,7 +519,7 @@
             out.write('{0} '.format(cvmat[i,j]))
         out.write('\n')
 
-def projectArray(homography, points, intrinsicCameraMatrix = None, distortionCoefficients = None):
+def projectArray(homography, points, intrinsicCameraMatrix = None, distortionCoefficients = None, newCameraMatrix = None):
     '''Returns the coordinates of the projected points through homography
     (format: array 2xN points)'''
     if points.shape[0] != 2:
@@ -532,10 +533,14 @@
         projected = augmentedPoints
 
     if intrinsicCameraMatrix is not None and distortionCoefficients is not None:
-        #projected[2,:] = 0
-        projected, jacobian = cv2.projectPoints(projected.T, (0,0,0), (0,0,0), intrinsicCameraMatrix, distortionCoefficients) # in: 3xN, out: 2x1xN
+        if newCameraMatrix is not None:
+            invNewCameraMatrix = inv(newCameraMatrix)
+            reducedPoints = dot(invNewCameraMatrix, projected)
+        else:
+            reducedPoints = projected
+        projected, jacobian = cv2.projectPoints(reducedPoints.T, (0.,0.,0.), (0.,0.,0.), intrinsicCameraMatrix, distortionCoefficients) # in: 3xN, out: 2x1xN
         projected = projected.reshape(-1,2).T
-    return projected[:2,:]
+        return projected[:2,:]
 
 def project(homography, p, intrinsicCameraMatrix = None, distortionCoefficients = None):
     '''Returns the coordinates of the projection of the point p with coordinates p[0], p[1]
@@ -551,7 +556,6 @@
 def invertHomography(homography):
     '''Returns an inverted homography
     Unnecessary for reprojection over camera image'''
-    from numpy.linalg import inv
     invH = inv(homography)
     invH /= invH[2,2]
     return invH