forked from eclub-iitk/desktopography
-
Notifications
You must be signed in to change notification settings - Fork 1
/
depthcamera.py
135 lines (112 loc) · 4.78 KB
/
depthcamera.py
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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
import sys
import numpy as np
#from sklearn import preprocessing
import cv2
REMAP_INTERPOLATION = cv2.INTER_LINEAR
DEPTH_VISUALIZATION_SCALE = 2048
def coords_mouse_disp(event,x,y,flags,param):
if event == cv2.EVENT_LBUTTONDBLCLK:
#print x,y,disp[y,x],filteredImg[y,x]
average=0
for u in range (-1,2):
for v in range (-1,2):
average += disp[y+u,x+v]
average=average/9
Distance= -593.97*average**(3) + 1506.8*average**(2) - 1373.1*average + 522.06
Distance= np.around(Distance*0.01,decimals=2)
print('Distance: '+ str(Distance)+' m')
if len(sys.argv) != 2:
print("Syntax: {0} CALIBRATION_FILE".format(sys.argv[0]))
sys.exit(1)
calibration = np.load(sys.argv[1], allow_pickle=False)
#print('st1')
imageSize = tuple(calibration["imageSize"])
leftMapX = calibration["leftMapX"]
leftMapY = calibration["leftMapY"]
leftROI = tuple(calibration["leftROI"])
rightMapX = calibration["rightMapX"]
rightMapY = calibration["rightMapY"]
rightROI = tuple(calibration["rightROI"])
# TODO: Use more stable identifiers
left = cv2.VideoCapture(1)
right = cv2.VideoCapture(2)
# Increase the resolution
# Use MJPEG to avoid overloading the USB 2.0 bus at this resolution
#left.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
#right.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
# Create another stereo for right this time
# WLS FILTER Parameters
lmbda = 80000
sigma = 1.8
visual_multiplier = 1.0
# The distortion in the left and right edges prevents a good calibration, so
# discard the edges
min_disp = 2
num_disp = 130-min_disp
kernel= np.ones((3,3),np.uint8)
#print('st2')
# TODO: Why these values in particular?
# TODO: Try applying brightness/contrast/gamma adjustments to the images
stereoMatcher = cv2.StereoBM_create()
stereoMatcher.setMinDisparity(2)
stereoMatcher.setNumDisparities(128)
stereoMatcher.setBlockSize(21)
stereoMatcher.setROI1(leftROI)
stereoMatcher.setROI2(rightROI)
stereoMatcher.setSpeckleRange(32)
stereoMatcher.setSpeckleWindowSize(100)
#print('st3')
stereoR=cv2.ximgproc.createRightMatcher(stereoMatcher)
wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=stereoMatcher)
wls_filter.setLambda(lmbda)
wls_filter.setSigmaColor(sigma)
# Grab both frames first, then retrieve to minimize latency between cameras
while(True):
if not left.grab() or not right.grab():
print("No more frames")
break
_, leftFrame = left.read()
#leftFrame = cropHorizontal(leftFrame)
_, rightFrame = right.read()
#rightFrame = cropHorizontal(rightFrame)
#rightHeight, rightWidth = rightFrame.shape[:2]
fixedLeft = cv2.remap(leftFrame, leftMapX, leftMapY, REMAP_INTERPOLATION)
fixedRight = cv2.remap(rightFrame, rightMapX, rightMapY, REMAP_INTERPOLATION)
grayLeft = cv2.cvtColor(fixedLeft, cv2.COLOR_BGR2GRAY)
grayRight = cv2.cvtColor(fixedRight, cv2.COLOR_BGR2GRAY)
disp = stereoMatcher.compute(grayLeft, grayRight)
#.astype(np.float32)/ 16
dispL= disp
dispR= stereoR.compute(grayRight,grayLeft)
dispL= np.int16(dispL)
dispR= np.int16(dispR)
# Using the WLS filter
filteredImg= wls_filter.filter(dispL,grayLeft,None,dispR)
filteredImg = cv2.normalize(src=filteredImg, dst=filteredImg, beta=0, alpha=255, norm_type=cv2.NORM_MINMAX);
filteredImg = np.uint8(filteredImg)
#cv2.imshow('Disparity Map', filteredImg)
disp= ((disp.astype(np.float32)/ 16)-min_disp)/num_disp # Calculation allowing us to have 0 for the most distant object able to detect
## # Resize the image for faster executions
## dispR= cv2.resize(disp,None,fx=0.7, fy=0.7, interpolation = cv2.INTER_AREA)
# Filtering the Results with a closing filter
closing= cv2.morphologyEx(disp,cv2.MORPH_CLOSE, kernel) # Apply an morphological filter for closing little "black" holes in the picture(Remove noise)
# Colors map
dispc= (closing-closing.min())*255
dispC= dispc.astype(np.uint8) # Convert the type of the matrix from float32 to uint8, this way you can show the results with the function cv2.imshow()
disp_Color= cv2.applyColorMap(dispC,cv2.COLORMAP_OCEAN) # Change the Color of the Picture into an Ocean Color_Map
filt_Color= cv2.applyColorMap(filteredImg,cv2.COLORMAP_OCEAN)
# Show the result for the Depth_image
#cv2.imshow('Disparity', disp)
#cv2.imshow('Closing',closing)
#cv2.imshow('Color Depth',disp_Color)
cv2.imshow('Filtered Color Depth',filt_Color)
# Mouse click
cv2.setMouseCallback("Filtered Color Depth",coords_mouse_disp,filt_Color)
#cv2.imshow('left',leftFrame)
#cv2.imshow('right',rightFrame)
#cv2.imshow('depth', disp / DEPTH_VISUALIZATION_SCALE)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
left.release()
right.release()
cv2.destroyAllWindows()