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)
