def stereo_to_xyz(img1_name, img2_name, predictor): # read rgb images as pan img1 = cv2.imread(img1_name, 0) img2 = cv2.imread(img2_name, 0) # read rgb images rgb_img1 = tifffile.imread(img1_name) rgb_img2 = tifffile.imread(img2_name) # get RPC metadata for both images rpc1 = RPC(img1_name) rpc2 = RPC(img2_name) # find corresponding points rows = img1.shape[0] cols = img1.shape[1] F, pts1, pts2 = match_rpc(rpc1, rpc2, rows, cols) # transpose the point matrices x1 = pts1.T x2 = pts2.T # set camera matrix to identity and distortion to zero d = None K = np.identity(3) # rectify the images rimg1, rimg2, rms, max_error, lH, rH, max_yparallax = rectify_images( img1, x1, img2, x2, K, d, F, shearing=False) print('F Matrix Residual RMS Error = ', rms, ' pixels') print('F Matrix Residual Max Error = ', max_error, ' pixels') # skip this pair if residual error is too large # this is not a good indicator of failure if rms > 1.0: return None, None, None, None, None # skip this pair if max y parallax is too large if max_yparallax > 2.0: return None, None, None, None, None # crop down to the center half of the images rows = img1.shape[0] cols = img1.shape[1] row_offset = int(rows / 4) col_offset = int(cols / 4) # rectify the RGB versions of the images and crop to center rgb_rimg1, rgb_rimg2, rgb_rms, rgb_max_error = rectify_images_rgb( rgb_img1, x1, rgb_img2, x2, K, d, F, shearing=False) rimg1 = rgb_rimg1[row_offset:rows - row_offset, col_offset:cols - col_offset, :] rimg2 = rgb_rimg2[row_offset:rows - row_offset, col_offset:cols - col_offset, :] # run DenseMapNet to get stereo disparities # and run IC-Net to get semantic labels if USE_SGM: disparity = sgbm(rimg1, rimg2) else: disparity = predictor.predict_stereo(rimg1, rimg2) # disparity = predictor.predict_stereo_wls(rimg1, rimg2) seg_rimg1 = predictor.predict_semantics(rimg1) # check for invalid disparity predictions rows = rimg1.shape[0] cols = rimg1.shape[1] valid = np.ones((rows, cols), dtype=bool) valid[disparity < -DMAX_SEARCH] = 0 valid[disparity > DMAX_SEARCH] = 0 disparity[disparity < -DMAX_SEARCH] = -DMAX_SEARCH disparity[disparity > DMAX_SEARCH] = DMAX_SEARCH print('Min disparity found: ', disparity.min()) print('Max disparity found: ', disparity.max()) # create a grayscale disparity image disparity_image = (disparity + DMAX_SEARCH) / (DMAX_SEARCH * 2.0) disparity_image[disparity_image < 0.0] = 0.0 disparity_image = img_as_ubyte(disparity_image) # create a color segmentation image cls_image = category_to_color(seg_rimg1) # get left epipolar image coordinates and right coordinates from disparities # add offsets from image cropping back to image coordinates # convert coordinates to original images using homographies from rectification rows = rimg1.shape[0] cols = rimg1.shape[1] left_rows, left_cols = np.mgrid[row_offset:rows + row_offset, col_offset:cols + col_offset] right_cols = deepcopy(left_cols) - disparity right_rows = deepcopy(left_rows) # valid[right_cols < 0] = 0 # valid[right_cols > cols-1] = 0 valid[right_cols < col_offset] = 0 valid[right_cols > cols + col_offset - 1] = 0 # left_rows = left_rows[:,DMAX_SEARCH:cols-DMAX_SEARCH] # left_cols = left_cols[:,DMAX_SEARCH:cols-DMAX_SEARCH] # right_rows = right_rows[:,DMAX_SEARCH:cols-DMAX_SEARCH] # right_cols = right_cols[:,DMAX_SEARCH:cols-DMAX_SEARCH] left_rows = left_rows.ravel() left_cols = left_cols.ravel() right_rows = right_rows.ravel() right_cols = right_cols.ravel() num = len(left_cols) print('left cols = ', num) uv1 = np.array((left_cols, left_rows, np.ones(num))) print(uv1.shape) print(rH.shape) xyw = np.matmul(np.linalg.inv(rH), uv1) left_cols = xyw[0] / xyw[2] left_rows = xyw[1] / xyw[2] uv2 = np.array((right_cols, right_rows, np.ones(num))) xyw = np.matmul(np.linalg.inv(lH), uv2) right_cols = xyw[0] / xyw[2] right_rows = xyw[1] / xyw[2] # get left segmentation labels for valid disparities # left_seg = seg_rimg1[:,DMAX_SEARCH:cols-DMAX_SEARCH] left_seg = seg_rimg1 print('left_seg shape = ', left_seg.shape) left_seg = left_seg.ravel() # crop and ravel valid point list print('valid shape = ', valid.shape) # valid = valid[:,DMAX_SEARCH:cols-DMAX_SEARCH] valid = valid.ravel() print('valid shape = ', valid.shape) # approximate RPCs with local 3x4 matrices # use center coordinate of one of the images for reference clat, clon, zc = rpc1.approximate_wgs84() xc, yc, zone_number, zone_letter = wgs84_to_utm(clat, clon) R1, rms1, ic1, jc1 = rpc1.to_matrix(clat, clon, zc) R2, rms2, ic2, jc2 = rpc2.to_matrix(clat, clon, zc) # triangulate to compute XYZ coordinates for each pixel print('Triangulating...') points1 = np.array((left_cols - ic1, left_rows - jc1)) points2 = np.array((right_cols - ic2, right_rows - jc2)) xyz = cv2.triangulatePoints(R1, R2, points1, points2) xyz /= xyz[3] xyz = xyz[0:3, valid] xyz[0, :] += xc xyz[1, :] += yc xyz[2, :] += zc xyz = np.transpose(xyz) print(zone_number, zone_letter) # add cls to the xyz array xyzc = np.zeros((xyz.shape[0], xyz.shape[1] + 1)) xyzc[:, 0:3] = xyz left_seg = left_seg[valid] xyzc[:, 3] = left_seg return xyzc, rimg1, rimg2, disparity_image, cls_image