بینایی ماشین — از صفر تا صد

1def cut(disparity, image, threshold):
2 for i in range(0, image.height):
3  for j in range(0, image.width):
4   # keep closer object
5   if cv.GetReal2D(disparity,i,j) > threshold:
6    cv.Set2D(disparity,i,j,cv.Get2D(image,i,j))
7
8# loading the stereo pair
9left  = cv.LoadImage('scene_l.bmp',cv.CV_LOAD_IMAGE_GRAYSCALE)
10right = cv.LoadImage('scene_r.bmp',cv.CV_LOAD_IMAGE_GRAYSCALE)
11
12disparity_left  = cv.CreateMat(left.height, left.width, cv.CV_16S)
13disparity_right = cv.CreateMat(left.height, left.width, cv.CV_16S)
14
15# data structure initialization
16state = cv.CreateStereoGCState(16,2)
17# running the graph-cut algorithm
18cv.FindStereoCorrespondenceGC(left,right,
19                          disparity_left,disparity_right,state)
20
21disp_left_visual = cv.CreateMat(left.height, left.width, cv.CV_8U)
22cv.ConvertScale( disparity_left, disp_left_visual, -16 );
23cv.Save( "disparity.pgm", disp_left_visual ); # save the map
24
25# cutting the object farthest of a threshold (120)
26cut(disp_left_visual,left,120)
27
28cv.NamedWindow('Disparity map', cv.CV_WINDOW_AUTOSIZE)
29cv.ShowImage('Disparity map', disp_left_visual)
30cv.WaitKey()

1import cv2
2import random
3import numpy as np
4import matplotlib
5matplotlib.use("TkAgg")
6from matplotlib import pyplot as plt
7
8
9grayscale_max = 255
10
11
12def load_image(filename):
13    image = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)
14    return image
15
16
17def show_image(title, image):
18    max_val = image.max()
19    # image = np.absolute(image)
20    image = np.divide(image, max_val)
21    # cv2.imshow(title, image)
22    cv2.imwrite(title+str(random.randint(1, 100))+'.jpg', image*grayscale_max)
23
24
25def add_padding(input, padding):
26    rows = input.shape[0]
27    columns = input.shape[1]
28    output = np.zeros((rows + padding * 2, columns + padding * 2), dtype=float)
29    output[ padding : rows + padding, padding : columns + padding] = input
30    return output
31
32
33def add_replicate_padding(image):
34    # zero_padded = add_padding(image, padding)
35    # size = image.shape[0]
36    top_row = image[0, :]
37    image = np.vstack((top_row, image))
38
39    bottom_row = image[-1, :]
40    image = np.vstack((image, bottom_row))
41
42    left_column = image[:, 0]
43    left_column = np.reshape(left_column, (left_column.shape[0], 1))
44    image = np.hstack((left_column, image))
45
46    right_column = image[:, -1]
47    right_column = np.reshape(right_column, (right_column.shape[0], 1))
48    image = np.hstack((image, right_column))
49
50    return image
51
52
53def euclid_dist(a, b):
54    distance = np.linalg.norm(a - b)
55    return distance
56
57
58def get_search_bounds(column, block_size, width):
59    disparity_range = 25
60    left_bound = column - disparity_range
61    if left_bound < 0:
62        left_bound = 0
63    right_bound = column + disparity_range
64    if right_bound > width:
65        right_bound = width - block_size + 1
66    return left_bound, right_bound
67
68
69def search_bounds(column, block_size, width, rshift):
70    disparity_range = 75
71    padding = block_size // 2
72    right_bound = column
73    if rshift:
74        left_bound = column - disparity_range
75        if left_bound < padding:
76            left_bound = padding
77        step = 1
78    else:
79        left_bound = column + disparity_range
80        if left_bound >= (width - 2*padding):
81            left_bound = width - 2*padding - 2
82        step = -1
83    return left_bound, right_bound, step
84
85
86# max disparity 30
87def disparity_map(left, right, block_size, rshift):
88
89    padding = block_size // 2
90    left_img = add_padding(left, padding)
91    right_img = add_padding(right, padding)
92
93    height, width = left_img.shape
94
95    # d_map = np.zeros((height - padding*2, width - padding*2), dtype=float)
96    d_map = np.zeros(left.shape , dtype=float)
97
98    for row in range(height - block_size + 1):
99        for col in range(width - block_size + 1):
100
101            bestdist = float('inf')
102            shift = 0
103            left_pixel = left_img[row:row + block_size, col:col + block_size]
104            l_bound, r_bound, step = search_bounds(col, block_size, width, rshift)
105
106            # for i in range(l_bound, r_bound - padding*2):
107            for i in range(l_bound, r_bound, step):
108                right_pixel = right_img[row:row + block_size, i:i + block_size]
109
110                # if euclid_dist(left_pixel, right_pixel) < bestdist :
111                ssd = np.sum((left_pixel - right_pixel) ** 2)
112                # print('row:',row,' col:',col,' i:',i,' bestdist:',bestdist,' shift:',shift,' ssd:',ssd)
113                if ssd < bestdist:
114                    bestdist = ssd
115                    shift = i
116
117            if rshift:
118                d_map[row, col] = col - shift
119            else:
120                d_map[row, col] = shift - col
121            print('Calculated Disparity at ('+str(row)+','+str(col)+') :', d_map[row,col])
122
123    return d_map
124
125
126def mean_square_error(disparity_map, ground_truth):
127    # ssd = np.sum((disparity_map - ground_truth)**2)
128    # mse = ssd/(ground_truth.shape[0]*ground_truth.shape[1])
129    mse = np.mean((disparity_map - ground_truth)**2)
130    return mse
131
132
133def consistency_map_mse_l(d_map_left, d_map_right, left_ground_truth):
134    rows, cols = d_map_left.shape
135    consistency_map = np.zeros((rows, cols))
136
137    for r in range(rows):
138        for c in range(cols):
139            left_pixel = d_map_left[r, c]
140
141            if cols > c - left_pixel > 0:
142                right_pixel = d_map_right[r, int(c - left_pixel)]
143            else:
144                right_pixel = d_map_right[r, c]
145
146            if left_pixel == right_pixel:
147                consistency_map[r, c] = left_pixel
148            else:
149                consistency_map[r, c] = 0
150
151    sum = 0
152    for r in range(rows):
153        for c in range(cols):
154            if consistency_map[r, c] != 0:
155                sum = sum + (left_ground_truth[r, c] - consistency_map[r, c]) ** 2
156
157    mse_c_left = sum / (rows * cols)
158    return mse_c_left, consistency_map
159
160
161def consistency_map_mse_r(d_map_left, d_map_right, right_ground_truth):
162    rows, cols = d_map_right.shape
163    consistency_map = np.zeros((rows, cols))
164
165    for r in range(rows):
166        for c in range(cols):
167            right_pixel = d_map_right[r, c]
168
169            if c + right_pixel < cols:
170                left_pixel = d_map_left[r, int(c + right_pixel)]
171            else:
172                left_pixel = d_map_left[r, c]
173
174            if right_pixel == left_pixel:
175                consistency_map[r, c] = right_pixel
176            else:
177                consistency_map[r, c] = 0
178
179    sum = 0
180    for r in range(rows):
181        for c in range(cols):
182            if consistency_map[r, c] != 0:
183                sum = sum + (right_ground_truth[r, c] - consistency_map[r, c]) ** 2
184
185    mse_c_right = sum / (rows * cols)
186    return mse_c_right, consistency_map
187
188
189def main():
190    l = load_image('view1.png')
191    r = load_image('view5.png')
192
193
194    # Disparity Maps
195    d_map_lr_3 = disparity_map(l, r, 3, True)
196    show_image('D_Map_lr_block3_', d_map_lr_3)
197
198    d_map_rl_3 = disparity_map(r, l, 3, False)
199    show_image('D_Map_rl_block3_', d_map_rl_3)
200
201    d_map_lr_9 = disparity_map(l, r, 9, True)
202    show_image('D_Map_lr_block9_', d_map_lr_9)
203
204    d_map_rl_9 = disparity_map(r, l, 9, False)
205    show_image('D_Map_rl_block9_', d_map_rl_9)
206
207
208    # Mean Squared Error
209    ground_truth_1 = load_image('disp1.png')
210    ground_truth_2 = load_image('disp5.png')
211
212    mse_3_lr = mean_square_error(d_map_lr_3, ground_truth_1)
213    print('MSE for view1 using block size of 3 is', mse_3_lr)
214
215    mse_3_rl = mean_square_error(d_map_rl_3, ground_truth_2)
216    print('MSE for view5 using block size of 3 is', mse_3_rl)
217
218    mse_9_lr = mean_square_error(d_map_lr_9, ground_truth_1)
219    print('MSE for view1 using block size of 9 is', mse_9_lr)
220
221    mse_9_rl = mean_square_error(d_map_lr_3, ground_truth_2)
222    print('MSE for view5 using block size of 9 is', mse_9_rl)
223
224
225    # MSE after Consistency Check
226    mse_3c_left, c_map_3cl = consistency_map_mse_l(d_map_lr_3, d_map_rl_3, ground_truth_1)
227    cv2.imwrite('consistency_map_block3_view1.jpg', c_map_3cl)
228    print('MSE for view1 after Consistency check using block size of 3 is', mse_3c_left)
229
230    mse_3c_right, c_map_3cr = consistency_map_mse_r(d_map_lr_3, d_map_rl_3, ground_truth_2)
231    cv2.imwrite('consistency_map_block3_view5.jpg', c_map_3cr)
232    print('MSE for view5 after Consistency check using block size of 3 is', mse_3c_right)
233
234    mse_9c_left, c_map_9cl = consistency_map_mse_l(d_map_lr_9, d_map_rl_9, ground_truth_1)
235    cv2.imwrite('consistency_map_block9_view1.jpg', c_map_9cl)
236    print('MSE for view1 after Consistency check using block size of 9 is', mse_9c_left)
237
238    mse_9c_right, c_map_9cr = consistency_map_mse_r(d_map_lr_9, d_map_rl_9, ground_truth_2)
239    cv2.imwrite('consistency_map_block9_view5.jpg', c_map_9cr)
240    print('MSE for view5 after Consistency check using block size of 9 is', mse_9c_right)
241
242    return
243
244
245main()

1import numpy as np
2from skimage import measure
3try:
4    import pycuda.driver as cuda
5    import pycuda.autoinit
6    from pycuda.compiler import SourceModule
7    FUSION_GPU_MODE = 1
8except Exception as err:
9    print('Warning: %s'%(str(err)))
10    print('Failed to import PyCUDA. Running fusion in CPU mode.')
11    FUSION_GPU_MODE = 0
12
13
14class TSDFVolume(object):
15
16    def __init__(self,vol_bnds,voxel_size):
17
18        # Define voxel volume parameters
19        self._vol_bnds = vol_bnds # rows: x,y,z columns: min,max in world coordinates in meters
20        self._voxel_size = voxel_size # in meters (determines volume discretization and resolution)
21        self._trunc_margin = self._voxel_size*5 # truncation on SDF
22
23        # Adjust volume bounds
24        self._vol_dim = np.ceil((self._vol_bnds[:,1]-self._vol_bnds[:,0])/self._voxel_size).copy(order='C').astype(int) # ensure C-order contigous
25        self._vol_bnds[:,1] = self._vol_bnds[:,0]+self._vol_dim*self._voxel_size
26        self._vol_origin = self._vol_bnds[:,0].copy(order='C').astype(np.float32) # ensure C-order contigous
27        print("Voxel volume size: %d x %d x %d"%(self._vol_dim[0],self._vol_dim[1],self._vol_dim[2]))
28
29        # Initialize pointers to voxel volume in CPU memory
30        self._tsdf_vol_cpu = np.ones(self._vol_dim).astype(np.float32)
31        self._weight_vol_cpu = np.zeros(self._vol_dim).astype(np.float32) # for computing the cumulative moving average of observations per voxel
32        self._color_vol_cpu = np.zeros(self._vol_dim).astype(np.float32)
33
34        # Copy voxel volumes to GPU
35        if FUSION_GPU_MODE:
36            self._tsdf_vol_gpu = cuda.mem_alloc(self._tsdf_vol_cpu.nbytes)
37            cuda.memcpy_htod(self._tsdf_vol_gpu,self._tsdf_vol_cpu)
38            self._weight_vol_gpu = cuda.mem_alloc(self._weight_vol_cpu.nbytes)
39            cuda.memcpy_htod(self._weight_vol_gpu,self._weight_vol_cpu)
40            self._color_vol_gpu = cuda.mem_alloc(self._color_vol_cpu.nbytes)
41            cuda.memcpy_htod(self._color_vol_gpu,self._color_vol_cpu)
42
43            # Cuda kernel function (C++)
44            self._cuda_src_mod = SourceModule("""
45              __global__ void integrate(float * tsdf_vol,
46                                        float * weight_vol,
47                                        float * color_vol,
48                                        float * vol_dim,
49                                        float * vol_origin,
50                                        float * cam_intr,
51                                        float * cam_pose,
52                                        float * other_params,
53                                        float * color_im,
54                                        float * depth_im) {
55                // Get voxel index
56                int gpu_loop_idx = (int) other_params[0];
57                int max_threads_per_block = blockDim.x;
58                int block_idx = blockIdx.z*gridDim.y*gridDim.x+blockIdx.y*gridDim.x+blockIdx.x;
59                int voxel_idx = gpu_loop_idx*gridDim.x*gridDim.y*gridDim.z*max_threads_per_block+block_idx*max_threads_per_block+threadIdx.x;
60                
61                int vol_dim_x = (int) vol_dim[0];
62                int vol_dim_y = (int) vol_dim[1];
63                int vol_dim_z = (int) vol_dim[2];
64                if (voxel_idx > vol_dim_x*vol_dim_y*vol_dim_z)
65                    return;
66                // Get voxel grid coordinates (note: be careful when casting)
67                float voxel_x = floorf(((float)voxel_idx)/((float)(vol_dim_y*vol_dim_z)));
68                float voxel_y = floorf(((float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z))/((float)vol_dim_z));
69                float voxel_z = (float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z-((int)voxel_y)*vol_dim_z);
70                // Voxel grid coordinates to world coordinates
71                float voxel_size = other_params[1];
72                float pt_x = vol_origin[0]+voxel_x*voxel_size;
73                float pt_y = vol_origin[1]+voxel_y*voxel_size;
74                float pt_z = vol_origin[2]+voxel_z*voxel_size;
75                // World coordinates to camera coordinates
76                float tmp_pt_x = pt_x-cam_pose[0*4+3];
77                float tmp_pt_y = pt_y-cam_pose[1*4+3];
78                float tmp_pt_z = pt_z-cam_pose[2*4+3];
79                float cam_pt_x = cam_pose[0*4+0]*tmp_pt_x+cam_pose[1*4+0]*tmp_pt_y+cam_pose[2*4+0]*tmp_pt_z;
80                float cam_pt_y = cam_pose[0*4+1]*tmp_pt_x+cam_pose[1*4+1]*tmp_pt_y+cam_pose[2*4+1]*tmp_pt_z;
81                float cam_pt_z = cam_pose[0*4+2]*tmp_pt_x+cam_pose[1*4+2]*tmp_pt_y+cam_pose[2*4+2]*tmp_pt_z;
82                // Camera coordinates to image pixels
83                int pixel_x = (int) roundf(cam_intr[0*3+0]*(cam_pt_x/cam_pt_z)+cam_intr[0*3+2]);
84                int pixel_y = (int) roundf(cam_intr[1*3+1]*(cam_pt_y/cam_pt_z)+cam_intr[1*3+2]);
85                // Skip if outside view frustum
86                int im_h = (int) other_params[2];
87                int im_w = (int) other_params[3];
88                if (pixel_x < 0 || pixel_x >= im_w || pixel_y < 0 || pixel_y >= im_h || cam_pt_z<0)
89                    return;
90                // Skip invalid depth
91                float depth_value = depth_im[pixel_y*im_w+pixel_x];
92                if (depth_value == 0)
93                    return;
94                // Integrate TSDF
95                float trunc_margin = other_params[4];
96                float depth_diff = depth_value-cam_pt_z;
97                if (depth_diff < -trunc_margin)
98                    return;
99                float dist = fmin(1.0f,depth_diff/trunc_margin);
100                float w_old = weight_vol[voxel_idx];
101                float obs_weight = other_params[5];
102                float w_new = w_old + obs_weight;
103                weight_vol[voxel_idx] = w_new;
104                tsdf_vol[voxel_idx] = (tsdf_vol[voxel_idx]*w_old+dist)/w_new;
105                // Integrate color
106                float old_color = color_vol[voxel_idx];
107                float old_b = floorf(old_color/(256*256));
108                float old_g = floorf((old_color-old_b*256*256)/256);
109                float old_r = old_color-old_b*256*256-old_g*256;
110                float new_color = color_im[pixel_y*im_w+pixel_x];
111                float new_b = floorf(new_color/(256*256));
112                float new_g = floorf((new_color-new_b*256*256)/256);
113                float new_r = new_color-new_b*256*256-new_g*256;
114                new_b = fmin(roundf((old_b*w_old+new_b)/w_new),255.0f);
115                new_g = fmin(roundf((old_g*w_old+new_g)/w_new),255.0f);
116                new_r = fmin(roundf((old_r*w_old+new_r)/w_new),255.0f);
117                color_vol[voxel_idx] = new_b*256*256+new_g*256+new_r;
118              }""")
119
120            self._cuda_integrate = self._cuda_src_mod.get_function("integrate")
121
122            # Determine block/grid size on GPU
123            gpu_dev = cuda.Device(0)
124            self._max_gpu_threads_per_block = gpu_dev.MAX_THREADS_PER_BLOCK
125            n_blocks = int(np.ceil(float(np.prod(self._vol_dim))/float(self._max_gpu_threads_per_block)))
126            grid_dim_x = min(gpu_dev.MAX_GRID_DIM_X,int(np.floor(np.cbrt(n_blocks))))
127            grid_dim_y = min(gpu_dev.MAX_GRID_DIM_Y,int(np.floor(np.sqrt(n_blocks/grid_dim_x))))
128            grid_dim_z = min(gpu_dev.MAX_GRID_DIM_Z,int(np.ceil(float(n_blocks)/float(grid_dim_x*grid_dim_y))))
129            self._max_gpu_grid_dim = np.array([grid_dim_x,grid_dim_y,grid_dim_z]).astype(int)
130            self._n_gpu_loops = int(np.ceil(float(np.prod(self._vol_dim))/float(np.prod(self._max_gpu_grid_dim)*self._max_gpu_threads_per_block)))
131
132
133   
134    
135    #         if new_bnds[dim,1] > self._vol_bnds[dim,1]: # expand upper bounds
136    #             n_voxels_expand = int(np.ceil((new_bnds[dim,1]-self._vol_bnds[dim,1])/self._voxel_size))
137    #             new_chunk_size = np.round((self._vol_bnds[:,1]-self._vol_bnds[:,0])/self._voxel_size).astype(int)
138    #             new_chunk_size[dim] = n_voxels_expand # size of expanding region (i.e. chunk)
139                
140    #             # Initialize chunks and concatenate to current voxel volume
141    #             self._tsdf_vol_cpu = np.concatenate((self._tsdf_vol_cpu,np.ones(new_chunk_size)),axis=dim)
142    #             self._weight_vol_cpu = np.concatenate((self._weight_vol_cpu,np.zeros(new_chunk_size)),axis=dim)
143    #             self._color_vol_cpu = np.concatenate((self._color_vol_cpu,np.zeros(new_chunk_size)),axis=dim)
144    #             self._vol_bnds[dim,1] += n_voxels_expand*self._voxel_size # update voxel volume bounds
145
146
147    def integrate(self,color_im,depth_im,cam_intr,cam_pose,obs_weight=1.):
148        im_h = depth_im.shape[0]
149        im_w = depth_im.shape[1]
150
151        # Fold RGB color image into a single channel image
152        color_im = color_im.astype(np.float32)
153        color_im = np.floor(color_im[:,:,2]*256*256+color_im[:,:,1]*256+color_im[:,:,0])
154
155        # GPU mode: integrate voxel volume (calls CUDA kernel)
156        if FUSION_GPU_MODE:
157            for gpu_loop_idx in range(self._n_gpu_loops):
158                self._cuda_integrate(self._tsdf_vol_gpu,
159                                     self._weight_vol_gpu,
160                                     self._color_vol_gpu,
161                                     cuda.InOut(self._vol_dim.astype(np.float32)),
162                                     cuda.InOut(self._vol_origin.astype(np.float32)),
163                                     cuda.InOut(cam_intr.reshape(-1).astype(np.float32)),
164                                     cuda.InOut(cam_pose.reshape(-1).astype(np.float32)),
165                                     cuda.InOut(np.asarray([gpu_loop_idx,self._voxel_size,im_h,im_w,self._trunc_margin,obs_weight],np.float32)),
166                                     cuda.InOut(color_im.reshape(-1).astype(np.float32)),
167                                     cuda.InOut(depth_im.reshape(-1).astype(np.float32)),
168                                     block=(self._max_gpu_threads_per_block,1,1),grid=(int(self._max_gpu_grid_dim[0]),int(self._max_gpu_grid_dim[1]),int(self._max_gpu_grid_dim[2])))
169
170        # CPU mode: integrate voxel volume (vectorized implementation)
171        else:
172
173            # Get voxel grid coordinates
174            xv,yv,zv = np.meshgrid(range(self._vol_dim[0]),range(self._vol_dim[1]),range(self._vol_dim[2]),indexing='ij')
175            vox_coords = np.concatenate((xv.reshape(1,-1),yv.reshape(1,-1),zv.reshape(1,-1)),axis=0).astype(int)
176
177            # Voxel coordinates to world coordinates
178            world_pts = self._vol_origin.reshape(-1,1)+vox_coords.astype(float)*self._voxel_size
179
180            # World coordinates to camera coordinates
181            world2cam = np.linalg.inv(cam_pose)
182            cam_pts = np.dot(world2cam[:3,:3],world_pts)+np.tile(world2cam[:3,3].reshape(3,1),(1,world_pts.shape[1]))
183
184            # Camera coordinates to image pixels
185            pix_x = np.round(cam_intr[0,0]*(cam_pts[0,:]/cam_pts[2,:])+cam_intr[0,2]).astype(int)
186            pix_y = np.round(cam_intr[1,1]*(cam_pts[1,:]/cam_pts[2,:])+cam_intr[1,2]).astype(int)
187
188            # Skip if outside view frustum
189            valid_pix = np.logical_and(pix_x >= 0,
190                        np.logical_and(pix_x < im_w,
191                        np.logical_and(pix_y >= 0,
192                        np.logical_and(pix_y < im_h,
193                                       cam_pts[2,:] > 0))))
194
195            depth_val = np.zeros(pix_x.shape)
196            depth_val[valid_pix] = depth_im[pix_y[valid_pix],pix_x[valid_pix]]
197
198            # Integrate TSDF
199            depth_diff = depth_val-cam_pts[2,:]
200            valid_pts = np.logical_and(depth_val > 0,depth_diff >= -self._trunc_margin)
201            dist = np.minimum(1.,np.divide(depth_diff,self._trunc_margin))
202            w_old = self._weight_vol_cpu[vox_coords[0,valid_pts],vox_coords[1,valid_pts],vox_coords[2,valid_pts]]
203            w_new = w_old + obs_weight
204            self._weight_vol_cpu[vox_coords[0,valid_pts],vox_coords[1,valid_pts],vox_coords[2,valid_pts]] = w_new
205            tsdf_vals = self._tsdf_vol_cpu[vox_coords[0,valid_pts],vox_coords[1,valid_pts],vox_coords[2,valid_pts]]
206            self._tsdf_vol_cpu[vox_coords[0,valid_pts],vox_coords[1,valid_pts],vox_coords[2,valid_pts]] = np.divide(np.multiply(tsdf_vals,w_old)+dist[valid_pts],w_new)
207
208            # Integrate color
209            old_color = self._color_vol_cpu[vox_coords[0,valid_pts],vox_coords[1,valid_pts],vox_coords[2,valid_pts]]
210            old_b = np.floor(old_color/(256.*256.))
211            old_g = np.floor((old_color-old_b*256.*256.)/256.)
212            old_r = old_color-old_b*256.*256.-old_g*256.
213            new_color = color_im[pix_y[valid_pts],pix_x[valid_pts]]
214            new_b = np.floor(new_color/(256.*256.))
215            new_g = np.floor((new_color-new_b*256.*256.)/256.)
216            new_r = new_color-new_b*256.*256.-new_g*256.
217            new_b = np.minimum(np.round(np.divide(np.multiply(old_b,w_old)+new_b,w_new)),255.);
218            new_g = np.minimum(np.round(np.divide(np.multiply(old_g,w_old)+new_g,w_new)),255.);
219            new_r = np.minimum(np.round(np.divide(np.multiply(old_r,w_old)+new_r,w_new)),255.);
220            self._color_vol_cpu[vox_coords[0,valid_pts],vox_coords[1,valid_pts],vox_coords[2,valid_pts]] = new_b*256.*256.+new_g*256.+new_r;
221
222
223    # Copy voxel volume to CPU
224    def get_volume(self):
225        if FUSION_GPU_MODE:
226            cuda.memcpy_dtoh(self._tsdf_vol_cpu,self._tsdf_vol_gpu)
227            cuda.memcpy_dtoh(self._color_vol_cpu,self._color_vol_gpu)
228        return self._tsdf_vol_cpu,self._color_vol_cpu
229
230
231    # Get mesh of voxel volume via marching cubes
232    def get_mesh(self):
233        tsdf_vol,color_vol = self.get_volume()
234
235        # Marching cubes
236        verts,faces,norms,vals = measure.marching_cubes_lewiner(tsdf_vol,level=0)
237        verts_ind = np.round(verts).astype(int)
238        verts = verts*self._voxel_size+self._vol_origin # voxel grid coordinates to world coordinates
239
240        # Get vertex colors
241        rgb_vals = color_vol[verts_ind[:,0],verts_ind[:,1],verts_ind[:,2]]
242        colors_b = np.floor(rgb_vals/(256*256))
243        colors_g = np.floor((rgb_vals-colors_b*256*256)/256)
244        colors_r = rgb_vals-colors_b*256*256-colors_g*256
245        colors = np.floor(np.asarray([colors_r,colors_g,colors_b])).T
246        colors = colors.astype(np.uint8)
247        return verts,faces,norms,colors
248
249
250# -------------------------------------------------------------------------------
251# Additional helper functions
252
253
254# Get corners of 3D camera view frustum of depth image
255def get_view_frustum(depth_im,cam_intr,cam_pose):
256    im_h = depth_im.shape[0]
257    im_w = depth_im.shape[1]
258    max_depth = np.max(depth_im)
259    view_frust_pts = np.array([(np.array([0,0,0,im_w,im_w])-cam_intr[0,2])*np.array([0,max_depth,max_depth,max_depth,max_depth])/cam_intr[0,0],
260                               (np.array([0,0,im_h,0,im_h])-cam_intr[1,2])*np.array([0,max_depth,max_depth,max_depth,max_depth])/cam_intr[1,1],
261                                np.array([0,max_depth,max_depth,max_depth,max_depth])])
262    view_frust_pts = np.dot(cam_pose[:3,:3],view_frust_pts)+np.tile(cam_pose[:3,3].reshape(3,1),(1,view_frust_pts.shape[1])) # from camera to world coordinates
263    return view_frust_pts
264
265
266# Save 3D mesh to a polygon .ply file
267def meshwrite(filename,verts,faces,norms,colors):
268
269    # Write header
270    ply_file = open(filename,'w')
271    ply_file.write("ply\n")
272    ply_file.write("format ascii 1.0\n")
273    ply_file.write("element vertex %d\n"%(verts.shape[0]))
274    ply_file.write("property float x\n")
275    ply_file.write("property float y\n")
276    ply_file.write("property float z\n")
277    ply_file.write("property float nx\n")
278    ply_file.write("property float ny\n")
279    ply_file.write("property float nz\n")
280    ply_file.write("property uchar red\n")
281    ply_file.write("property uchar green\n")
282    ply_file.write("property uchar blue\n")
283    ply_file.write("element face %d\n"%(faces.shape[0]))
284    ply_file.write("property list uchar int vertex_index\n")
285    ply_file.write("end_header\n")
286
287    # Write vertex list
288    for i in range(verts.shape[0]):
289        ply_file.write("%f %f %f %f %f %f %d %d %d\n"%(verts[i,0],verts[i,1],verts[i,2],norms[i,0],norms[i,1],norms[i,2],colors[i,0],colors[i,1],colors[i,2]))
290    
291    # Write face list
292    for i in range(faces.shape[0]):
293        ply_file.write("3 %d %d %d\n"%(faces[i,0],faces[i,1],faces[i,2]))
294
295    ply_file.close()

1import numpy as np
2import cv2
3import time
4import fusion
5
6
7# (Optional) sample code to compute 3D bounds (in world coordinates) around convex hull of all camera view frustums in dataset
8print("Estimating voxel volume bounds...")
9n_imgs = 1000
10cam_intr = np.loadtxt("data/camera-intrinsics.txt",delimiter=' ')
11vol_bnds = np.zeros((3,2))
12for i in range(n_imgs):
13
14    # Read depth image and camera pose
15    depth_im = cv2.imread("data/frame-%06d.depth.png"%(i),-1).astype(float)/1000. # depth is saved in 16-bit PNG in millimeters
16    depth_im[depth_im == 65.535] = 0 # set invalid depth to 0 (specific to 7-scenes dataset)
17    cam_pose = np.loadtxt("data/frame-%06d.pose.txt"%(i)) # 4x4 rigid transformation matrix
18
19    # Compute camera view frustum and extend convex hull
20    view_frust_pts = fusion.get_view_frustum(depth_im,cam_intr,cam_pose)
21    vol_bnds[:,0] = np.minimum(vol_bnds[:,0],np.amin(view_frust_pts,axis=1))
22    vol_bnds[:,1] = np.maximum(vol_bnds[:,1],np.amax(view_frust_pts,axis=1))
23
24# ---------------------------------------------------------------------
25
26# Initialize voxel volume
27print("Initializing voxel volume...")
28tsdf_vol = fusion.TSDFVolume(vol_bnds,voxel_size=0.02)
29
30# Loop through RGB-D images and fuse them together
31t0_elapse = time.time()
32for i in range(n_imgs):
33    print("Fusing frame %d/%d"%(i+1,n_imgs))
34
35    # Read RGB-D image and camera pose
36    color_image = cv2.cvtColor(cv2.imread("data/frame-%06d.color.jpg"%(i)),cv2.COLOR_BGR2RGB)
37    depth_im = cv2.imread("data/frame-%06d.depth.png"%(i),-1).astype(float)/1000. # depth is saved in 16-bit PNG in millimeters
38    depth_im[depth_im == 65.535] = 0 # set invalid depth to 0 (specific to 7-scenes dataset)
39    cam_pose = np.loadtxt("data/frame-%06d.pose.txt"%(i)) # 4x4 rigid transformation matrix
40
41    # Integrate observation into voxel volume (assume color aligned with depth)
42    tsdf_vol.integrate(color_image,depth_im,cam_intr,cam_pose,obs_weight=1.)
43
44fps = n_imgs/(time.time()-t0_elapse)
45print("Average FPS: %.2f"%(fps))
46
47# Get mesh from voxel volume and save to disk (can be viewed with Meshlab)
48print("Saving to mesh.ply...")
49verts,faces,norms,colors = tsdf_vol.get_mesh()
50fusion.meshwrite("mesh.ply",verts,faces,norms,colors)

1import numpy as np
2import cv2
3import time
4
5SHOW_STEREOMATCHING_DISPARITY_IMG           = True
6
7STEREOMATCHING_SGBM_MIN_DISPARITY           = 0
8    # normally, it's 0 but sometimes rectification algo can shift images, so this param needs to be adjusted accordingly
9STEREOMATCHING_SGBM_N_DISPARITIES			= 512#1024
10    # in the current implementation, this param must be divisible by 16
11STEREOMATCHING_SGBM_MATCHING_WIN_SIZE		= 19
12    # it must be an odd number >= 1. Normally, it should be somewhere in the 3..11 range
13STEREOMATCHING_SGBM_DISPARITY_SMOOTH_P1		= 0		# Thuy check: max 19								# FIXED PARAM
14    # control disparity smoothness. Larger values -> smoother disparity.
15    # P1 is the penalty on disparity change by + or - 1 between neighbor pixels
16STEREOMATCHING_SGBM_DISPARITY_SMOOTH_P2		= 0		# Thuy check: max 95								# FIXED PARAM
17    # control disparity smoothness. Is the penalty on disparity change by more than 1 between neighbor pixels. P2 > P1
18STEREOMATCHING_SGBM_MAX_DISPARITY_CHECK		= 0															# FIXED PARAM
19    # max allowed diff (in pixel) in the left-right disparity check. Set it to a non-positive value to disable the check
20STEREOMATCHING_SGBM_PREFILTER_CAP			= 0															# FIXED PARAM
21    # truncation value for the prefiltered image pixels. Algo first computes x-derivative at each pixel & clips its
22    # value by [-preFilterCap, preFilterCap] interval. Result values are passed to Birchfield-Tomasi pixel cost func
23STEREOMATCHING_SGBM_UNIQUENESS_RATIO		= 15	# good range: 5 -> 15								# FIXED PARAM
24    # margin in percent by which the best (min) computed cost func value should 'win' the 2nd best value to consider
25    # the found match correct. A value within the 5-15 range is good enough
26STEREOMATCHING_SGBM_SPECKLE_WIN_SIZE		= 200	# 0 to disable. Otherwise, good range: 50 -> 200	# FIXED PARAM
27    # max size of smooth disparity regions to consider their noise speckles and invalidate. Set to 0 to disable
28    # speckle filtering. Otherwise, set in the 50-200 range
29STEREOMATCHING_SGBM_SPECKLE_RANGE   		= 2		# good range: 1 -> 2								# FIXED PARAM
30    # max disparity variation within each connected component. If do speckle filtering, set the param to a positive
31    # value, it'll be implicitly x 16. Normally, 1 or 2 is good enough
32STEREOMATCHING_SGBM_MODE					= cv2.STEREO_SGBM_MODE_SGBM_3WAY
33    # STEREO_SGBM_MODE_HH to run the full-scale 2-pass dynamic programming algo. It consumes O(W*H*numDisparities)
34    #       bytes (large for 640x480 and huge for HD-size). Mutual info cost func is not implemented. Instead, a simpler
35    #       Birchfield-Tomasi sub-pixel metric is used. Though, COLOR images are supported as well.
36    # STEREO_SGBM_MODE_SGBM: does not support COLOR images
37    # STEREO_SGBM_MODE_SGBM_3WAY: 2-3 times faster than MODE_SGBM with minimal degradation in quality and uses universal
38    #       HAL intrinsics. Does not support COLOR images
39	# Pre- & post- processing steps from K. Konolige algorithm StereoBM::operator() are included, for example:
40    #       pre-filtering (CV_STEREO_BM_XSOBEL type) and post-filtering (uniqueness check, quadratic interpolation and
41    #       speckle filtering)
42
43STEREOMATCHING_PLY_FILTER_RANGE_Z_FROM		= 0
44STEREOMATCHING_PLY_FILTER_RANGE_Z_TO		= 6
45STEREOMATCHING_PLY_FILTER_RANGE_XY			= 5000														# FIXED PARAM
46STEREOMATCHING_PLY_FILTER_BLACK_COLOR_THR	= 30
47
48
49# ************************************************************************************************
50# **********                 FUNCTIONS: DISPLAY                                         **********
51# ************************************************************************************************
52
53def ShowDisparityImageSGBM(disparityImg):
54    disImg = ResizeImage(disparityImg, 0.25)
55
56    # ref: https://rdmilligan.wordpress.com/2016/05/23/disparity-of-stereo-images-with-python-and-opencv/
57    disImg = disImg.astype(np.float32) / 16.0
58    disImg = (disImg - STEREOMATCHING_SGBM_MIN_DISPARITY) / STEREOMATCHING_SGBM_N_DISPARITIES
59    cv2.imshow('Disparity image SGBM', disImg)
60    cv2.waitKey(0)
61    return;
62
63
64# ************************************************************************************************
65# **********                 FUNCTIONS: COMMON                                          **********
66# ************************************************************************************************
67
68def ResizeImage(img, scale):
69    imgResized = cv2.resize(src=img, dsize=(0,0), fx=scale, fy=scale, interpolation=cv2.INTER_LINEAR)
70    return imgResized;
71
72
73def RotateImage(srcImg, angleToRotate):
74    height, width = srcImg.shape[:2]
75    maxLen = max(height, width)
76    rotationMat = cv2.getRotationMatrix2D(center=(maxLen/2,maxLen/2), angle=angleToRotate, scale=1.0)
77    desImg = cv2.warpAffine(src=srcImg, M=rotationMat, dsize=(maxLen, maxLen))
78    return desImg;
79
80
81def RotateLeftAndRightImagesAccordingToOurSystem(leftImg, rightImg):
82    height, width = leftImg.shape[:2]
83    leftImg90 = RotateImage(leftImg, 90)     # 1. rotate left img 90-deg counter-clockwise (left direction)
84    rightImg270 = RotateImage(rightImg, 270) # 2. rotate right img 90-deg clockwise (right direction)
85                                             # cuz img is horizontal, but EXIF changes according to camera gravity
86    leftImgRotated = leftImg90[0:width, 0:height]              # 3. crop left img
87    rightImgRotated = rightImg270[0:width, width-height:width] # 4. crop right img
88                                    # ref (extract ROI): http://docs.opencv.org/3.2.0/d3/df2/tutorial_py_basic_ops.html
89    return leftImgRotated, rightImgRotated;
90
91
92def ConvertLeftAndRightImagesToGray(leftImg, rightImg):
93    leftImgGray = cv2.cvtColor(src=leftImg, code=cv2.COLOR_RGB2GRAY)
94    rightImgGray = cv2.cvtColor(src=rightImg, code=cv2.COLOR_RGB2GRAY)
95    return leftImgGray, rightImgGray;
96
97
98# ************************************************************************************************
99# **********                 FUNCTIONS: 3D RECONSTRUCTION                               **********
100# ************************************************************************************************
101
102def LoadCalibResult(calibFilename):
103    fs = cv2.FileStorage(source=calibFilename, flags=cv2.FILE_STORAGE_READ)
104    intrinsic  = fs.getNode('intrinsic').mat() # ref: https://github.com/opencv/opencv_contrib/issues/834
105    distortion = fs.getNode('distortion').mat()
106    rotation   = fs.getNode('rotation').mat()
107    projection = fs.getNode('projection').mat()
108    Q          = fs.getNode('Q').mat()
109    return intrinsic, distortion, rotation, projection, Q;
110
111
112def RectifyLeftAndRightImagesUsingCalibMatrices(leftImg, rightImg, \
113                                                leftIntrinsic, leftDistortion, leftRotation, leftProjection, \
114                                                rightIntrinsic, rightDistortion, rightRotation, rightProjection):
115    height, width = leftImg.shape[:2]
116
117    # precompute maps for cvRemap()
118    leftMapX, leftMapY = cv2.initUndistortRectifyMap(cameraMatrix=leftIntrinsic, distCoeffs=leftDistortion, \
119                            R=leftRotation, newCameraMatrix=leftProjection, size=(width,height), m1type=cv2.CV_16SC2)
120    rightMapX, rightMapY = cv2.initUndistortRectifyMap(cameraMatrix=rightIntrinsic, distCoeffs=rightDistortion, \
121                            R=rightRotation, newCameraMatrix=rightProjection, size=(width, height), m1type=cv2.CV_16SC2)
122                    # check for detail: http://docs.opencv.org/2.4.8/modules/imgproc/doc/geometric_transformations.html
123                    #                   http://docs.opencv.org/3.2.0/dc/dbb/tutorial_py_calibration.html
124    # rectify 2 images using maps
125    leftImgRectified = cv2.remap(src=leftImg, map1=leftMapX, map2=leftMapY, interpolation=cv2.INTER_LINEAR)
126    rightImgRectified = cv2.remap(src=rightImg, map1=rightMapX, map2=rightMapY, interpolation=cv2.INTER_LINEAR)
127    return leftImgRectified, rightImgRectified;
128
129
130def StereoMatching(leftImg, rightImg):
131    stereo = cv2.StereoSGBM_create(minDisparity     = STEREOMATCHING_SGBM_MIN_DISPARITY, \
132                                   numDisparities   = STEREOMATCHING_SGBM_N_DISPARITIES, \
133                                   blockSize        = STEREOMATCHING_SGBM_MATCHING_WIN_SIZE, \
134                                   P1               = STEREOMATCHING_SGBM_DISPARITY_SMOOTH_P1, \
135                                   P2               = STEREOMATCHING_SGBM_DISPARITY_SMOOTH_P2, \
136                                   disp12MaxDiff    = STEREOMATCHING_SGBM_MAX_DISPARITY_CHECK, \
137                                   preFilterCap     = STEREOMATCHING_SGBM_PREFILTER_CAP, \
138                                   uniquenessRatio  = STEREOMATCHING_SGBM_UNIQUENESS_RATIO, \
139                                   speckleWindowSize= STEREOMATCHING_SGBM_SPECKLE_WIN_SIZE, \
140                                   speckleRange     = STEREOMATCHING_SGBM_SPECKLE_RANGE, \
141                                   mode             = STEREOMATCHING_SGBM_MODE)
142    disparityImg = stereo.compute(leftImg, rightImg)#.astype(np.float32) / 16.0  # is 16-bit signed single-channel
143    return disparityImg;
144
145
146def StereoMatchingWithCalibFiles(leftImg, rightImg, leftCalibFilename, rightCalibFilename):
147    # load calibration files
148    leftIntrinsic, leftDistortion, leftRotation, leftProjection, leftQ = LoadCalibResult(leftCalibFilename)
149    rightIntrinsic, rightDistortion, rightRotation, rightProjection, rightQ = LoadCalibResult(rightCalibFilename)
150
151    # rotate, rectify, convert-to-gray the left and right images
152    leftImgRotated, rightImgRotated = RotateLeftAndRightImagesAccordingToOurSystem(leftImg, rightImg)
153    leftImgRectified, rightImgRectified = RectifyLeftAndRightImagesUsingCalibMatrices(leftImgRotated, rightImgRotated, \
154                                                      leftIntrinsic, leftDistortion, leftRotation, leftProjection, \
155                                                      rightIntrinsic, rightDistortion, rightRotation, rightProjection)
156    leftImgRectifiedGray, rightImgRectifiedGray = ConvertLeftAndRightImagesToGray(leftImgRectified, rightImgRectified)
157
158    # stereo matching
159    disparityImg = StereoMatching(leftImgRectifiedGray, rightImgRectifiedGray)
160    return disparityImg, leftImgRectified, leftQ;
161
162
163def CheckPlyFileExportCondition(disparityX, disparityY, disparityZ, pixelB, pixelG, pixelR):
164    # position conditions
165    condPosX = (-STEREOMATCHING_PLY_FILTER_RANGE_XY < disparityX and disparityX < STEREOMATCHING_PLY_FILTER_RANGE_XY)
166    condPosY = (-STEREOMATCHING_PLY_FILTER_RANGE_XY < disparityY and disparityY < STEREOMATCHING_PLY_FILTER_RANGE_XY)
167    condPosZ = (STEREOMATCHING_PLY_FILTER_RANGE_Z_FROM<disparityZ and disparityZ<STEREOMATCHING_PLY_FILTER_RANGE_Z_TO)
168
169    # color condition
170    condColorB = (STEREOMATCHING_PLY_FILTER_BLACK_COLOR_THR < pixelB)
171    condColorG = (STEREOMATCHING_PLY_FILTER_BLACK_COLOR_THR < pixelG)
172    condColorR = (STEREOMATCHING_PLY_FILTER_BLACK_COLOR_THR < pixelR)
173
174    return (condPosX and condPosY and condPosZ and condColorB and condColorG and condColorR);
175
176
177def SaveWorldImageToPLY(worldImg, leftImg, plyFilename):
178    worldImgHeight, worldImgWidth = worldImg.shape[:2]
179
180    with open(plyFilename, 'w') as f: # write to a new file (NOT APPEND)
181        f.write('ply\n')
182        f.write('format ascii 1.0\n')
183
184        # count possible display points
185        count = 0
186        for i in range(0, worldImgHeight):
187            for j in range(0, worldImgWidth):
188                disparityX = worldImg.item(i, j, 0) # to access all B,G,R values --> call .item() separately for all
189                disparityY = worldImg.item(i, j, 1) # http://docs.opencv.org/3.2.0/d3/df2/tutorial_py_basic_ops.html
190                disparityZ = worldImg.item(i, j, 2)
191                pixelB = leftImg.item(i, j, 0)
192                pixelG = leftImg.item(i, j, 1)
193                pixelR = leftImg.item(i, j, 2)
194                if CheckPlyFileExportCondition(disparityX, disparityY, disparityZ, pixelB, pixelG, pixelR):
195                    count = count + 1
196        f.write('element vertex ' + str(count) + '\n')
197
198        # other headers
199        f.write('property float x\n')
200        f.write('property float y\n')
201        f.write('property float z\n')
202        f.write('property uchar red\n')
203        f.write('property uchar green\n')
204        f.write('property uchar blue\n')
205        f.write('end_header\n')
206
207        # write points
208        for i in range(0, worldImgHeight):
209            for j in range(0, worldImgWidth):
210                disparityX = worldImg.item(i, j, 0)  # to access all B,G,R values --> call .item() separately for all
211                disparityY = worldImg.item(i, j, 1)  # http://docs.opencv.org/3.2.0/d3/df2/tutorial_py_basic_ops.html
212                disparityZ = worldImg.item(i, j, 2)
213                pixelB = leftImg.item(i, j, 0)
214                pixelG = leftImg.item(i, j, 1)
215                pixelR = leftImg.item(i, j, 2)
216                if CheckPlyFileExportCondition(disparityX, disparityY, disparityZ, pixelB, pixelG, pixelR):
217                    f.write(format(disparityX,'.4f') + ' ' + format(disparityY,'.4f') + ' ' + format(disparityZ,'.4f') \
218                            + ' ' + str(pixelR) + ' ' + str(pixelG) + ' ' + str(pixelB) + '\n')
219    return;
220
221
222def SaveDisparityImageToPLY(disparityImg, leftImg, perspectiveMatrix, plyFilename):
223    worldImg = cv2.reprojectImageTo3D(disparity=disparityImg, Q=perspectiveMatrix, handleMissingValues=True)
224    SaveWorldImageToPLY(worldImg, leftImg, plyFilename)
225    return;
226
227
228
229# ************************************************************************************************
230# **********                                    MAIN                                    **********
231# ************************************************************************************************
232
233leftImg = cv2.imread('0002_A01.jpeg')
234rightImg = cv2.imread('0002_A02.jpeg')
235# leftImg = cv2.imread('img2.bmp')
236# rightImg = cv2.imread('img1.bmp')
237
238start = time.time()
239disparityImg, leftImgRectified, perspectiveMatrix = StereoMatchingWithCalibFiles(leftImg, rightImg, 'A01-A02_l.txt', 'A01-A02_r.txt')
240print(time.time() - start)
241
242# if SHOW_STEREOMATCHING_DISPARITY_IMG:
243#     ShowDisparityImageSGBM(disparityImg)
244
245start = time.time()
246SaveDisparityImageToPLY(disparityImg, leftImgRectified, perspectiveMatrix, 'test.ply')
247print(time.time() - start)