diff python/cvutils.py @ 929:be28a3538dc9

work in progress on projection
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 12 Jul 2017 18:00:53 -0400
parents 063d1267585d
children 7db0f2853bfd
line wrap: on
line diff
--- a/python/cvutils.py	Wed Jul 12 01:24:31 2017 -0400
+++ b/python/cvutils.py	Wed Jul 12 18:00:53 2017 -0400
@@ -264,9 +264,9 @@
         if obj.hasFeatures():
             for f in obj.getFeatures():
                 if f.existsAtInstant(frameNum):
-                    projectedPosition = f.projectPositions[:, frameNum-f.getFirstInstant()]
-                    x.append(projectedPosition[0])
-                    y.append(projectedPosition[1])
+                    p = f.getPositionAtInstant(frameNum)
+                    x.append(p.x)
+                    y.append(p.y)
         xmin = min(x)
         xmax = max(x)
         ymin = min(y)
@@ -521,31 +521,32 @@
 def projectArray(homography, points, intrinsicCameraMatrix = None, distortionCoefficients = None):
     '''Returns the coordinates of the projected points through homography
     (format: array 2xN points)'''
-    if points.shape[0] not in [2, 3]:
+    if points.shape[0] != 2:
         raise Exception('points of dimension {0} {1}'.format(points.shape[0], points.shape[1]))
 
-    augmentedPoints = append(points,[[1]*points.shape[1]], 0)
+    augmentedPoints = append(points,[[1]*points.shape[1]], 0) # 3xN
     if homography is not None and homography.size>0:
         prod = dot(homography, augmentedPoints)
         projected = prod/prod[2]
-        projected[3,:] = 0
     else:
         projected = augmentedPoints
 
     if intrinsicCameraMatrix is not None and distortionCoefficients is not None:
-        projected = cv2.projectPoints(projected, None, None, intrinsicCameraMatrix, distortionCoefficients)
-    return projected
+        #projected[2,:] = 0
+        projected, jacobian = cv2.projectPoints(projected.T, (0,0,0), (0,0,0), intrinsicCameraMatrix, distortionCoefficients) # in: 3xN, out: 2x1xN
+        projected = projected.reshape(-1,2).T
+    return projected[:2,:]
 
-def project(homography, p):
+def project(homography, p, intrinsicCameraMatrix = None, distortionCoefficients = None):
     '''Returns the coordinates of the projection of the point p with coordinates p[0], p[1]
     through homography'''
-    return projectArray(homography, array([[p[0]],[p[1]]]))
+    return projectArray(homography, array([[p[0]],[p[1]]]), intrinsicCameraMatrix, distortionCoefficients)
 
-def projectTrajectory(homography, trajectory):
+def projectTrajectory(homography, trajectory, intrinsicCameraMatrix = None, distortionCoefficients = None):
     '''Projects a series of points in the format
     [[x1, x2, ...],
     [y1, y2, ...]]'''
-    return projectArray(homography, array(trajectory))
+    return projectArray(homography, array(trajectory), intrinsicCameraMatrix, distortionCoefficients)
 
 def invertHomography(homography):
     '''Returns an inverted homography