Project4a/stereoDepth_orb.py at master · tkurtiak/Project4a · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
#!/usr/bin/env python2

import cv2
import numpy as np


def stereoDepthORB(leftimg,rightimg,f,B):


	orb = cv2.ORB_create()
	kp1, des1 = orb.detectAndCompute(leftimg,None)
	kp2, des2 = orb.detectAndCompute(rightimg,None)
	bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
	matches = bf.match(des1,des2)

	# print np.shape(matches)


	# Extract points from matches
	matches = sorted(matches, key = lambda x:x.distance)
	print len(kp1)
	num_matches= int(.5*len(matches))

	points = np.zeros((num_matches,2))
	delta = np.zeros((num_matches,2))
	dist = np.zeros((num_matches))
	matchMask = np.zeros((num_matches,2))


	# ratio test as per Lowe's paper
	# source: https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_feature2d/py_matcher/py_matcher.html
	for i in range(0,num_matches):
		print matches[i].trainIdx
		points[i] = kp2[matches[i].trainIdx].pt
		delta[i] = np.subtract(kp1[matches[i].queryIdx].pt,kp2[matches[i].trainIdx].pt)
		dist[i] = np.sqrt(delta[i,0]**2+delta[i,1]**2)#matches[i][0].distance

		if np.abs(delta[i,0]/delta[i,1]) > 10:
			#x is bigger than y, so is more or less horizontal
			matchMask[i]=[1,0]


	# print delta
	matchMaskbool = matchMask.astype('bool')
	points = points[matchMaskbool[:,0]]
	delta = delta[matchMaskbool[:,0]]
	dist = dist[matchMaskbool[:,0]]

	## Filter out bad feature matches
	# print delta


	# print dist


	d=dist
	Z = np.divide(f*B,d)
	# print Z
	return Z, d, points.astype('int')