-
Notifications
You must be signed in to change notification settings - Fork 0
/
reconstruct.py
150 lines (117 loc) · 4.59 KB
/
reconstruct.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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
import cv2
import numpy as np
import glob
from tqdm import tqdm
import PIL.ExifTags
import PIL.Image
from matplotlib import pyplot as plt
#=====================================
# Function declarations
#=====================================
#Function to create point cloud file
def outputpointcloud(vertices, colors, filename):
colors = colors.reshape(-1,3)
vertices = np.hstack([vertices.reshape(-1,3),colors])
ply_header = '''ply
format ascii 1.0
element vertex %(vert_num)d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
'''
with open(filename, 'w') as f:
f.write(ply_header %dict(vert_num=len(vertices)))
np.savetxt(f,vertices,'%f %f %f %d %d %d')
#Function that Downsamples image x number (reduce_factor) of times.
def downsample(image, reduce_factor):
for i in range(0,reduce_factor):
#Check if image is color or grayscale
if len(image.shape) > 2:
row,col = image.shape[:2]
else:
row,col = image.shape
image = cv2.pyrDown(image, dstsize= (col//2, row // 2))
return image
#=========================================================
# Stereo 3D reconstruction
#=========================================================
#Load camera parameters
ret = np.load('C:/Me/UIC/Computer Vision 2/cvproject/ret.npy')
K = np.load('C:/Me/UIC/Computer Vision 2/cvproject/K.npy')
dist = np.load('C:/Me/UIC/Computer Vision 2/cvproject/dist.npy')
#Specify image paths
path1 = 'C:/Me/UIC/Computer Vision 2/cvproject/hallway/1.jpg'
path2 = 'C:/Me/UIC/Computer Vision 2/cvproject/hallway/2.jpg'
#Load pictures
img1 = cv2.imread(path1)
img2 = cv2.imread(path2)
#Get height and width. Note: It assumes that both pictures are the same size. They HAVE to be same size and height.
h,w = img_2.shape[:2]
#Get optimal camera matrix for better undistortion
new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(K,dist,(w,h),1,(w,h))
#Undistort images
img1_undistorted = cv2.undistort(img1, K, dist, None, new_camera_matrix)
img_2_undistorted = cv2.undistort(img_2, K, dist, None, new_camera_matrix)
#Downsample each image 3 times (because they're too big)
img1_downsampled = downsample(img1_undistorted,3)
img_2_downsampled = downsample(img_2_undistorted,3)
#cv2.imwrite('undistorted_left.jpg', img1_downsampled)
#cv2.imwrite('undistorted_right.jpg', img_2_downsampled)
#Set disparity parameters
#Note: disparity range is tuned according to specific parameters obtained through trial and error.
win_size = 5
min_disp = -1
max_disp = 63 #min_disp * 9
num_disp = max_disp - min_disp # Needs to be divisible by 16
#Create Block matching object.
stereo = cv2.StereoSGBM_create(minDisparity= min_disp,
numDisparities = num_disp,
blockSize = 5,
uniquenessRatio = 5,
speckleWindowSize = 5,
speckleRange = 5,
disp12MaxDiff = 2,
P1 = 8*3*win_size**2,#8*3*win_size**2,
P2 =32*3*win_size**2) #32*3*win_size**2)
#Compute disparity map
print ("\nComputing the disparity map...")
disparity_map = stereo.compute(img1_downsampled, img_2_downsampled)
#Show disparity map before generating 3D cloud to verify that point cloud will be usable.
plt.imshow(disparity_map,'gray')
plt.show()
#Generate point cloud.
print ("\nGenerating the 3D map...")
#Get new downsampled width and height
h,w = img_2_downsampled.shape[:2]
#Load focal length.
focal_length = np.load('C:/Me/UIC/Computer Vision 2/cvproject/focal.npy')
#Perspective transformation matrix
#This transformation matrix is from the openCV documentation, didn't seem to work for me.
Q = np.float32([[1,0,0,-w/2.0],
[0,-1,0,h/2.0],
[0,0,0,-focal_length],
[0,0,1,0]])
#This transformation matrix is derived from Prof. Didier Stricker's power point presentation on computer vision.
#Link : https://ags.cs.uni-kl.de/fileadmin/inf_ags/3dcv-ws14-15/3DCV_lec01_camera.pdf
Q2 = np.float32([[1,0,0,0],
[0,-1,0,0],
[0,0,focal_length*0.05,0], #Focal length multiplication obtained experimentally.
[0,0,0,1]])
#Reproject points into 3D
points_3D = cv2.reprojectImageTo3D(disparity_map, Q2)
#Get color points
colors = cv2.cvtColor(img1_downsampled, cv2.COLOR_BGR2RGB)
#Get rid of points with value 0 (i.e no depth)
mask_map = disparity_map > disparity_map.min()
#Mask colors and points.
output_points = points_3D[mask_map]
output_colors = colors[mask_map]
#Define name for output file
output_file = 'reconstructed.ply'
#Generate point cloud
print ("\n Creating the output file... \n")
outputpointcloud(output_points, output_colors, output_file)