def __init__(self, cam_type='mono'): self.cam_type = cam_type self.model = load_model( '/Users/jeff/Code/harrisonscode/AutonomousVehicle/src/python/visual/model.pkl' ) cascade_filename = '/Users/jeff/Code/harrisonscode/AutonomousVehicle/src/python/visual/haarcascade/face.xml' # Recursive call to load all cascades in memory for x in xrange(0, len(filelist)): if not filelist[x] in default: continue desc = [5, 1.1] for y in xrange(0, len(descriptors)): if descriptors[y][0] == filelist[x]: desc[0] = descriptors[y][1] desc[1] = descriptors[y][2] break else: descriptors.append([filelist[x], 5, 1.1]) exec( 'self.%s = CascadedDetector(cascade_fn="%s", minNeighbors=%s, scaleFactor=%s)' % (filelist[x], filenames[x], desc[0], desc[1])) # Calibration self.calibration = StereoCalibration( input_folder= '/Users/jeff/Code/harrisonscode/AutonomousVehicle/src/python/visual/calibration_data' ) print "Models loaded: Starting streaming thread" threading.Thread(target=self.run).start()
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 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)
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 __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 start_calibration(self): calibrator = StereoCalibrator(self.rows, self.columns, self.square_size, (self.img_width, self.img_height)) photo_counter = 0 print("Start calibration, press any key on image to move to the next") while photo_counter != self.total_photos: print('Import pair No ' + str(photo_counter)) leftName = 'capture/pairs/left_' + str(photo_counter).zfill( 2) + '.png' rightName = 'capture/pairs/right_' + str(photo_counter).zfill( 2) + '.png' photo_counter = photo_counter + 1 if os.path.isfile(leftName) and os.path.isfile(rightName): imgLeft = cv2.imread(leftName, 1) imgRight = cv2.imread(rightName, 1) try: calibrator._get_corners(imgLeft) calibrator._get_corners(imgRight) except ChessboardNotFoundError as error: print(error) print("Pair No " + str(photo_counter) + " ignored") else: calibrator.add_corners((imgLeft, imgRight), True) print('End cycle') print('Starting calibration... It can take several minutes!') calibration = calibrator.calibrate_cameras() calibration.export('calibrate/calib_result') print('Calibration complete!') # Lets rectify and show last pair after calibration calibration = StereoCalibration(input_folder='calibrate/calib_result') rectified_pair = calibration.rectify((imgLeft, imgRight)) cv2.imshow('Left CALIBRATED', rectified_pair[0]) cv2.imshow('Right CALIBRATED', rectified_pair[1]) cv2.imwrite("calibrate/rectifyed_left.jpg", rectified_pair[0]) cv2.imwrite("calibrate/rectifyed_right.jpg", rectified_pair[1]) cv2.waitKey(0)
def main(): parser = ArgumentParser( description="Read images taken from a calibrated " "stereo pair, compute disparity maps from them and " "show them interactively to the user, allowing the " "user to tune the stereo block matcher settings in " "the GUI.", parents=[STEREO_BM_FLAG]) parser.add_argument( "calibration_folder", help="Directory where calibration files for the stereo " "pair are stored.") parser.add_argument("image_folder", help="Directory where input images are stored.") parser.add_argument("--bm_settings", help="File to save last block matcher settings to.", default="") args = parser.parse_args() calibration = StereoCalibration(input_folder=args.calibration_folder) input_files = find_files(args.image_folder) if args.use_stereobm: block_matcher = StereoBM() else: block_matcher = StereoSGBM() image_pair = [cv2.imread(image) for image in input_files[:2]] input_files = input_files[2:] rectified_pair = calibration.rectify(image_pair) tuner = BMTuner(block_matcher, calibration, rectified_pair) while input_files: image_pair = [cv2.imread(image) for image in input_files[:2]] rectified_pair = calibration.rectify(image_pair) tuner.tune_pair(rectified_pair) input_files = input_files[2:] for param in block_matcher.parameter_maxima: print("{}\n".format(tuner.report_settings(param))) if args.bm_settings: block_matcher.save_settings(args.bm_settings)
def __init__(self): # Depth map default preset self.SWS = 5 self.PFS = 5 self.PFC = 29 self.MDS = -30 self.NOD = 160 self.TTH = 100 self.UR = 10 self.SR = 14 self.SPWS = 100 # Camera settimgs cam_width = 1280 cam_height = 480 # Final image capture settings scale_ratio = 0.5 # Camera resolution height must be dividable by 16, and width by 32 cam_width = int((cam_width + 31) / 32) * 32 cam_height = int((cam_height + 15) / 16) * 16 print("Used camera resolution: " + str(cam_width) + " x " + str(cam_height)) #TODO Buffer for captured image settings maybe remove self.img_width = int(cam_width * scale_ratio) self.img_height = int(cam_height * scale_ratio) #self.capture = np.zeros((img_height, img_width, 4), dtype=np.uint8) #print ("Scaled image resolution: "+str(img_width)+" x "+str(img_height)) # Implementing calibration data print('Read calibration data and rectifying stereo pair...') self.calibration = StereoCalibration(input_folder='calib_result') self.disparity = np.zeros((self.img_width, self.img_height), np.uint8) self.sbm = cv2.StereoBM_create(numDisparities=0, blockSize=21) self.load_map_settings("3dmap_set.txt")
def lets_get_it_started(self): self.running = True # check files, if not - calibrate stereocamera if not os.path.exists('calib_result'): self.calibrate() print('Load settings') # load data self.calibration = StereoCalibration(input_folder='calib_result') self.distances = [] f1, frame_L = self.left_cam.read() f2, frame_R = self.right_cam.read() while self.running: if f1 and f2: tic = round(time.time(), 3) box_left, circle_left = self.search_signs(frame_L) box_right, circle_right = self.search_signs(frame_R) box_centers, circle_centers = self.paint( box_left, box_right, circle_left, circle_right, frame_L, frame_R) toc = round(time.time(), 3) cv2.putText(frame_L, str(toc - tic), (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), lineType=cv2.LINE_AA) disparity = self.create_disparity(frame_L, frame_R) for center in box_centers: dot = disparity[center[0]][center[1]] distance = 0.004 * 0.072 / 0.0495 * dot # (focal_l * base) / (size_of_pix * dispar) self.distances.append(distance) left_im = self.get_qimage(frame_L, True) right_im = self.get_qimage(frame_R, True) # disparity_map = self.get_qimage(disparity, False) self.change_left.emit(left_im) self.change_right.emit(right_im)
from stereovision.stereo_cameras import StereoPair from stereovision.stereo_cameras import CalibratedPair from stereovision.calibration import StereoCalibration from stereovision.blockmatchers import StereoBM import numpy as np # NOTE: INCLUDED CALIB FILES ARE FOR THE (LEFT, RIGHT) STEREOPAIR. # DO NOT USE (LEFT, CENTER) # camera numbers devices = (1, 2) st_pair = StereoPair(devices) st_bm = StereoBM(stereo_bm_preset=.0, search_range=80, window_size=21, settings=None) load_calib = StereoCalibration(st_pair, input_folder='calib_leftright') cal_pair = CalibratedPair(devices, load_calib, st_bm) cal_pair.live_point_cloud(devices) np.save("cloudtest.npy", np.ndarray(cal_pair.live_point_cloud(devices)))
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 Stereo_Vision(): def __init__(self): # Depth map default preset self.SWS = 5 self.PFS = 5 self.PFC = 29 self.MDS = -30 self.NOD = 160 self.TTH = 100 self.UR = 10 self.SR = 14 self.SPWS = 100 # Camera settimgs cam_width = 1280 cam_height = 480 # Final image capture settings scale_ratio = 0.5 # Camera resolution height must be dividable by 16, and width by 32 cam_width = int((cam_width + 31) / 32) * 32 cam_height = int((cam_height + 15) / 16) * 16 print("Used camera resolution: " + str(cam_width) + " x " + str(cam_height)) #TODO Buffer for captured image settings maybe remove self.img_width = int(cam_width * scale_ratio) self.img_height = int(cam_height * scale_ratio) #self.capture = np.zeros((img_height, img_width, 4), dtype=np.uint8) #print ("Scaled image resolution: "+str(img_width)+" x "+str(img_height)) # Implementing calibration data print('Read calibration data and rectifying stereo pair...') self.calibration = StereoCalibration(input_folder='calib_result') self.disparity = np.zeros((self.img_width, self.img_height), np.uint8) self.sbm = cv2.StereoBM_create(numDisparities=0, blockSize=21) self.load_map_settings("3dmap_set.txt") def stereo_depth_map(self, img_L, img_R): rectified_pair = self.calibration.rectify((img_L, img_R)) Left = rectified_pair[0] Right = rectified_pair[1] self.disparity = self.sbm.compute(img_L, img_R) #Z = (10000*6*0.3)/self.disparity[coord] return (self.disparity) def load_map_settings(self, fName): print('Loading parameters from file...') f = open(fName, 'r') data = json.load(f) self.SWS = data['SADWindowSize'] self.PFS = data['preFilterSize'] self.PFC = data['preFilterCap'] self.MDS = data['minDisparity'] self.NOD = data['numberOfDisparities'] self.TTH = data['textureThreshold'] self.UR = data['uniquenessRatio'] self.SR = data['speckleRange'] self.SPWS = data['speckleWindowSize'] #self.sbm.setSADWindowSize(SWS) self.sbm.setPreFilterType(1) self.sbm.setPreFilterSize(self.PFS) self.sbm.setPreFilterCap(self.PFC) self.sbm.setMinDisparity(self.MDS) self.sbm.setNumDisparities(self.NOD) self.sbm.setTextureThreshold(self.TTH) self.sbm.setUniquenessRatio(self.UR) self.sbm.setSpeckleRange(self.SR) self.sbm.setSpeckleWindowSize(self.SPWS) f.close() print('Parameters loaded from file ' + fName)
imageWidth = data['imageWidth'] jointWidth = data['jointWidth'] leftIndent = data['leftIndent'] rightIndent = data['rightIndent'] f.close() image_size = (imageWidth,photo_Height) # Read image and split it in a stereo pair print('Read and split image...') imgLeft = pair_img [0:photo_Height,leftIndent:imageWidth] #Y+H and X+W imgRight = pair_img [0:photo_Height,rightIndent:rightIndent+imageWidth] #Y+H and X+W # Implementing calibration data print('Read calibration data and rectifying stereo pair...') calibration = StereoCalibration(input_folder='ress') rectified_pair = calibration.rectify((imgLeft, imgRight)) # Depth map function SWS = 5 PFS = 5 PFC = 29 MDS = -25 NOD = 128 TTH = 100 UR = 10 SR = 15 SPWS = 100 def stereo_depth_map(rectified_pair):
from stereovision.calibration import StereoCalibrator, StereoCalibration from stereovision.blockmatchers import StereoBM, StereoSGBM import cv2 calib_dir = './dataset_split_v2/CAM00054' #if(not os.path.exists(calib_dir)): calibrator = StereoCalibrator(8, 5, 2, (1536, 2048)) for idx in range(1, 14): calibrator.add_corners((cv2.imread(calib_dir+"_s1.jpg"), cv2.imread(calib_dir+"_s2.jpg"))) calibration = calibrator.calibrate_cameras() print ("Calibation error:", calibrator.check_calibration(calibration)) calibration.export(calib_dir) calibration = StereoCalibration(input_folder=calib_dir) if True: block_matcher = StereoBM() else: block_matcher = StereoSGBM() for idx in range(1, 14): image_pair = (cv2.imread('images/left%02d.jpg' %idx), cv2.imread('images/right%02d.jpg' %idx)) rectified_pair = calibration.rectify(image_pair) disparity = block_matcher.get_disparity(rectified_pair) norm_coeff = 255 / disparity.max() cv2.imshow('Disparity %02d' %idx, disparity * norm_coeff / 255) for line in range(0, int(rectified_pair[0].shape[0] / 20)): rectified_pair[0][line * 20, :] = (0, 0, 255)
def CreateCalibObject(calibResultFolder): # Implementing calibration data global calibration print('Read calibration data and rectifying stereo pair...') calibration = StereoCalibration(input_folder=calibResultFolder)
img_width = int (cam_width * scale_ratio) img_height = int (cam_height * scale_ratio) capture = np.zeros((img_height, img_width, 4), dtype=np.uint8) print ("Scaled image resolution: "+str(img_width)+" x "+str(img_height)) # Initialize the camera camera = PiCamera(stereo_mode='side-by-side',stereo_decimate=False) camera.resolution=(cam_width, cam_height) camera.framerate = 20 camera.hflip = True in_folder = 'newcams_calib_result' # Implementing calibration data print('Read calibration data and rectifying stereo pair...') calibration = StereoCalibration(input_folder=in_folder) # Initialize interface windows cv2.namedWindow("Image") cv2.moveWindow("Image", 50,100) #cv2.namedWindow("left") #cv2.moveWindow("left", 450,100) #cv2.namedWindow("right") #cv2.moveWindow("right", 850,100) disparity = np.zeros((img_width, img_height), np.uint8) sbm = cv2.StereoBM_create(numDisparities=0, blockSize=21) def stereo_depth_map(rectified_pair):
from stereovision.stereo_cameras import StereoPair from stereovision.stereo_cameras import CalibratedPair from stereovision.calibration import StereoCalibration as StereoCalibration from stereovision.blockmatchers import BlockMatcher as Blockmatcher file_path = 'calib' calibration = StereoCalibration(input_folder="../../calib/") devices = (1, 2) st_pair = StereoPair(devices) bl_match = Blockmatcher(calibration) cal_pair = CalibratedPair(st_pair, calibration, bl_match) if calibration: calibration.export("calibrated_cameras")
data = json.load(f) sbm.setPreFilterType(1) sbm.setPreFilterSize(data['preFilterSize']) sbm.setPreFilterCap(data['preFilterCap']) sbm.setMinDisparity(data['minDisparity']) sbm.setNumDisparities(data['numberOfDisparities']) sbm.setTextureThreshold(data['textureThreshold']) sbm.setUniquenessRatio(data['uniquenessRatio']) sbm.setSpeckleRange(data['speckleRange']) sbm.setSpeckleWindowSize(data['speckleWindowSize'] ) f.close() sbm = cv2.StereoBM_create(numDisparities=0, blockSize=21) threading.Thread(target=load_map_settings ("3dmap_set.txt")).start() calibration = StereoCalibration(input_folder='calib_result') stop = False img_width = int ((int((1280+31)/32)*32) * 0.5) img_height = int ((int((480+15)/16)*16) * 0.5) capture = np.zeros((img_height, img_width, 4), dtype=np.uint8) camera = PiCamera(stereo_mode='side-by-side',stereo_decimate=False) camera.resolution=((int((1280+31)/32)*32), (int((480+15)/16)*16)) camera.framerate = 20 camera.vflip = True #ser = serial.Serial('/dev/ttyACM0', 9600, timeout=0) #ser.flush() disparity = np.zeros((img_width, img_height), np.uint8) theta=0 minLineLength = 5 maxLineGap = 10
import numpy as np import cv2 import glob import sys from scipy.linalg import hadamard from stereovision.calibration import StereoCalibrator from stereovision.calibration import StereoCalibration from stereovision.point_cloud import PointCloud indir = 'output' calibration = StereoCalibration(input_folder=indir) print('get camera projection matrix') R1 = np.zeros(shape=(3, 3)) R2 = np.zeros(shape=(3, 3)) P1 = np.zeros(shape=(3, 4)) P2 = np.zeros(shape=(3, 4)) Q = np.zeros(shape=(4, 4)) mtx1 = calibration.cam_mats['left'] mtx2 = calibration.cam_mats['right'] dst1 = calibration.dist_coefs['left'] dst2 = calibration.dist_coefs['right'] R = calibration.rot_mat T = calibration.trans_vec imageSize = (640, 480) height = 480 width = 640 cv2.stereoRectify(mtx1, dst1, mtx2, dst2, imageSize, R, T, R1, R2, P1, P2, Q)
class videorecClient(): calibration, face = '', '' cam_type = 'mono' def __init__(self, cam_type='mono'): self.cam_type = cam_type self.model = load_model( '/Users/jeff/Code/harrisonscode/AutonomousVehicle/src/python/visual/model.pkl' ) cascade_filename = '/Users/jeff/Code/harrisonscode/AutonomousVehicle/src/python/visual/haarcascade/face.xml' # Recursive call to load all cascades in memory for x in xrange(0, len(filelist)): if not filelist[x] in default: continue desc = [5, 1.1] for y in xrange(0, len(descriptors)): if descriptors[y][0] == filelist[x]: desc[0] = descriptors[y][1] desc[1] = descriptors[y][2] break else: descriptors.append([filelist[x], 5, 1.1]) exec( 'self.%s = CascadedDetector(cascade_fn="%s", minNeighbors=%s, scaleFactor=%s)' % (filelist[x], filenames[x], desc[0], desc[1])) # Calibration self.calibration = StereoCalibration( input_folder= '/Users/jeff/Code/harrisonscode/AutonomousVehicle/src/python/visual/calibration_data' ) print "Models loaded: Starting streaming thread" threading.Thread(target=self.run).start() def run(self): global last_ball avg = None while True: # Init vars offset, angle, sign_text = '', '', '' combined, checked_definites, past, high_confidence = [], [], [], [] current_motion, ball = [], [] current_state, in_motion, upside_down = memory.get( 'current_state').split('|||') filelist = [] exec('filelist = %s' % current_state) # Get length of buffered data length1 = recvall(sock1, 16) if length1 == None: print "Cam error" break if self.cam_type == 'stereo': length2 = recvall(sock2, 16) if length2 == None: print "Cam error" break # Process buffed data from socket into numpy buf1 = recvall(sock1, int(length1)) if self.cam_type == 'stereo': buf2 = recvall(sock2, int(length2)) data1 = numpy.fromstring(buf1, dtype='uint8') if self.cam_type == 'stereo': data2 = numpy.fromstring(buf2, dtype='uint8') # Form numpys into frames frame1 = cv2.imdecode(data1, 1) if self.cam_type == 'stereo': frame2 = cv2.imdecode(data2, 1) # Forming images resized from frames img = cv2.resize(frame1, (frame1.shape[1], frame1.shape[0]), interpolation=cv2.INTER_CUBIC) if self.cam_type == 'stereo': img2 = cv2.resize(frame2, (frame2.shape[1], frame2.shape[0]), interpolation=cv2.INTER_CUBIC) # Flips the img if upside_down == 'False': img = cv2.flip(img, 0) if self.cam_type == 'stereo': img2 = cv2.flip(img2, 1) # Disparity from the images to determine distance if self.cam_type == 'stereo': rectified_pair = self.calibration.rectify((img, img2)) disp = cv2.StereoMatcher(rectified_pair[0], rectified_pair[1]) # Motion detect if (in_motion == 'False') and (current_state == 'motion'): # Preprocessing for motion detection gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) gray = cv2.GaussianBlur(gray, (21, 21), 0) if avg is None: avg = gray.copy().astype("float") cv2.accumulateWeighted(gray, avg, 0.5) frameDelta = cv2.absdiff(gray, cv2.convertScaleAbs(avg)) thresh = cv2.threshold(frameDelta, 5, 255, cv2.THRESH_BINARY)[1] thresh = cv2.dilate(thresh, None, iterations=2) _, cnts, ret, = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for c in cnts: # If the contour is too small, ignore it if cv2.contourArea(c) < 5000: continue (x, y, w, h) = cv2.boundingRect(c) c_x = ((x + (x + w)) / 2) * 2 c_y = ((y + (y + h)) / 2) * 2 if self.cam_type == 'stereo': combined.extend([ 'motion', str(c_x), str(c_y), '100', str(disp[c_x, c_y]) ]) else: combined.extend( [['motion', str(c_x), str(c_y), '100', '0']]) # Ball Detection if current_state == 'ball': # Particle filtering #if (pf is None) and (current_state == 'ball'): # pf = ParticleFilter(NUM_PARTICLES,(-frame1.shape[0]/2, 3*frame1.shape[0]/2),(-frame1.shape[1]/2, 3*frame1.shape[1]/2),(10, max(frame1.shape)/2)) #pf.elapse_time() # Preprocessing for ball detection blurred = cv2.GaussianBlur(img, (11, 11), 0) hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, orangeLower, orangeUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) #contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2] #pf.observe(contours) #x,y,radius = pf.return_most_likely(frame1) #pf.resample() cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = cnts[0] if imutils.is_cv2() else cnts[1] center = None ball = [] # If there's at least one ball if len(cnts) > 0: c = max(cnts, key=cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) # Only proceed if the radius meets a minimum size (distance from AV) if radius > 5: #print "RADIUS: " + str(radius) if self.cam_type == 'stereo': ball = [ 'ball', str(x), str(y), '100', str(disp[x, y]) ] else: ball = [ 'ball', str(center[0]), str(center[1]), str(radius), '0' ] combined.extend([ball]) #if self.cam_type == 'stereo': ball=['ball',str(x),str(y),'100',str(disp[x,y])] #else: ball=['ball',str(x),str(y),'100','0'] ''' # Out of bounds - resort to last ball location if (float(ball[1])>float(last_ball[0]+100)) or (float(ball[1])<float(last_ball[0]-100)) or (float(ball[2])>(last_ball[0]+100)) or (float(ball[2])<(last_ball[0]-100)): if last_ball[0] != 0: ball[1] = last_ball[0] ball[2] = last_ball[1] last_ball = ball[1:][:-1] ''' # Haar cascade objects if (current_state == 'default') or (current_state == 'signs'): # Recursive call to look for matching, selected cascades for x in xrange(0, len(filelist)): for i, r in enumerate( eval('self.%s' % filelist[x]).detect(img)): # Initialize image, coords & prediction filename = filelist[x] x0, y0, x1, y1 = r exec('%s = img[y0:y1, x0:x1]' % filename) exec('%s = cv2.cvtColor(%s,cv2.COLOR_BGR2GRAY)' % (filename, filename)) if 'face' in filename: exec( '%s = cv2.resize(%s, self.model.image_size, interpolation = cv2.INTER_CUBIC)' % (filename, filename)) exec('prediction = self.model.predict(%s)[0]' % filename) self.face = self.model.subject_names[prediction] if self.cam_type == 'stereo': combined.extend([[ filename, str((x0 + x1) / 2), str((y0 + y1) / 2), '80', str(disp[(x0 + x1) / 2, (y0 + y1) / 2]) ]]) else: combined.extend([[ filename, str((x0 + x1) / 2), str((y0 + y1) / 2), '80', '0' ]]) ''' # Tensorflow objects if (current_state == 'default'): # Resize frame to 300x300 (required by tensorflow) frame_resized = cv2.resize(frame1,(300,300)) blob = cv2.dnn.blobFromImage(frame_resized, 0.007843, (300, 300), (127.5, 127.5, 127.5), False) # Set to network the input blob net.setInput(blob) # Prediction of network detections = net.forward() # Size of frame resize (300x300) cols = frame_resized.shape[1] rows = frame_resized.shape[0] # Tensorflow analysis for i in range(detections.shape[2]): confidence = detections[0, 0, i, 2] #Confidence of prediction if confidence > 0.2: # Filter prediction class_id = int(detections[0, 0, i, 1]) # Class label # Object location _xLeftBottom = int(detections[0, 0, i, 3] * cols) _yLeftBottom = int(detections[0, 0, i, 4] * rows) _xRightTop = int(detections[0, 0, i, 5] * cols) _yRightTop = int(detections[0, 0, i, 6] * rows) # Factor for scale to original size of frame heightFactor = frame1.shape[0]/300.0 widthFactor = frame1.shape[1]/300.0 # Scale object detection to frame xLeftBottom = int(widthFactor * _xLeftBottom) yLeftBottom = int(heightFactor * _yLeftBottom) yRightTop = int(heightFactor * _yRightTop) # Label and confidence of prediction in frame resized if class_id in classNames: label = classNames[class_id] labelSize, baseLine = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) yLeftBottom = max(yLeftBottom, labelSize[1]) xCenter = (_xLeftBottom+_xRightTop)/2 yCenter = (_yLeftBottom+_yRightTop)/2 if self.cam_type == 'stereo': combined.extend([[label,str(xCenter),str(yCenter),str(confidence),disp[int(widthFactor*xCenter),int(heightFactor*yCenter)]]]) else: combined.extend([[label,str(xCenter),str(yCenter),str(confidence),'0']]) ''' checked_definites = copy(combined) # Inputs current objects into past 5 collection groupings past_definites[0] = copy(combined) for count in xrange(1, len(past_definites)): past_definites[5 - count] = past_definites[4 - count] [past.extend(z) for z in past_definites] ''' # High confidence detected objects are treated as foolproof if combined and (isinstance(combined[0],list) == True): high_confidence = [z for z in combined if float(z[3]) > 99.1] elif combined and (float(combined[3]) > 99.1): high_confidence = combined for x in xrange(0,len(high_confidence)): # Add them to definite objects currently detected, if not already there if not [z for z in checked_definites if high_confidence[x][0] in checked_definites]: checked_definites.extend(high_confidence[x]) # Checks if there is correlation over time for x in xrange(0, len(past)): names_list = [z[0] for z in past] for y in xrange(0,len(names_list)): # If there are more than 4 instances of object, and object is currently being detected names_check = [z for z in checked_definites if names_list[y] in z[0]] print str(names_check) if (names_list.count(names_list[y]) > 4) and (names_check) and (not [z for z in checked_definites if high_confidence[x][0] in checked_definites]): checked_definites.extend(names_check) # Lane detection & road signs if (current_state == 'sign'): # Gets speed limit from road signs if [z for z in checked_definites if 'sign' in z[0]]: sign_text = pytesseract.image_to_string(img) # Lane Detection offset,angle = l.get_data(img) ''' ''' # Looks for objects that overlap with any balls if ball: x = int(ball[1]) y = int(ball[2]) overlap = [z for z in checked_definites if (int(z[1])>(x-30)) and (int(z[1])<(x+30)) and (int(z[2])>(y-30)) and (int(z[2])<(x+30)) and (z[0]!='ball') and (z[0] in person_features)] ''' #cv2.imshow('test',img) #if cv2.waitKey(1) == 27: break #print str(checked_definites) # Uploads the results to AV memory.set( 'objects_detected', str(checked_definites) + '|||' + sign_text + '|||' + offset + '|||' + angle) # Says we can receive what's in the buffer next sock1.send('OK') if self.cam_type == 'stereo': sock2.send('OK')
right_camera = Start_Cameras(1).start() load_map_settings("../3dmap_set.txt") cv2.namedWindow("DepthMap") while True: left_grabbed, left_frame = left_camera.read() right_grabbed, right_frame = right_camera.read() if left_grabbed and right_grabbed: #Convert BGR to Grayscale left_gray_frame = cv2.cvtColor(left_frame, cv2.COLOR_BGR2GRAY) right_gray_frame = cv2.cvtColor(right_frame, cv2.COLOR_BGR2GRAY) #calling all calibration results calibration = StereoCalibration(input_folder='../calib_result') rectified_pair = calibration.rectify( (left_gray_frame, right_gray_frame)) disparity_color, disparity_normalized = stereo_depth_map( rectified_pair) #Mouse clicked function cv2.setMouseCallback("DepthMap", onMouse, disparity_normalized) #Show depth map and image frames output = cv2.addWeighted(left_frame, 0.5, disparity_color, 0.5, 0.0) cv2.imshow("DepthMap", np.hstack((disparity_color, output))) cv2.imshow("Frames", np.hstack((left_frame, right_frame)))
def nothing(x): pass # video1 = cv2.VideoCapture(0) video1.set(3,352) video1.set(4,288) video2 = cv2.VideoCapture(1) video2.set(3,352) video2.set(4,288) # This assumes you've already calibrated your camera and have saved the # calibration files to disk. You can also initialize an empty calibration and # calculate the calibration, or you can clone another calibration from one in # memory calibration = StereoCalibration(input_folder='./data') cv2.namedWindow('Tuner') cv2.createTrackbar('Campresent','Tuner',0,3,nothing) cv2.createTrackbar('numDis','Tuner',-1600,160,nothing) cv2.createTrackbar('window_size','Tuner',0,21,nothing) cv2.createTrackbar('PreFilterCap','image',1,63,nothing) cv2.createTrackbar('PreFilterSize','image',5,255,nothing) cv2.createTrackbar('PreFilterType','image',0,1,nothing) while 1: ret1, frame1 = video1.read() ret2, frame2 = video2.read() imgR = cv2.cvtColor(frame1,cv2.COLOR_RGB2GRAY) imgL = cv2.cvtColor(frame2,cv2.COLOR_RGB2GRAY)
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
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.ui_utils import find_files, BMTuner, STEREO_BM_FLAG if __name__ == '__main__': calibration = StereoCalibration(input_folder='.\\calibration_result') input_files = find_files('.\\test') use_stereobm = False if use_stereobm: block_matcher = StereoBM() else: block_matcher = StereoSGBM() image_pair = [cv2.imread(image) for image in input_files[:2]] input_files = input_files[2:] rectified_pair = calibration.rectify(image_pair) tuner = BMTuner(block_matcher, calibration, rectified_pair) while input_files: image_pair = [cv2.imread(image) for image in input_files[:2]] rectified_pair = calibration.rectify(image_pair) tuner.tune_pair(rectified_pair) input_files = input_files[2:] for param in block_matcher.parameter_maxima: print("{}\n".format(tuner.report_settings(param))) block_matcher.save_settings('bm_setting')
leftName = './pairs/left_' + str(photo_counter).zfill(2) + '.png' rightName = './pairs/right_' + str(photo_counter).zfill(2) + '.png' if os.path.isfile(leftName) and os.path.isfile(rightName): imgLeft = cv2.imread(leftName, 1) imgRight = cv2.imread(rightName, 1) try: calibrator._get_corners(imgLeft) calibrator._get_corners(imgRight) except ChessboardNotFoundError as error: print(error) print("Pair No " + str(photo_counter) + " ignored") else: calibrator.add_corners((imgLeft, imgRight), True) print('End cycle') print('Starting calibration... It can take several minutes!') calibration = calibrator.calibrate_cameras() calibration.export('calib_result') print('Calibration complete!') # Lets rectify and show last pair after calibration calibration = StereoCalibration(input_folder='calib_result') rectified_pair = calibration.rectify((imgLeft, imgRight)) cv2.imshow('Left CALIBRATED', rectified_pair[0]) cv2.imshow('Right CALIBRATED', rectified_pair[1]) cv2.imwrite("rectifyed_left.jpg", rectified_pair[0]) cv2.imwrite("rectifyed_right.jpg", rectified_pair[1]) cv2.waitKey(0)
elif key == "7": drone.turnAngle(-10, 1) elif key == "9": drone.turnAngle(10, 1) elif key == "4": drone.turnAngle(-45, 1) elif key == "6": drone.turnAngle(45, 1) elif key == "1": drone.turnAngle(-90, 1) elif key == "3": drone.turnAngle(90, 1) elif key == "8": drone.moveUp() elif key == "2": drone.moveDown() elif key == "*": drone.doggyHop() elif key == "+": drone.doggyNod() elif key == "-": drone.doggyWag() elif key == "o": auto = not auto calibration = StereoCalibration( input_folder= "/home/pi/Desktop/programs2017/UAV-Stereo-Vision-Webcams/USBcamCalib11") block_matcher = StereoBM() # from stereovision.blockmatchers block_matcher.search_range = 48 block_matcher.bm_preset = 0 block_matcher.window_size = 31 minBoxArea = 2000 maxLengthWidthRatio = 3 height = 480 width = 640 vcL = cv2.VideoCapture(0) vcR = cv2.VideoCapture(1) vcL.set(4, height) vcL.set(3, width) vcR.set(4, height)
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())
import cv2 from stereovision.calibration import StereoCalibration import time import numpy as np import os cv2.namedWindow('disparity', cv2.WINDOW_AUTOSIZE) # left_path = os.path.join('D:\learn\8 sem\diplom\diplom', 'left1.jpg') # right_path = os.path.join('D:\learn\8 sem\diplom\diplom', 'right.jpg') calibrat = StereoCalibration( input_folder='D:\learn\8 sem\diplom\diplom\calib_result') left_cam = cv2.VideoCapture(1) right_cam = cv2.VideoCapture(2) maxDisp = 0 wsize = 9 w, h = 640, 480 while True: # f1, frame_L = left_cam.read() # f2, frame_R = right_cam.read() frame_L = cv2.imread( os.path.join('D:\learn\8 sem\diplom\diplom', 'resources\pictures', 'left1.jpg')) frame_R = cv2.imread( os.path.join('D:\learn\8 sem\diplom\diplom', 'resources\pictures', 'lright1.jpg')) rectified_pair = calibrat.rectify((frame_L, frame_R)) left = cv2.cvtColor(rectified_pair[0], cv2.COLOR_BGR2GRAY) right = cv2.cvtColor(rectified_pair[1], cv2.COLOR_BGR2GRAY)
if __name__ == "__main__": """Let user tune all images in the input folder and report chosen values.""" parser = argparse.ArgumentParser(description="Read images taken from a " "calibrated stereo pair, compute " "disparity maps from them and show them " "interactively to the user, allowing the " "user to tune the stereo block matcher " "settings in the GUI.") parser.add_argument("calibration_folder", help="Directory where calibration files for the stereo " "pair are stored.") parser.add_argument("image_folder", help="Directory where input images are stored.") args = parser.parse_args() calibration = StereoCalibration( input_folder=args.calibration_folder) input_files = find_files(args.image_folder) calibrated_pair = CalibratedPair(None, calibration,blockmatchers.StereoBM()) image_pair = [cv2.imread(image) for image in input_files[:2]] rectified_pair = calibration.rectify(image_pair) tuner = StereoBMTuner(calibrated_pair, rectified_pair) chosen_arguments = [] while input_files: image_pair = [cv2.imread(image) for image in input_files[:2]] image_names = [image for image in input_files[:2]] print image_names rectified_pair = calibration.rectify(image_pair) tuner.tune_pair(rectified_pair) chosen_arguments.append((calibrated_pair.block_matcher.stereo_bm_preset, calibrated_pair.block_matcher.search_range, calibrated_pair.block_matcher.window_size))
def __init__(self): imgLeft = cv2.imread( 'tuner/scene/left_scene.png', 0) # pair_img [0:photo_height,0:image_width] #Y+H and X+W imgRight = cv2.imread( 'tuner/scene/right_scene.png', 0 ) # pair_img [0:photo_height,image_width:photo_width] #Y+H and X+W # Implementing calibration data print('Read calibration data and rectifying stereo pair...') calibration = StereoCalibration(input_folder='calibrate/calib_result') self.rectified_pair = calibration.rectify((imgLeft, imgRight)) disparity = self.stereo_depth_map(self.rectified_pair) # Set up and draw interface # Draw left image and depth map plt.subplots_adjust(left=0.15, bottom=0.5) plt.subplot(1, 2, 1) self.dmObject = plt.imshow(self.rectified_pair[0], 'gray') saveax = plt.axes([0.3, 0.38, 0.15, 0.04]) #stepX stepY width height self.buttons = Button(saveax, 'Save settings', color=self.axcolor, hovercolor='0.975') self.buttons.on_clicked(self.save_map_settings) loadax = plt.axes([0.5, 0.38, 0.15, 0.04]) #stepX stepY width height self.buttonl = Button(loadax, 'Load settings', color=self.axcolor, hovercolor='0.975') self.buttonl.on_clicked(self.load_map_settings) plt.subplot(1, 2, 2) self.dmObject = plt.imshow(disparity, aspect='equal', cmap='jet') self.SWSaxe = plt.axes( [0.15, 0.01, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.PFSaxe = plt.axes( [0.15, 0.05, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.PFCaxe = plt.axes( [0.15, 0.09, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.MDSaxe = plt.axes( [0.15, 0.13, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.NODaxe = plt.axes( [0.15, 0.17, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.TTHaxe = plt.axes( [0.15, 0.21, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.URaxe = plt.axes( [0.15, 0.25, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.SRaxe = plt.axes( [0.15, 0.29, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.SPWSaxe = plt.axes( [0.15, 0.33, 0.7, 0.025], facecolor=self.axcolor) #stepX stepY width height self.sSWS = Slider(self.SWSaxe, 'SWS', 5.0, 255.0, valinit=5) self.sPFS = Slider(self.PFSaxe, 'PFS', 5.0, 255.0, valinit=5) self.sPFC = Slider(self.PFCaxe, 'PreFiltCap', 5.0, 63.0, valinit=29) self.sMDS = Slider(self.MDSaxe, 'MinDISP', -100.0, 100.0, valinit=-25) self.sNOD = Slider(self.NODaxe, 'NumOfDisp', 16.0, 256.0, valinit=128) self.sTTH = Slider(self.TTHaxe, 'TxtrThrshld', 0.0, 1000.0, valinit=100) self.sUR = Slider(self.URaxe, 'UnicRatio', 1.0, 20.0, valinit=10) self.sSR = Slider(self.SRaxe, 'SpcklRng', 0.0, 40.0, valinit=15) self.sSPWS = Slider(self.SPWSaxe, 'SpklWinSze', 0.0, 300.0, valinit=100) self.sSWS.on_changed(self.update) self.sPFS.on_changed(self.update) self.sPFC.on_changed(self.update) self.sMDS.on_changed(self.update) self.sNOD.on_changed(self.update) self.sTTH.on_changed(self.update) self.sUR.on_changed(self.update) self.sSR.on_changed(self.update) self.sSPWS.on_changed(self.update) print('Show interface to user') plt.show()