rcorners = cv.cornerSubPix( gray, corners, (11, 11), (-1, -1), (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)) img_points.append(rcorners) obj_points.append(objp) # Draw and display the corners cv.drawChessboardCorners(img, PATTERN, rcorners, ret) cv.imshow('Original image', img) cv.waitKey(250) cv.destroyAllWindows() #---------------------------------------------- # Get the calibrated camera parameters rms_err, calib_mtx, dist_coeffs, rvecs, tvecs = cv.calibrateCamera( obj_points, img_points, frame_size, None, None) print('Overall RMS re-projection error: %0.3f' % rms_err) #---------------------------------------------- # Save the parameters file = getSaveFileName(strfile='RoboDK-Camera-Settings.yaml', defaultextension='.yaml', filetypes=[('YAML files', '.yaml')]) cv_file = cv.FileStorage(file, cv.FILE_STORAGE_WRITE) if not cv_file.isOpened(): raise Exception('Failed to save calibration file') cv_file.write("camera_matrix", calib_mtx) cv_file.write("dist_coeff", dist_coeffs) cv_file.write("camera_size", frame_size) cv_file.release()
for m in range(12): for n in range(12): depth_crop[m][n] = int(((float(depth_crop[m][n]) - depth_min)/(depth_max-depth_min))*255) crop_show = np.zeros(3*12*12) crop_show.resize(12,12,3) crop_show[:,:,0] = depth_crop.reshape([12,12]) #blue cv2.imwrite('/home/xingduan/YupengHan/Rtest/%dcls_err/%derr_input.png' %(net, i), crop_show) for j in range(totalnum): if cls_imgs[j] == err_imgs[i]: print "j: ", j fs = cv2.FileStorage(org_xmls[j], cv2.FileStorage_READ) org_img = fs.getNode('depth').mat() fs.release() # print "org_img.shape: ", org_img.shape depth_min = org_img.min() depth_max = org_img.max() for m in range(400): for n in range(640): org_img[m][n] = int(((float(org_img[m][n]) - depth_min)/(depth_max-depth_min))*255) temp_show = np.zeros(3*400*640) temp_show.resize(400,640,3) temp_show[:,:,0] = org_img.reshape([400,640]) #blue
import numpy as np from scipy.optimize import minimize import cv2 # 棋盘格上的三维点 Pts = np.random.uniform(-15, 15, (3, 88)) Pts = np.vstack((Pts, np.ones((1, 88)))) fs = cv2.FileStorage( '/home/zexi/robotcalibration/StereoCamParams_720_Matlab.xml', cv2.FileStorage_READ) R21 = fs.getNode('R_rl').mat() t21 = fs.getNode('t_rl').mat() K1 = fs.getNode('K_left').mat() K2 = fs.getNode('K_right').mat() E = fs.getNode('E').mat() F = fs.getNode('F').mat() fs.release() print(R21) print(t21) t0, t1, t2 = t21 tprod = np.array([[0, -t2, t1], [t2, 0, -t0], [-t1, t0, 0]]) E_computed = np.array(tprod @ R21, dtype=np.float64) print(E) print(E_computed) print('---') F_computed = np.linalg.inv(K2).T @ E_computed @ np.linalg.inv(K1) print(F / F[2, 2])
import numpy as np import cv2 # Check for left and right camera IDs # These values can change depending on the system # comes from here but modified for only one camera # https://learnopencv.com/depth-perception-using-stereo-camera-python-c/ Cam_id = 1 Cam= cv2.VideoCapture(Cam_id) # Reading the mapping values for stereo image rectification rectify_map = False cv_file = cv2.FileStorage("stereo_rectify_maps.xml", cv2.FILE_STORAGE_READ) Left_Stereo_Map_x = cv_file.getNode("Left_Stereo_Map_x").mat() Left_Stereo_Map_y = cv_file.getNode("Left_Stereo_Map_y").mat() Right_Stereo_Map_x = cv_file.getNode("Right_Stereo_Map_x").mat() Right_Stereo_Map_y = cv_file.getNode("Right_Stereo_Map_y").mat() cv_file.release() def nothing(x): pass cv2.namedWindow('disp',cv2.WINDOW_NORMAL) cv2.resizeWindow('disp',600,600) cv2.createTrackbar('numDisparities','disp',1,17,nothing) cv2.createTrackbar('blockSize','disp',5,50,nothing) cv2.createTrackbar('preFilterType','disp',1,1,nothing)
# Initialize the threaded camera # You can run the unthreaded camera instead by changing the line below. # Look for any differences in frame rate and latency. camera = ThreadedWebcam() # UnthreadedWebcam() #camera = UnthreadedWebcam() camera.start() # Initialize the SimpleBlobDetector params = cv.SimpleBlobDetector_Params() detector = cv.SimpleBlobDetector_create(params) # Attempt to open a SimpleBlobDetector parameters file if it exists, # Otherwise, one will be generated. # These values WILL need to be adjusted for accurate and fast blob detection. fs = cv.FileStorage("params.yaml", cv.FILE_STORAGE_READ) #yaml, xml, or json if fs.isOpened(): detector.read(fs.root()) else: print("WARNING: params file not found! Creating default file.") fs2 = cv.FileStorage("params.yaml", cv.FILE_STORAGE_WRITE) detector.write(fs2) fs2.release() fs.release() # Create windows cv.namedWindow(WINDOW1) cv.namedWindow(WINDOW2)
def main(): import argparse parser = argparse.ArgumentParser( description='Plot camera calibration extrinsics.', formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--calibration', type=str, default='left_intrinsics.yml', help='YAML camera calibration file.') parser.add_argument('--cam_width', type=float, default=0.064 / 2, help='Width/2 of the displayed camera.') parser.add_argument('--cam_height', type=float, default=0.048 / 2, help='Height/2 of the displayed camera.') parser.add_argument('--scale_focal', type=float, default=40, help='Value to scale the focal length.') parser.add_argument( '--patternCentric', action='store_true', help='The calibration board is static and the camera is moving.') args = parser.parse_args() fs = cv.FileStorage(cv.samples.findFile(args.calibration), cv.FILE_STORAGE_READ) board_width = int(fs.getNode('board_width').real()) board_height = int(fs.getNode('board_height').real()) square_size = fs.getNode('square_size').real() camera_matrix = fs.getNode('camera_matrix').mat() extrinsics = fs.getNode('extrinsic_parameters').mat() import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-variable fig = plt.figure() ax = fig.gca(projection='3d') ax.set_aspect("auto") cam_width = args.cam_width cam_height = args.cam_height scale_focal = args.scale_focal min_values, max_values = draw_camera_boards(ax, camera_matrix, cam_width, cam_height, scale_focal, extrinsics, board_width, board_height, square_size, args.patternCentric) X_min = min_values[0] X_max = max_values[0] Y_min = min_values[1] Y_max = max_values[1] Z_min = min_values[2] Z_max = max_values[2] max_range = np.array([X_max - X_min, Y_max - Y_min, Z_max - Z_min ]).max() / 2.0 mid_x = (X_max + X_min) * 0.5 mid_y = (Y_max + Y_min) * 0.5 mid_z = (Z_max + Z_min) * 0.5 ax.set_xlim(mid_x - max_range, mid_x + max_range) ax.set_ylim(mid_y - max_range, mid_y + max_range) ax.set_zlim(mid_z - max_range, mid_z + max_range) ax.set_xlabel('x') ax.set_ylabel('z') ax.set_zlabel('-y') ax.set_title('Extrinsic Parameters Visualization') plt.show() print('Done')
object_points.append(objp) # objp are added to the object_points image_points.append(mod_corners) # Corners are added to the image_points img = cv2.drawChessboardCorners(img, (9, 6), corners, ret) # Draw and display the corners # img = cv2.drawChessboardCorners(img, (9,6), corners,ret) # cv2.imshow('img',img) # cv2.waitKey(500) # cv2.destroyAllWindows() return gray.shape[::-1], object_points, image_points image_size, object_points, left_image_points = extract_3dto2d("left") _, _, right_image_points = extract_3dto2d("right") #stereo_parameters = cv2.FileStorage("../../stereo_parameters.xml", cv2.FILE_STORAGE_READ) stereo_parameters = cv2.FileStorage("../../parameters/stereo_parameters.xml", cv2.FILE_STORAGE_READ) left_intrinsic_parameters = stereo_parameters.getNode("left_intrinsic_parameters").mat() left_distortion_coefficients = stereo_parameters.getNode("left_distortion_coefficients").mat() right_intrinsic_parameters = stereo_parameters.getNode("right_intrinsic_parameters").mat() right_distortion_coefficients = stereo_parameters.getNode("right_distortion_coefficients").mat() R = stereo_parameters.getNode("R").mat() T = stereo_parameters.getNode("T").mat() E = stereo_parameters.getNode("E").mat() F = stereo_parameters.getNode("F").mat() R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_intrinsic_parameters, left_distortion_coefficients, right_intrinsic_parameters, right_distortion_coefficients, image_size, R, T) map_left_1, map_left_2 = cv2.initUndistortRectifyMap(left_intrinsic_parameters, left_distortion_coefficients, R1, P1, image_size, cv2.CV_8UC1) map_right_1, map_right_2 = cv2.initUndistortRectifyMap( right_intrinsic_parameters, right_distortion_coefficients, R2, P2, image_size, cv2.CV_8UC1)
import time import sys import numpy as np import cv2 sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') import cv2 import cv2.aruco as aruco import numpy as np import rectangleArea as ra import math # reding calibration matrices calibrationResultPath = "res//" fsContent = cv2.FileStorage(calibrationResultPath + "calibrationValues0.yaml", cv2.FILE_STORAGE_READ) mtxNode = fsContent.getNode('camera_matrix') distNode = fsContent.getNode('dist_coeff') mtx = np.asarray(mtxNode.mat()) distor = np.asarray(distNode.mat()) print("--------------------") print(mtx) print("--------------------") print(distor) print("--------------------") # mtx = np.array([[2.6822003708394282e+03, 0., 1.5588865381021240e+03], [0., 2.6741978758743703e+03, 1.2303469240154550e+03], [0., 0., 1.]]) # distor = np.array([2.0426196677407879e-01, -3.3902097431574091e-01, -4.1813964792274307e-03, -1.0425257413809015e-02, 8.2004709580884308e-02])
continue for box in rectangles: x_left, y_top, x_right, y_bottom, _ = box crop_w = x_right - x_left + 1 crop_h = y_bottom - y_top + 1 # ignore box that is too small or beyond image border if crop_w < out_size or crop_h < out_size: continue # compute intersection over union(IoU) between current box and all gt boxes Iou = IoU(box, gts) if len(Iou) == 0: continue # save negative images and write label fs = cv2.FileStorage(depth_path, cv2.FileStorage_READ) img = fs.getNode('depth').mat() fs.release() if Iou[0] < 0.3: cropped_im = img[y_top:y_bottom + 1, x_left:x_right + 1] resized_im = cv2.resize(cropped_im, (out_size, out_size), interpolation=cv2.INTER_LINEAR) # Iou with all gts must below 0.3 save_file = os.path.join(hard_img_dir, "%d.npy" % h_id) f1.write("negative_hard/%d.npy 0\n" % h_id) np.save(save_file, resized_im) h_id += 1 print "image_idx: %d, h_id: %d" % (image_idx, h_id) f1.close()
def loadMat(file_path): fs = cv2.FileStorage(file_path, cv2.FILE_STORAGE_READ) im = fs.getNode("mat").mat() fs.release() return im
images = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12] for fname in images: print(fname) img = cv2.imread(str(fname) + '.jpg') gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, (8, 6), None) if ret == True: objpoints.append(objp) corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) imgpoints.append(corners2) img = cv2.drawChessboardCorners(img, (8, 6), corners2, ret) cv2.imshow('img', img) cv2.waitKey(500) ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) path = 'calibmat.yaml' cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_WRITE) cv_file.write("camera", mtx) cv_file.write("distort", dist) cv_file.release() cv2.destroyAllWindows()
# Get the camera capture = cv.VideoCapture(DEVICE_ID) if not capture.isOpened(): RDK.ShowMessage("Selected device id [{0}] could not be opened. Ensure the camera is plugged in and that no other application is using it.".format(DEVICE_ID)) quit() # Set the camera resolution. If the resolution is not found, it will default to the next available resolution. capture.set(cv.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH) capture.set(cv.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT) width, height = int(capture.get(cv.CAP_PROP_FRAME_WIDTH)), int(capture.get(cv.CAP_PROP_FRAME_HEIGHT)) print('Camera resolution: {0}, {1}'.format(width, height)) #---------------------------------------------- # Get the camera calibration parameters file = getOpenFileName(strfile='RoboDK-Camera-Settings.yaml', strtitle='Open calibration settings file..', defaultextension='.yaml', filetypes=[('YAML files', '.yaml')]) cv_file = cv.FileStorage(file, cv.FILE_STORAGE_READ) if not cv_file.isOpened(): RDK.ShowMessage("Failed to process calibration file") quit() CAMERA_MTX = cv_file.getNode("camera_matrix").mat() DIST_COEFFS = cv_file.getNode("dist_coeff").mat() CALIB_SIZE = cv_file.getNode("camera_size").mat().astype(int) cv_file.release() # If the calibrated camera resolution and the current resolution differs, approximate the camera matrix c_width, c_height = CALIB_SIZE if (width, height) != (c_width, c_height): RDK.ShowMessage("The calibrated resolution and the current resolution differs. Approximated calibration matrix will be used.") CAMERA_MTX[0][0] = (width / c_width) * CAMERA_MTX[0][0] # fx' = (dimx' / dimx) * fx CAMERA_MTX[1][1] = (height / c_height) * CAMERA_MTX[1][1] # fy' = (dimy' / dimy) * fy
import sys ros_cv2_path = '/opt/ros/kinetic/lib/python2.7/dist-packages' if ros_cv2_path in sys.path: sys.path.remove(ros_cv2_path) import cv2 sys.path.append(ros_cv2_path) else: import cv2 import numpy as np import os if __name__ == "__main__": # root_dir = "../real_data/12_16_20_55" root_dir = "../real_data/intrinisc_data" files = os.listdir(root_dir) fs2 = cv2.FileStorage(root_dir + '/intrinsic.yml', cv2.FileStorage_READ) # R = cv2.cv.Load(root + '/' + png1 + '.yml', name="transform") A = fs2.getNode('intrinsic').mat() discoff = fs2.getNode('dist').mat() fs2.release() for file in files: if file.endswith("color.bmp"): # if file.endswith(".bmp"): path = os.path.join(root_dir, file) img = cv2.imread(path) img_undiscoff = cv2.undistort(img, A, discoff) cv2.namedWindow("undistored", 0) cv2.imshow("undistored", img_undiscoff) cv2.waitKey(0) img = cv2.resize(img, (512, 512))
# Read Network model_name = "model-f6b98070.onnx" # MiDaS v2.1 Large #model_name = "model-small.onnx"; # MiDaS v2.1 Small # Load the DNN model model = cv2.dnn.readNet(path_model + model_name) if (model.empty()): print("Could not load the neural net! - Check path") # Set backend and target to CUDA to use GPU #model.setPreferableBackend(cv2.dnn.DNN_BACKEND_CUDA) #model.setPreferableTarget(cv2.dnn.DNN_TARGET_CUDA) cv_file = cv2.FileStorage() cv_file.open('stereoMap.xml', cv2.FileStorage_READ) Q = cv_file.getNode('q').mat() # Webcam cap = cv2.VideoCapture(0) # Read in the image success, img = cap.read() cv2.imshow('image', img) cv2.waitKey(0) img = downsample_image(img, 3)
def prepare_instance(self, idx): """ Prepare a single instance """ imshinyfolder = self.imfolders[idx] re = {} re['valid'] = True re['shinyfolder'] = imshinyfolder try: imgtname = glob.glob('%s/ori*.txt' % imshinyfolder) imgtfile = cv2.FileStorage(imgtname[0], flags=cv2.FileStorage_READ) imgt = imgtfile.getFirstTopLevelNode().mat() imgt = imgt / (np.max(imgt) + eps) imgt = np.power(imgt, 0.3) # it should be 3 channel? if not self.isrgb: imgt = cv2.cvtColor(imgt, cv2.COLOR_BGR2GRAY) imgt = np.expand_dims(imgt, axis=2) except: re['valid'] = False return re if self.ismerge: # merge with diffuse imdiffusefolder = imshinyfolder.replace(self.shinyfolder, self.diffusefolder) pos1 = imdiffusefolder.find('shine_') pos2 = imdiffusefolder.find('-rot_') strr = 'shine_0.0000' strr0 = imdiffusefolder[pos1:pos2] imdiffusefolder = imdiffusefolder.replace(strr0, strr) re['diffusefolder'] = imdiffusefolder # we will go along ourter border, but drop some images idxs = [] lightxnum = 7 lightynum = 7 for x in range(lightxnum): for y in range(lightynum): borx = (x == 0) or (x == lightxnum - 1) bory = (y == 0) or (y == lightxnum - 1) bor = borx or bory if bor: idx = x * lightynum + y idxs.append(idx) # next, w will random select them for i in range(len(idxs)): if np.random.rand() < self.dropratio: idxs[i] = -1 # read images limages_ims = [] limages_lposes = [] limages_maxvals = [] idx = -1 gtheight, gtwidth, _ = imgt.shape for x in range(lightxnum): for y in range(lightynum): borx = (x == 0) or (x == lightxnum - 1) bory = (y == 0) or (y == lightynum - 1) bor = borx or bory if not bor: continue lx = 0.4 * (x - 3) ly = 0.4 * (y - 3) lxim = lx * np.ones(shape=(gtheight, gtwidth, 1), dtype=np.float32) lyim = ly * np.ones(shape=(gtheight, gtwidth, 1), dtype=np.float32) lightpos = np.concatenate((lxim, lyim), axis=2) limages_lposes.append(lightpos) idx += 1 # assume it is a bad capture! if idxs[idx] == -1: im = np.zeros((gtheight, gtwidth, 1), dtype=np.float32) if self.isrgb: im = np.tile(im, (1, 1, 3)) limages_ims.append(im) limages_maxvals.append(0) continue imshy = cv2.imread( '%s/combine-light_%d_%d.png' % (imshinyfolder, x, y), self.loader) if imshy is None: re['valid'] = False return re # blur it imshy = cv2.blur(imshy, (5, 5)).astype(np.float32) / 255.0 if not self.isrgb: imshy = np.expand_dims(imshy, axis=2) if self.ismerge: imdiff = cv2.imread( '%s/combine-light_%d_%d.png' % (imdiffusefolder, x, y), self.loader) if imdiff is None: re['valid'] = False return re imdiff = cv2.blur(imdiff, (5, 5)).astype(np.float32) / 255.0 if not self.isrgb: imdiff = np.expand_dims(imdiff, axis=2) # ratio = 0.7 + 0.2 * np.random.rand() # ratio = 0.1 * np.random.rand() ratio = self.mergelow + self.mergerange * np.random.rand() # print ratio im = (1 - ratio) * imshy + ratio * imdiff else: im = imshy # noise! noise1 = 0.05 * np.random.randn(256, 256, 1) noise2 = 0.03 * np.random.randn(256, 256, 1) im = im + noise1 + noise2 * im im = np.clip(im, 0.0, 1.0) bor = np.random.randint(9) im = border0(im, bor) ''' maskim = self.masks[idx] im = im * maskim ''' limages_ims.append(im) limages_maxvals.append(np.max(im)) maxval = np.max(limages_maxvals) + eps limages_ims = [im / maxval for im in limages_ims] limages_lightims = [ np.concatenate((im * 2 - 1, lxy), axis=2) for im, lxy in zip(limages_ims, limages_lposes) ] re['imlight'] = np.concatenate(limages_lightims, axis=2) re['gt'] = imgt * 2 - 1 if self.datadebug: cv2.imshow("imgt", imgt) cv2.waitKey(0) imall = np.concatenate(limages_ims, axis=1) cv2.imshow("merge", imall) cv2.waitKey(0) return re
import numpy as np import gazeClass as gc import cv2 import face_alignment from skimage import io import dlib from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt import math from gazeApi import * # Read camera parameters(intrinsic and extrinsic parameters) dtype = np.float64 fs = cv2.FileStorage("camera.yaml", cv2.FILE_STORAGE_READ) para = np.array(fs.getNode("extrinsic_parameters").mat(), np.float64) # Compute screen pose camera = gc.Camera() screen = gc.Screen() camera.R_camInScreen, camera.t_camInScreen = screen.ScreenPoseEstimation(para[:, 0:3], para[:, 3:6], camera) # Get 3d facial landmark model input_name = 'lk' pic = io.imread('./pic/' + input_name + '.jpg') fa = face_alignment.FaceAlignment(face_alignment.LandmarksType._3D, enable_cuda=True, flip_input=False) preds = fa.get_landmarks(pic)[-1] landmark3d = get_3dLandmarks_from_face_alignment(preds) # input_name2 = 'lk3' # pic2 = io.imread('./pic/' + input_name2 + '.jpg') # fa2 = face_alignment.FaceAlignment(face_alignment.LandmarksType._2D, enable_cuda=True, flip_input=False)
import cv2 from lk import * from warps import RBWarp from skimage.filters import sobel fs = cv2.FileStorage("test_data/tsukuba_depth_L_00001.xml", cv2.FILE_STORAGE_READ) fn = fs.getNode("depth") ref_depth = fn.mat() fs = cv2.FileStorage("test_data/tsukuba_depth_L_00030.xml", cv2.FILE_STORAGE_READ) fn = fs.getNode("depth") cur_depth = fn.mat() ref_img = cv2.imread('test_data/tsukuba_rgb_1.png', cv2.IMREAD_GRAYSCALE) cur_img = cv2.imread('test_data/tsukuba_rgb_30.png', cv2.IMREAD_GRAYSCALE) #Depth is in cm so convert to m ref_depth[:] = ref_depth[:] / 100.0 num_levels = 3 ref_pyr = [ref_img] cur_pyr = [cur_img] ref_depth_pyr = [ref_depth] Ks = [TsukubaCameraK()] for level in range(0, num_levels - 1): ref_pyr.append(cv2.pyrDown(ref_pyr[level])) cur_pyr.append(cv2.pyrDown(cur_pyr[level])) ref_depth_pyr.append(pyrdown_median(ref_depth_pyr[level]))
return rectangles threshold = [0.92, 0.5, 0] test_name = '/home/hanyupeng/Project/only4test.txt' test_file = open(test_name, 'r') test_data = test_file.readlines() test_data = [x.strip('').split(' ') for x in test_data] test_data = [x[0] for x in test_data] idx = -1 for img_path in test_data: img_path = '/home/hanyupeng/DepthData/' + img_path idx += 1 rectangles = detectFace(img_path, threshold) fs = cv2.FileStorage(img_path, cv2.FileStorage_READ) img = fs.getNode('depth').mat() fs.release() draw = img.copy() draw = trans2img(draw) for i in range(len(rectangles)): # save_path = '/%d.png' %i draw = draw_rect(draw, rectangles[i],'BlueViolet') cv2.imwrite('/home/xingduan/YupengHan/inference/test_result/%d.jpg' %idx, draw)
fIdx = 57 import odotools as odotools file_directory = "../../../Datasets/kitti/odometry/poses" dataset = '../../../Datasets/kitti/odometry/00/image_2' odotools.read_poses( file_directory, "00", fIdx) gt_poses = np.zeros((0, 2), dtype=np.float) # (x, y) relative camera coordinates i = 1 poses = np.zeros((0, 2), dtype=np.float) # (x, y) relative camera coordinates t_p = np.zeros(3).transpose() despl = np.zeros(3).transpose() fs = cv2.FileStorage("../C++/ODO/Output_fast_complete.yaml", cv2.FILE_STORAGE_READ) fn = fs.getNode("Trayectoria") data = np.asanyarray(fn.mat()) print(fn.mat()) while True: im1_path = '%s/%06d.png' % (dataset, fIdx) im2_path = '%s/%06d.png' % (dataset, fIdx + 1) im1 = cv2.imread(im1_path) im2 = cv2.imread(im2_path) if fIdx == 500: plt.waitforbuttonpress() if im1 is None: raise Exception("File %s does not exist" % im0_path)
def detectFace(img_path, threshold): # img = cv2.imread(img_path) fs = cv2.FileStorage(img_path, cv2.FileStorage_READ) img = fs.getNode('depth').mat() fs.release() origin_h, origin_w= img.shape scales = tools.calculateScales(img) # scales = [0.1, 0.07] out = [] for scale in scales: hs = int(origin_h * scale) ws = int(origin_w * scale) net_12.blobs['data'].reshape(1, 1, hs, ws) net_12.blobs['data'].data[...] = preprocess(img, (ws, hs)) out_ = net_12.forward() out.append(out_) image_num = len(scales) rectangles = [] for i in range(image_num): cls_prob = out[i]['prob1'][0][1] print "cls_prob.shape: ", cls_prob.shape roi = out[i]['conv4-2'][0] out_h, out_w = cls_prob.shape print "out_h: ", out_h print "out_w: ", out_w out_side = max(out_h, out_w) rectangle = tools.detect_face_12net( cls_prob, roi, out_side, 1 / scales[i], origin_w, origin_h, threshold[0]) rectangles.extend(rectangle) rectangles = tools.NMS(rectangles, 0.7, 'iou') if len(rectangles) == 0: print "rect drop to 0 at 12net" return rectangles doc12 = open('/home/xingduan/YupengHan/inference/saving_docs/12/12doc.txt', 'w') for temp_rectangle in rectangles: doc12.write('%d %d %d %d %f\n' %(temp_rectangle[0], temp_rectangle[1], temp_rectangle[2], temp_rectangle[3], temp_rectangle[4])) # Here might be a problme net_24.blobs['data'].reshape(len(rectangles), 1, 24, 24) crop_number = 0 for rectangle in rectangles: rectangle = rectangular2square(rectangle, img) crop_img = img[int(rectangle[1]):int( rectangle[3]), int(rectangle[0]):int(rectangle[2])] net_24.blobs['data'].data[crop_number] = preprocess(crop_img, (24, 24)) crop_number += 1 # Here might be a problme out = net_24.forward() cls_prob = out['prob1'] roi_prob = out['ip_roi'] rectangles = tools.filter_face_24net( cls_prob, roi_prob, rectangles, origin_w, origin_h, threshold[1]) doc24 = open('/home/xingduan/YupengHan/inference/saving_docs/24/24doc.txt', 'w') for temp_rectangle in rectangles: doc24.write('%d %d %d %d %f\n' %(temp_rectangle[0], temp_rectangle[1], temp_rectangle[2], temp_rectangle [3], temp_rectangle[4])) if len(rectangles) == 0: print "rect drop to 0 at 24net" return rectangles net_48.blobs['data'].reshape(len(rectangles), 1, 48, 48) crop_number = 0 for rectangle in rectangles: rectangle = rectangular2square(rectangle, img) crop_img = img[int(rectangle[1]):int( rectangle[3]), int(rectangle[0]):int(rectangle[2])] net_48.blobs['data'].data[crop_number] = preprocess(crop_img, (48, 48)) crop_number += 1 out = net_48.forward() cls_prob = out['prob1'] roi_prob = out['ip_roi'] rectangles = tools.filter_face_48net( cls_prob, roi_prob, rectangles, origin_w, origin_h, threshold[2]) return rectangles
cv2.imshow("Result", output) # Recontruct face using mean face and EigenFaces def reconstructFace(*args): # Start with the mean / average face output = averageFace for i in range(0, args[0]): weight = np.dot(imVector, eigenVectors[i]) output = output + eigenFaces[i] * weight displayResult(im, output) # Read model file modelFile = "C:/Users/Zanis/Desktop/AI Docs/AI02/PCA/newDB.yml" file = cv2.FileStorage(modelFile, cv2.FILE_STORAGE_READ) # Extract mean vector mean = file.getNode("mean").mat() # Extract Eigen Vectors eigenVectors = file.getNode("eigenVectors").mat() # Extract size of the images used in training sz = file.getNode("size").mat() sz = (int(sz[0, 0]), int(sz[1, 0]), int(sz[2, 0])) print("size of eigenfaces : ", sz) numEigenFaces = eigenVectors.shape[0] # shape[0] get size of eigenvectors.. # Extract mean vector and reshape it to obtain average face averageFace = mean.reshape(sz)
# _*_ coding:utf-8 _*_ # @time: 2020/9/29 下午4:41 # @author: 张新新 # @email: [email protected] import math import numpy as np import cv2 w = 640 h = 480 theta = 57 * math.pi / 180 Df = 3.5 Dn = 0.01 fx = fy = w / (2 * (math.tan(theta / 2))) u0 = w / 2 v0 = h / 2 print(fx) intrinsic = np.array([[fx, 0, u0], [0, fy, v0], [0, 0, 1]]) dist = np.array([0, 0, 0, 0, 0]) fs = cv2.FileStorage("../config/intrinsic_gt.yml", cv2.FileStorage_WRITE) fs.write("intrinsic", intrinsic) fs.write("dist", dist)
# # if calibration pattern is found, add object points, # # image points (after refining them) # if ret == True: # objpoints.append(objp) # # # Refine the corners of the detected corners # corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) # imgpoints.append(corners2) # # # Draw and display the corners # img = cv2.drawChessboardCorners(img, (7,6), corners2,ret) # ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None) # File storage in OpenCV cv_file = cv2.FileStorage("calib_images/test.yaml", cv2.FILE_STORAGE_READ) # Note : we also have to specify the type # to retrieve otherwise we only get a 'None' # FileNode object back instead of a matrix mtx = cv_file.getNode("camera_matrix").mat() dist = cv_file.getNode("dist_coeff").mat() cv_file.release() # times = [] ###------------------ ARUCO TRACKER --------------------------- while (True): # # Start time
def planarHomography(imgPath, indicator): img_l = cv2.imread(imgPath) m = np.array([[8, 0, 0], [0, 8, 0], [0, 0, 1]]).T.reshape(3, 3) n = np.array([[2, 5, 9], [1, 7, 3], [2, 7, 3], [3, 5, 3]]) #print(m) objp = np.zeros((9 * 6, 3), np.float32) objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) objp = np.dot(objp, m) for x in range(54): objp[x][0] += 200 objp[x][1] += 400 objpoints = [] # 2d point in real world space imgpoints = [] # 2d points in image plane. # gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY) # Loading parameters fs_l = cv2.FileStorage("../../parameters/left_camera_intrinsics.xml", cv2.FILE_STORAGE_READ) cameraMatrix_l = fs_l.getNode("camera_intrinsic") distMatrix_l = fs_l.getNode("distort_coefficients") fs_projection = cv2.FileStorage( "../../parameters/stereo_rectification.xml", cv2.FILE_STORAGE_READ) projMtx1 = fs_projection.getNode("rectified_projection_matrix_1") projMtx2 = fs_projection.getNode("rectified_projection_matrix_2") gray = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY) # Find the chess board corners ret, corners = cv2.findChessboardCorners(gray, (9, 6), None) if ret: objpoints.append(objp) corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) imgpoints.append(corners2) # Draw and display the corners img = cv2.drawChessboardCorners(img_l, (9, 6), corners2, ret) #cv2.imwrite(r'../../output/task_5/l_calibrated.png', img) #cv2.imshow("Corners", img) #cv2.waitKey(0) #print(corners) # Undistort left image h, w = img_l.shape[:2] mapx, mapy = cv2.initUndistortRectifyMap(cameraMatrix_l.mat(), distMatrix_l.mat(), None, projMtx1.mat(), (w, h), 5) dst = cv2.remap(img_l, mapx, mapy, cv2.INTER_LINEAR) if indicator is "l0": cv2.imwrite("../../output/task_5/undistored_left_0.png", dst) else: cv2.imwrite("../../output/task_5/undistored_left_1.png", dst) # Finding homography h, status = cv2.findHomography(corners2, objp) print(h) img_warp = cv2.warpPerspective(dst, h, (dst.shape[1], dst.shape[0])) if indicator is "l0": cv2.imwrite("../../output/task_5/warped_left_0.png", img_warp) else: cv2.imwrite("../../output/task_5/warped_left_1.png", img_warp)
else: quit() # quit only if you cannot see nrows and ncols else: config_file = rospy.get_param('~config_file') print('++++++++\n++++++++ config_file: ', config_file) if not os.path.isfile(config_file): print('FATAL...cannot find config_file: ', config_file) rospy.logerr( '[whole_image_descriptor_compute_server]FATAL...cannot find config_file: ' + config_file) quit() print('++++++++ READ opencv-yaml file: ', config_file) fs = cv2.FileStorage(config_file, cv2.FILE_STORAGE_READ) fs_image_width = int(fs.getNode("image_width").real()) fs_image_height = int(fs.getNode("image_height").real()) print('++++++++ opencv-yaml:: image_width=', fs_image_width, ' image_height=', fs_image_height) print('++++++++') ## ## Load nrows and ncols directly as config ## if True: # read from param `nrows` and `ncols` if fs_image_width < 0: if (not rospy.has_param('~nrows') or not rospy.has_param('~ncols') or not rospy.has_param('~nchnls')): print( 'FATAL...cannot find param either of ~nrows, ~ncols, ~nchnls. This is needed to determine size of the input image to allocate GPU memory'
# -*- coding: utf-8 -*- """ Created on Thu Mar 11 13:15:19 2021 @author: ghedi """ import numpy as np import cv2 import matplotlib.pyplot as plt %matplotlib qt for i in range(4): plt.figure() fs = cv2.FileStorage('histogram' + str(i)+ '.yml', cv2.FILE_STORAGE_READ) mat = fs.getNode("hist").mat() plt.imshow(mat, aspect='auto') plt.figure() fs = cv2.FileStorage('histogram' + str(i+10)+ '.yml', cv2.FILE_STORAGE_READ) mat = fs.getNode("hist").mat() plt.imshow(mat, aspect='auto')
sys.exit() cameras = [] for i in range(cam_list.GetSize()): cam = cam_list.GetByIndex(i) print("camera {} serial: {}".format(i, cam.GetUniqueID())) camera_serial = args["camera"] if camera_serial == "0": camera_serial = cam_list.GetByIndex(0).GetUniqueID() print("no camera specified (use -c), using the first one in the list {}". format(camera_serial)) filename = "models/{}.xml".format(camera_serial) fs = cv2.FileStorage(filename, cv2.FILE_STORAGE_READ) if fs.isOpened() == False: print("couldn't open camera calibration {} aborting".format(filename)) sys.exit() intrinsics = fs.getNode("Intrinsics").mat() dist_coeffs = fs.getNode("Distortion").mat() fs.release() cam = cam_list.GetBySerial(camera_serial) try: cam.Init() cam.AcquisitionMode.SetValue(PySpin.AcquisitionMode_Continuous)
def run(self): ''' Get stream, read aruco, get data, start controller ''' print("INIT: Starting controller!") #One last warning before starting user_inp = input("Enter q to exit. Just press enter to start\n") if user_inp == "q": print("EXIT: Controller exited!") return 0 #First frame takes time to load up #Take off self.drone.takeoff() time.sleep(1) self.take_off_activated = True print("LOG: Successful takeoff") #NOTE: First few frames take time to flush out #Run while emergency land is off img_id = 0 while not self.emergency_land_activated: #Key press k = cv2.waitKey(1) #Detect marker _, frame = self.cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) corners, ids, rejected = aruco.detectMarkers(gray, self.aruco_dict, parameters = self.params) #Draw the marker self.detected = aruco.drawDetectedMarkers(frame, corners) #Get the rotation and translation vectors if np.all(ids != None): rvec_list = [] tvec_list = [] #Get the estimated pose of the marker wrt camera, marker size = 9cm rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, 9, self.cam_mtx, self.dist_coeff) #Also draw axes for i in range(len(ids)): aruco.drawAxis(self.detected, self.cam_mtx, self.dist_coeff, rvecs[i], tvecs[i], 4.5) #Get the rotation matrix using Rodrigues formula r_mat, _ = cv2.Rodrigues(rvecs[0][0]) #The only translation vector to use t_vec = tvecs[0][0] #Get transformation matrix self.M_mat = np.zeros((4,4)) self.M_mat[:3, :3] = r_mat self.M_mat[:3, 3] = t_vec self.M_mat[3,3] = 1 #Get inverse transformation, of camera wrt marker self.M_inv = np.linalg.inv(self.M_mat) #Get camera location wrt marker self.cam_coords = self.M_inv[:3, 3] # print("Camera coordinates:", self.cam_coords) self.cv_file = cv2.FileStorage("drone_loc.yaml", cv2.FILE_STORAGE_WRITE) self.cv_file.write("curr_loc", self.cam_coords) self.cv_file.release() #Show detection cv2.imshow("Tello Detected", self.detected) #Take pictures with s if k == ord("s"): cv2.imwrite("tello_img{0}.jpg".format(img_id), frame) print("LOG: Saved image as tello_img{0}.jpg".format(img_id)) img_id += 1 #Emergency land if k == 27: self.emergency_land() break #End statement self.cv_file = cv2.FileStorage("drone_loc.yaml", cv2.FILE_STORAGE_WRITE) self.cv_file.write("curr_loc", np.array([0,0,0])) self.cv_file.release() print("EXIT: Exited successfully")
def SaveMtx(mtx, dist, path='calibration.yml'): # Python code to write the image (OpenCV 3.2) fs = cv2.FileStorage(path, cv2.FILE_STORAGE_WRITE) fs.write('camera_matrix', mtx) fs.write('dist_coeff', dist) fs.release()
rospy.init_node('oneshot_detection_client', anonymous=True) pub_topic_name = rospy.get_param('~output_topic', '/prometheus/object_detection/oneshot_det') config = rospy.get_param('~camera_parameters', 'camera_param.yaml') OBJECT_NAME = rospy.get_param('~object_name', 'object') OBJECT_HEIGHT = rospy.get_param('~object_height', 0.2) # m # print(pub_topic_name) # print(object_names_txt) # print(cls_names) # print(config) fs = cv2.FileStorage(config, cv2.FileStorage_READ) image_width = int(fs.getNode('image_width').real()) image_height = int(fs.getNode('image_height').real()) camera_matrix = fs.getNode('camera_matrix').mat() distortion_coefficients = fs.getNode('distortion_coefficients').mat() fs.release() print(image_width) print(image_height) print(camera_matrix) print(distortion_coefficients) print(OBJECT_NAME) print(OBJECT_HEIGHT) pub = rospy.Publisher(pub_topic_name, MultiDetectionInfo, queue_size=1) rate = rospy.Rate(100)