def main(): """Produce PLY point clouds from stereo image pair.""" parser = argparse.ArgumentParser(description="Read images taken with " "stereo pair and use them to produce 3D " "point clouds that can be viewed with " "MeshLab.", parents=[STEREO_BM_FLAG]) parser.add_argument("calibration", help="Path to calibration folder.") parser.add_argument("left", help="Path to left image") parser.add_argument("right", help="Path to right image") parser.add_argument("output", help="Path to output file.") parser.add_argument("--bm_settings", help="Path to block matcher's settings.") args = parser.parse_args() image_pair = [cv2.imread(image) for image in [args.left, args.right]] calib_folder = args.calibration if args.use_stereobm: block_matcher = StereoBM() else: block_matcher = StereoSGBM() if args.bm_settings: block_matcher.load_settings(args.bm_settings) camera_pair = CalibratedPair(None, StereoCalibration(input_folder=calib_folder), block_matcher) rectified_pair = camera_pair.calibration.rectify(image_pair) points = camera_pair.get_point_cloud(rectified_pair) points = points.filter_infinity() points.write_ply(args.output)
def run(): motor_controller = MotorController('/dev/cu.usbmodem14211') sp = StereoPair(DEVICES_ID) block_matcher = StereoBM() camera_pair = CalibratedPair( None, StereoCalibration(input_folder='calibration/'), block_matcher) current_value = None last_size = None i = 0 while True: frames_single_img = sp.get_frames() points = camera_pair.get_point_cloud(frames_single_img) points = points.filter_infinity() number = [] for level1 in points.coordinates: if not level1[2] == 512: number.append([0.0, 0.0, level1[2]]) number_size = len(number) if last_size: current_value = (number_size + last_size) / 2 # print current_value last_size = number_size i += 1 motor_state = False if not current_value >= 3000: print current_value motor_state = True motor_controller.send(motor_state)
class NavAid(object): def __init__(self): # Setup face detector. self._detector = FaceDetector() self._speaker = Speaker() print('=' * 80 + '\nSetting up camera feed...') self._cameras = StereoPair([1, 2]) print('-' * 80 + '\nNavAid property initialized. Ready to move!') self.stereoMatcher = cv.StereoSGBM_create() # self.stereoMatcher = cv2.StereoBM_create() self.stereoMatcher.setMinDisparity(0) self.stereoMatcher.setNumDisparities(256) self.stereoMatcher.setBlockSize(8) # self.stereoMatcher.setROI1(leftROI) # self.stereoMatcher.setROI2(rightROI) self.stereoMatcher.setSpeckleRange(3) self.stereoMatcher.setSpeckleWindowSize(8) def run(self, full_operation_mode=False): if full_operation_mode: self.setup_full_operation() plt.ion() while True: left_image, right_image = self._cameras.get_frames() if left_image is not None and right_image is not None: left_imgage, right_image = self.anonymize( left_image, right_image) cv.imshow('Left', left_image) cv.imshow('Right', right_image) if full_operation_mode: distances = self.process_depth_view( left_image, right_image) self.send_instructions(distances) self._speaker._engine.runAndWait() ch = cv.waitKey(1) if ch == 27: break cv.destroyAllWindows() def setup_full_operation(self): print( 'WARNING: This step should be run after calibration is successful.' ) self._stereo_rig = CalibratedPair( None, StereoCalibration(input_folder='calib'), StereoBM()) def process_depth_view2(self, left, right): # Rectify images. rectified_pair = self._stereo_rig.calibration.rectify([left, right]) cv.imshow('rect_left', rectified_pair[0]) cv.imshow('rect_right', rectified_pair[1]) points, disparity = self._stereo_rig.get_point_cloud(rectified_pair) points = points.filter_infinity() # points.write_ply('output/pcd.ply') # DEPTH_VISUALIZATION_SCALE = 2048 # cv.imshow('depth', disparity / DEPTH_VISUALIZATION_SCALE) cv.imshow('depth', disparity) plt.imshow(disparity) # plt.show() def process_depth_view(self, left, right): grayLeft = cv.cvtColor(left, cv.COLOR_BGR2GRAY) grayRight = cv.cvtColor(right, cv.COLOR_BGR2GRAY) depth = self.stereoMatcher.compute(grayLeft, grayRight) selem = disk(4) result = dilation(depth, selem) result = erosion(result, selem) plt.imshow(result, cmap='viridis') distances = np.ones_like(result) * 10000.0 non_zero = result > 0.0 distances[non_zero] = (F * T) / result[non_zero] # for i in range(result.shape[0]): # for j in range(result.shape[1]): # if result[i, j] > 0.0: # distances[i, j] = (F * T) / result[i, j] # else: # result[i, j] = 10000.0 return distances def send_instructions(self, depth_view): # Convert depth to instruction print('Distances', depth_view) if (depth_view < 2.0).count(): instruction = WARNING + PAUSE + ' obstacle ' + AHEAD self._speaker.say_direction(instruction) else: self._speaker.say_direction('All clear ahead.') return True def anonymize(self, left, right): left, left_faces = self._detector.find_faces_haar(left) left = self._detector.pixelate_faces(left, left_faces) right, right_faces = self._detector.find_faces_haar(right) right = self._detector.pixelate_faces(right, right_faces) return left, right def prepare_to_calibrate(self, num_pictures, output_folder='output'): progress = ProgressBar(maxval=num_pictures, widgets=[Bar("=", "[", "]"), " ", Percentage()]) if not os.path.exists(output_folder): os.makedirs(output_folder) progress.start() with ChessboardFinder((1, 2)) as pair: for i in range(num_pictures): frames = pair.get_chessboard(9, 7, True) for side, frame in zip(("left", "right"), frames): number_string = str(i + 1).zfill(len(str(num_pictures))) filename = "{}_{}.png".format(side, number_string) output_path = os.path.join(output_folder, filename) cv.imwrite(output_path, frame) progress.update(progress.maxval - (num_pictures - i)) for i in range(10): pair.show_frames(1) progress.finish() def calibrate(self, output_folder='output', calibration_folder='calib'): # Make the calibration step. print('>' * 5 + ' Performing calibration....') parser = argparse.ArgumentParser() args = parser.parse_args() args.rows = 7 args.columns = 9 args.square_size = 2.2 args.input_files = find_files(output_folder) args.output_folder = output_folder args.calibrate_folder = calibrate_folder args.show_chessboards = True calibrate_folder(args)
class Point_cloud: def __init__(self, calib_foler='calib_files_test', SGBM_setting='settings/SGBM'): self.calibration = StereoCalibration(input_folder=calib_foler) self.block_matcher = StereoSGBM() self.block_matcher.load_settings(settings=SGBM_setting) self.CalibratedPair = CalibratedPair(None, self.calibration, self.block_matcher) self.imageSize = (1280, 720) self.left_img = None self.right_img = None def load_image(self, leftPath, rightPath): self.left_img = cv2.imread(leftPath) self.right_img = cv2.imread(rightPath) def rectify_image(self): return self.calibration.rectify([self.left_img, self.right_img]) def get_disparity(self): imagePair = self.rectify_image() return self.block_matcher.get_disparity(imagePair) def get_point_cloud(self, filename): rectify_pair = self.CalibratedPair.calibration.rectify( [self.left_img, self.right_img]) points = self.CalibratedPair.get_point_cloud(rectify_pair) points = points.filter_infinity() points.write_ply(filename) def highlight_point_cloud(self, rectangle): rectify_pair = self.CalibratedPair.calibration.rectify( [self.left_img, self.right_img]) disparity = self.get_disparity() points = self.block_matcher.get_3d(disparity, self.calibration.disp_to_depth_mat) colors = rectify_pair[0] #Conver to homogeneous coord pta = rectangle[0] + (1, ) ptb = rectangle[1] + (1, ) ptc = rectangle[2] + (1, ) ptd = rectangle[3] + (1, ) for row in range(0, self.imageSize[1]): for col in range(0, self.imageSize[0]): p = (col, row, 1) if pointInQuadrilateral(p, pta, ptb, ptc, ptd): colors[row, col, 0] = 255 colors[row, col, 1] = 0 colors[row, col, 2] = 0 point_cloud = PointCloud(points, colors) point_cloud.write_ply('test.ply') def find_pos(self, quadra): rectify_pair = self.CalibratedPair.calibration.rectify( [self.left_img, self.right_img]) disparity = self.block_matcher.get_disparity(rectify_pair) points = self.block_matcher.get_3d(disparity, self.calibration.disp_to_depth_mat) #Find the larger rectangle that contains quadrilateral rmin = min(map(lambda x: x[0], quadra)) rmax = max(map(lambda x: x[0], quadra)) cmin = min(map(lambda x: x[1], quadra)) cmax = max(map(lambda x: x[1], quadra)) point_lst = [] pta = quadra[0] + (1, ) ptb = quadra[1] + (1, ) ptc = quadra[2] + (1, ) ptd = quadra[3] + (1, ) for row in range(rmin, rmax + 1): for col in range(cmin, cmax + 1): p = (col, row, 1) # if pointInQuadrilateral(p, pta, ptb, ptc, ptd): point_lst.append(points[col, row]) x_list = map(lambda x: x[0], point_lst) x_list.sort() y_list = map(lambda x: x[1], point_lst) y_list.sort() z_list = map(lambda x: x[2], point_lst) z_list.sort() print(len(x_list)) median = (x_list[len(x_list) / 2], y_list[len(y_list) / 2], z_list[len(z_list) / 3]) return median
import cv2 from stereovision.blockmatchers import StereoBM, StereoSGBM from stereovision.calibration import StereoCalibration from stereovision.stereo_cameras import CalibratedPair from stereovision.ui_utils import STEREO_BM_FLAG if __name__ == '__main__': image_pair = [ cv2.imread(image) for image in ['./test/left_test.jpg', './test/right_test.jpg'] ] calib_folder = 'calibration_result' use_stereobm = False bm_settings = 'bm_setting' if use_stereobm: block_matcher = StereoBM() else: block_matcher = StereoSGBM() if bm_settings: block_matcher.load_settings(bm_settings) camera_pair = CalibratedPair(None, StereoCalibration(input_folder=calib_folder), block_matcher) rectified_pair = camera_pair.calibration.rectify(image_pair) points = camera_pair.get_point_cloud(rectified_pair) points = points.filter_infinity() points.write_ply('./test/test_poc')
class Point_cloud: def __init__(self, calib_foler = 'calib_files_test', SGBM_setting = 'settings/SGBM'): self.calibration = StereoCalibration(input_folder=calib_foler) self.block_matcher = StereoSGBM() self.block_matcher.load_settings(settings=SGBM_setting) self.CalibratedPair = CalibratedPair(None, self.calibration, self.block_matcher) self.imageSize = (1280, 720) self.left_img = None self.right_img = None def load_image(self, leftPath, rightPath): self.left_img = cv2.imread(leftPath) self.right_img = cv2.imread(rightPath) def rectify_image(self): return self.calibration.rectify([self.left_img, self.right_img]) def get_disparity(self): imagePair = self.rectify_image() return self.block_matcher.get_disparity(imagePair) def get_point_cloud(self, filename): rectify_pair = self.CalibratedPair.calibration.rectify([self.left_img, self.right_img]) points = self.CalibratedPair.get_point_cloud(rectify_pair) points = points.filter_infinity() points.write_ply(filename) def highlight_point_cloud(self, rectangle): rectify_pair = self.CalibratedPair.calibration.rectify([self.left_img, self.right_img]) disparity = self.get_disparity() points = self.block_matcher.get_3d(disparity, self.calibration.disp_to_depth_mat) colors = rectify_pair[0] #Conver to homogeneous coord pta = rectangle[0] + (1,) ptb = rectangle[1] + (1,) ptc = rectangle[2] + (1,) ptd = rectangle[3] + (1,) for row in range(0, self.imageSize[1]): for col in range(0, self.imageSize[0]): p = (col, row, 1) if pointInQuadrilateral(p, pta, ptb, ptc, ptd): colors[row, col, 0] = 255 colors[row, col, 1] = 0 colors[row, col, 2] = 0 point_cloud = PointCloud(points, colors) point_cloud.write_ply('test.ply') def find_pos(self, quadra): rectify_pair = self.CalibratedPair.calibration.rectify([self.left_img, self.right_img]) disparity = self.block_matcher.get_disparity(rectify_pair) points = self.block_matcher.get_3d(disparity, self.calibration.disp_to_depth_mat) #Find the larger rectangle that contains quadrilateral rmin = min(map(lambda x: x[0], quadra)) rmax = max(map(lambda x: x[0], quadra)) cmin = min(map(lambda x: x[1], quadra)) cmax = max(map(lambda x: x[1], quadra)) point_lst = [] pta = quadra[0] + (1,) ptb = quadra[1] + (1,) ptc = quadra[2] + (1,) ptd = quadra[3] + (1,) for row in range(rmin, rmax + 1): for col in range(cmin, cmax + 1): p = (col, row, 1) # if pointInQuadrilateral(p, pta, ptb, ptc, ptd): point_lst.append(points[col, row]) x_list = map(lambda x: x[0], point_lst) x_list.sort() y_list = map(lambda x: x[1], point_lst) y_list.sort() z_list = map(lambda x: x[2], point_lst) z_list.sort() print(len(x_list)) median = (x_list[len(x_list) / 2], y_list[len(y_list) / 2], z_list[len(z_list) / 3]) return median