Mercurial Hosting > traffic-intelligence

annotate scripts/compute-homography.py @ 443:51810d737d86

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
timeout in compute-homography.py for Paul
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 27 Jan 2014 01:21:56 -0500
parents 5f75d6c23ed5
children 6551a3cf1750
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
1 #! /usr/bin/env python
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
2
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
3 import sys,getopt
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
4
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
5 import matplotlib.pyplot as plt
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
6 import numpy as np
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
7 import cv2
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
8
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
9 import cvutils
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
10 import utils
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
11
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
12 options, args = getopt.getopt(sys.argv[1:], 'hp:i:w:n:u:',['help'])
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
13 options = dict(options)
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
14
217
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
15 # TODO process camera intrinsic and extrinsic parameters to obtain image to world homography, taking example from Work/src/python/generate-homography.py script
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
16 # cameraMat = load(videoFilenamePrefix+'-camera.txt');
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
17 # T1 = cameraMat[3:6,:].copy();
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
18 # A = cameraMat[0:3,0:3].copy();
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
19
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
20 # # pay attention, rotation may be the transpose
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
21 # # R = T1[:,0:3].T;
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
22 # R = T1[:,0:3];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
23 # rT = dot(R, T1[:,3]/1000);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
24 # T = zeros((3,4),'f');
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
25 # T[:,0:3] = R[:];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
26 # T[:,3] = rT;
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
27
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
28 # AT = dot(A,T);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
29
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
30 # nPoints = 4;
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
31 # worldPoints = cvCreateMat(nPoints, 3, CV_64FC1);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
32 # imagePoints = cvCreateMat(nPoints, 3, CV_64FC1);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
33
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
34 # # extract homography from the camera calibration
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
35 # worldPoints = cvCreateMat(4, 3, CV_64FC1);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
36 # imagePoints = cvCreateMat(4, 3, CV_64FC1);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
37
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
38 # worldPoints[0,:] = [[1, 1, 0]];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
39 # worldPoints[1,:] = [[1, 2, 0]];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
40 # worldPoints[2,:] = [[2, 1, 0]];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
41 # worldPoints[3,:] = [[2, 2, 0]];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
42
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
43 # wPoints = [[1,1,2,2],
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
44 # [1,2,1,2],
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
45 # [0,0,0,0]];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
46 # iPoints = utils.worldToImage(AT, wPoints);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
47
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
48 # for i in range(nPoints):
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
49 # imagePoints[i,:] = [iPoints[:,i].tolist()];
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
50
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
51 # H = cvCreateMat(3, 3, CV_64FC1);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
52
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
53 # cvFindHomography(imagePoints, worldPoints, H);
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
54
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
55 if '--help' in options.keys() or '-h' in options.keys() or len(options) == 0:
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
56 print('Usage: {0} --help|-h [-p point-correspondences.txt] [ -i video-frame] [ -w world-frame] [n number-points] [-u unit-per-pixel=1]'.format(sys.argv[0]))
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
57 print('''The input data can be provided either as point correspondences already saved
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
58 in a text file or inputed by clicking a certain number of points (>=4)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
59 in a video frame and a world image.
217
ba71924cadf5 added comment/TODO
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
60
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
61 The point correspondence file contains at least 4 non-colinear point coordinates
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
62 with the following format:
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
63 - the first two lines are the x and y coordinates in the projected space (usually world space)
236
eb4525853030 added script to display trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 235
diff changeset
64 - the last two lines are the x and y coordinates in the origin space (usually image space)
eb4525853030 added script to display trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 235
diff changeset
65
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
66 If providing video and world images, with a number of points to input
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
67 and a ration to convert pixels to world distance unit (eg meters per pixel),
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
68 the images will be shown in turn and the user should click
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
69 in the same order the corresponding points in world and image spaces. ''')
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
70 sys.exit()
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
71
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
72 homography = np.array([])
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
73 if '-p' in options.keys():
302
9d88a4d97473 corrected bug in compute-homography
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 238
diff changeset
74 worldPts, videoPts = cvutils.loadPointCorrespondences(options['-p'])
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
75 homography, mask = cv2.findHomography(videoPts, worldPts) # method=0, ransacReprojThreshold=3
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
76 elif '-i' in options.keys() and '-w' in options.keys():
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
77 nPoints = 4
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
78 if '-n' in options.keys():
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
79 nPoints = int(options['-n'])
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
80 unitsPerPixel = 1
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
81 if '-u' in options.keys():
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
82 unitsPerPixel = float(options['-u'])
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
83 worldImg = plt.imread(options['-w'])
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
84 videoImg = plt.imread(options['-i'])
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
85 print('Click on {0} points in the video frame'.format(nPoints))
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
86 plt.figure()
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
87 plt.imshow(videoImg)
443
51810d737d86 timeout in compute-homography.py for Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 346
diff changeset
88 videoPts = np.array(plt.ginput(nPoints, timeout=3000))
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
89 print('Click on {0} points in the world image'.format(nPoints))
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
90 plt.figure()
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
91 plt.imshow(worldImg)
443
51810d737d86 timeout in compute-homography.py for Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 346
diff changeset
92 worldPts = unitsPerPixel*np.array(plt.ginput(nPoints, timeout=3000))
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
93 plt.close('all')
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
94 homography, mask = cv2.findHomography(videoPts, worldPts)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
95 # save the points in file
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
96 f = open('point-correspondences.txt', 'a')
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
97 np.savetxt(f, worldPts.T)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
98 np.savetxt(f, videoPts.T)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
99 f.close()
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
100
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
101 if homography.size>0:
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
102 np.savetxt('homography.txt',homography)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
103
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
104 if '-i' in options.keys() and homography.size>0:
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
105 videoImg = cv2.imread(options['-i'])
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
106 worldImg = cv2.imread(options['-w'])
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
107 invHomography = np.linalg.inv(homography)
238
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
108 projectedWorldPts = cvutils.projectArray(invHomography, worldPts.T).T
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
109 if '-u' in options.keys():
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
110 unitsPerPixel = float(options['-u'])
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
111 projectedVideoPts = cvutils.projectArray(invHomography, videoPts.T).T
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
112 for i in range(worldPts.shape[0]):
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
113 cv2.circle(videoImg,tuple(np.int32(np.round(videoPts[i]))),2,cvutils.cvRed)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
114 cv2.circle(videoImg,tuple(np.int32(np.round(projectedWorldPts[i]))),2,cvutils.cvBlue)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
115 if '-u' in options.keys():
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
116 cv2.circle(worldImg,tuple(np.int32(np.round(worldPts[i]/unitsPerPixel))),2,cvutils.cvRed)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
117 cv2.circle(worldImg,tuple(np.int32(np.round(projectedVideoPts[i]/unitsPerPixel))),2,cvutils.cvRed)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
118 #print('img: {0} / projected: {1}'.format(videoPts[i], p))
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
119 cv2.imshow('video frame',videoImg)
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
120 if '-u' in options.keys():
be3761a09b20 added functions to input point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 237
diff changeset
121 cv2.imshow('world image',worldImg)
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
122 cv2.waitKey()
346
5f75d6c23ed5 added opencv function to destroy OpenCV windows (seems to work only on Windows)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 334
diff changeset
123 cv2.destroyAllWindows()