def dataPrep(imgs):
# Convert the Bayer pattern encoded imageimgto acolorimage
color_image = cv2.cvtColor(imgs, cv2.COLOR_BayerGR2BGR)
# Extract the camera parameters usingReadCameraModel.py
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel(
'Oxford_dataset\model')
undistorted_image = UndistortImage(color_image, LUT)
return undistorted_image
def get_image(file):
"""
Inputs:
file: the file to be read
Outputs:
image: numpy array representing the image
"""
image = cv2.imread(file, 0)
image = cv2.cvtColor(image, cv2.COLOR_BayerGR2BGR)
fx, fy, cx, cy, camera_image, LUT = ReadCameraModel("data/model/")
k_matrix = np.zeros((3, 3))
k_matrix[0, 0] = fx
k_matrix[1, 1] = fy
k_matrix[2, 2] = 1
k_matrix[0, 2] = cx
k_matrix[1, 2] = cy
image = UndistortImage(image, LUT)
return (image, k_matrix)
SF[2, 2] = 0
F = Uf @ SF @ Vf
F = t2.T @ F @ t1
F = F / F[2, 2]
return F
if __name__ == '__main__':
value = '/home/srujan/PycharmProjects/visual_odometry/stereo/centre'
# global F
# fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel.ReadCameraModel('./model')
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel.ReadCameraModel(
'/home/srujan/PycharmProjects/visual_odometry/model')
# calibration matrix
k = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
image_list = []
filenames = [
img for img in glob.glob(
"/home/srujan/PycharmProjects/visual_odometry/stereo/centre/*.png")
]
# filenames = [img for img in glob.glob("/home/srujan/PycharmProjects/visual_odometry/samples/*.png")]
filenames.sort()
for img in filenames:
image_list.append(img)
# Hae Lee Kim
# Zhengliang Liu
# ENPM673 Project 5
import cv2
import numpy as np
import random
from matplotlib import pyplot as plt
from ReadCameraModel import *
from UndistortImage import *
import csv
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel(
'C:/Users/Zheng/ENPM673/Oxford_dataset/model/')
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
H = np.identity(4)
# print(K)
#img_green=cv2.imread("Green/green_frame%d.jpg" %i)
i = 1
# img_red=cv2.imread("img/frame%d.jpg" %i)
for i in range(740, 3872):
j = i + 1
img_1 = cv2.imread("img/frame%d.jpg" % i, 0)
img_2 = cv2.imread("img/frame%d.jpg" % j, 0)
# img1 = cv2.resize(img_1,(0,0),fx=0.5,fy=0.5)
# img2 = cv2.resize(img_2,(0,0),fx=0.5,fy=0.5)
sift = cv2.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
def readandUndistortImages(path, LUT):
imgc = []
names = []
for filename in glob.glob(path):
names.append(filename)
names.sort()
print(names)
# Making input video
# fourcc = cv2.VideoWriter_fourcc(*'XVID')
# count = 1
# for filename in names:
# img1 = cv2.imread(filename, 0)
# color_image = cv2.cvtColor(img1, cv2.COLOR_BayerGR2BGR)
# undistortimg1 = UndistortImage(color_image, LUT)
# if count == 1:
# out = cv2.VideoWriter('inputVideo.avi', fourcc, 5.0, (undistortimg1.shape[1], undistortimg1.shape[0]))
# count += 1
# cv2.imshow('image1', undistortimg1)
# if cv2.waitKey(1) & 0xFF == ord('q'):
# cv2.destroyAllWindows()
# imgc.append(undistortimg1)
# out.write(undistortimg1)
# out.release()
# return imgc
path = "Oxford_dataset/stereo/centre/*.png"
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('Oxford_dataset/model')
undistortedImages = readandUndistortImages(path, LUT)
import cv2
import glob
import random
from MyUtils import *
import ReadCameraModel as r
import matplotlib.pyplot as plt
import os
random.seed(1)
path = "/home/aditya/PycharmProjects/VisualOdometry/Undistorted2"
camera_path = "/home/aditya/Oxford_dataset/model"
save_path = r"/home/aditya/PycharmProjects/VisualOdometry/op2/frame"
fx, fy, cx, cy, G_camera_img, LUT = r.ReadCameraModel(camera_path)
camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
list_fname = [fname for fname in os.listdir(path)]
list_fname.sort()
dataset_size = len(list_fname)
p0 = np.array([0, 0, 0, 1])
H = np.eye(4)
#
for index in range(20, dataset_size - 1):
fname1 = path + "/" + list_fname[index]
fname2 = path + "/" + list_fname[index + 1]
img1 = cv2.imread(fname1, 0)
img2 = cv2.imread(fname2, 0)
# cv2.imshow("First Frame: ", img1)
# -*- coding: utf-8 -*-
"""
Created on Fri Apr 24 18:24:24 2020
@author: nsraj
"""
import cv2
import numpy as np
import glob
import matplotlib.pyplot as plt
from ReadCameraModel import *
from UndistortImage import *
import random
from skimage.measure import ransac
fx , fy , cx , cy , camera_image , LUT = ReadCameraModel ( 'model/')
K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1])
K = np.reshape(K,(3,3))
print(K)
images = np.load('image_list.npy')
# Initiate SIFT detector
orb = cv2.ORB_create()
img1 = images[300]
print('shape of img1 is :', img1.shape)
img1_orig = img1
img1 = cv2.cvtColor(img1,cv2.COLOR_RGB2GRAY)
img2 = images [301]
print('shape of img2 is :', img2.shape)
import cv2
import glob
from ReadCameraModel import *
from UndistortImage import *
i = 0
for img in glob.glob("stereo/centre/*.png"):
image = cv2.imread(img, 0)
color_image = cv2.cvtColor(image, cv2.COLOR_BayerGR2BGR)
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('.\model')
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
undistorted_image = UndistortImage(color_image, LUT)
filtered_image = cv2.GaussianBlur(undistorted_image, (5, 5), 0)
#cv2.imwrite('Data/Final_Image{}.png'.format(i),filtered_image)
#i = i+1
print(K)
def main():
########## DATA PREPARATION ##########################
fig, ax = plt.subplots(3, 2)
# Read the image in Bayer form
img_bayer = cv2.imread(
'../Oxford_dataset/stereo/centre/1399381445767267.png')
img_bayer_next = cv2.imread(
'../Oxford_dataset/stereo/centre/1399381445829757.png')
# cv2.imshow("img_bayer", img_bayer)
ax[0][0].imshow(img_bayer)
ax[0][1].imshow(img_bayer_next)
img_gray = cv2.cvtColor(img_bayer, cv2.COLOR_BGR2GRAY)
img_gray_next = cv2.cvtColor(img_bayer_next, cv2.COLOR_BGR2GRAY)
img_color = cv2.cvtColor(img_gray, cv2.COLOR_BayerGR2BGR)
img_color_next = cv2.cvtColor(img_gray_next, cv2.COLOR_BayerGR2BGR)
# cv2.imshow("img_color", img_color)
ax[1][0].imshow(img_color)
ax[1][1].imshow(img_color_next)
# Extract camera parameters
fx, fy, cx, cy, G_cam_img, lut = ReadCameraModel.ReadCameraModel(
models_dir='../Oxford_dataset/model')
# print(fx, fy, cx, cy, G_cam_img, lut)
# Undistort Image
img_undistort = UndistortImage.UndistortImage(image=img_bayer, LUT=lut)
img_undistort_next = UndistortImage.UndistortImage(image=img_bayer_next,
LUT=lut)
# cv2.imshow("img_undistort", img_undistort)
ax[2][0].imshow(img_undistort)
ax[2][1].imshow(img_undistort_next)
# plt.show()
######### PIPELINE ##################################
# Feature matching: point correspondence
features = featureMatch.featureMatch(img_1=img_undistort,
img_2=img_undistort_next)
x1 = features[:, :3]
x2 = features[:, 3:]
# print("x1", x1[0])
# print("x2", x2[0])
pts1 = np.int32(x1[:, :2])
# pts1 = pts1.reshape(-1,1,2)
pts2 = np.int32(x2[:, :2])
# pts2 = pts2.reshape(-1,1,2)
# print("pts1", pts1[0])
# print("pts2", pts2[0])
x1_inlier, x2_inlier = inliersRansac.getInliersRansac(features=features,
threshold=0.02,
size=8,
num_inliers=0.6 *
features.shape[0],
num_iters=500)
f = fundamentalMatrix.estimateFundamentalMatrix(x1=x1_inlier, x2=x2_inlier)
print("Fundamnetal Matrix Normalised: ", f)
F, _ = cv2.findFundamentalMat(x1, x2, cv2.RANSAC)
print("Fundamental Matrix Direct: ", F)
# for i in range(0,):
# x2 = x2[3]
# val_1 = np.matmul(x2.T, np.matmul(F,x1[3]))
# val_2 = np.matmul(x2.T, np.matmul(f,x1[3]))
# print("val_1: ", val_1)
# print("val_2: ", val_2)
# k = np.array([[fx, 0, cx],
# [0, fy, cy],
# [0, 0, 1]])
# print("k", k)
# sys.exit(0)
# e = essentialmatrix.estimateEssentialMatrix(fundamental_matrix=f, camera_matrix=k)
# print("e", e)
# C, R = cameraPose.extractCameraPose(essential_matrix=e)
# print("Position: ", C)
# print("Orientation: ", R)
# world_points = []
# for i in range(x1_inlier.shape[0]):
# x1 = x1_inlier[i]
# x2 = x2_inlier[i]
# for j in range(0,4):
# for k in range(0,4):
# X = triangulation.getTrinagulation(fundamental_matrix=f, c_1=C[j], c_2=C[k], r_1=R[j], r_2=R[k], x1=x1, x2=x2)
# world_points.append(X)
# world_points = np.array(world_points)
# print("world_points", world_points)
# Calculating my epiline
# my_line_1 =
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, f)
lines2 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F)
lines1 = lines1.reshape(-1, 3)
lines2 = lines2.reshape(-1, 3)
img3, img4 = drawLines.drawlines(img_color, img_color_next, lines1, pts1,
pts2)
img5, img6 = drawLines.drawlines(img_color, img_color_next, lines2, pts1,
pts2)
plt.subplot(121), plt.imshow(img3)
plt.subplot(122), plt.imshow(img4)
plt.show()
plt.subplot(221), plt.imshow(img5)
plt.subplot(222), plt.imshow(img6)
plt.show()
def main():
fig, ax = plt.subplots()
result = []
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel(
'Oxford_dataset/model')
K = intrinsicMatrix(fx, fy, cx, cy, 0)
cap = cv2.VideoCapture('../input/inputVideo.avi')
frame2 = cv2.imread('./Oxford_dataset/stereo/centre/1399381444704913.png')
frame2 = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
Ht = np.eye(4)
frameNumber = 0
while (cap.isOpened()):
frame1 = frame2
_, frame2 = cap.read()
if frame2 is None:
break
frame2 = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
#####################################################################
# USING CREATED FUNCTIONS
#####################################################################
matches, kp1, kp2, des1, des2 = featureMatching(frame1, frame2)
F, inliersP1, inliersP2 = getInliersRansac(matches, kp1, kp2)
listKp1, listKp2 = getKeyPointCoordinates(matches, kp1, kp2)
E = estimateFundamentalMatrix(K, F)
# E,_ = cv2.findEssentialMat(listKp1,listKp2,cameraMatrix=K,method=cv2.RANSAC)
R1, R2, R3, R4, C1, C2, C3, C4 = extractCameraPose(E)
Xset1 = linearTriangulation(K, np.zeros((3, 1)), np.eye(3), C1, R1,
inliersP1, inliersP2)
Xset2 = linearTriangulation(K, np.zeros((3, 1)), np.eye(3), C2, R2,
inliersP1, inliersP2)
Xset3 = linearTriangulation(K, np.zeros((3, 1)), np.eye(3), C3, R3,
inliersP1, inliersP2)
Xset4 = linearTriangulation(K, np.zeros((3, 1)), np.eye(3), C4, R4,
inliersP1, inliersP2)
Xset = [Xset1, Xset2, Xset3, Xset4]
Cset = [C1, C2, C3, C4]
Rset = [R1, R2, R3, R4]
C, R = disambiguateCameraPose(Cset, Rset, Xset)
#####################################################################
# USING OPENCV
#####################################################################
# matches,kp1,kp2,des1,des2 = featureMatching(frame1,frame2)
# listKp1,listKp2 = getKeyPointCoordinates(matches,kp1,kp2)
# Ecv,_ = cv2.findEssentialMat(listKp1,listKp2,cameraMatrix=K,method=cv2.RANSAC)
# points, R, C, mask = cv2.recoverPose(Ecv, listKp1, listKp2,cameraMatrix=K)
if C is None or R is None:
continue
####################################################################
# HOMOGENOUS TRANSFORMATION
####################################################################
Htn = np.hstack((R, C))
Htn = np.vstack((Htn, [0, 0, 0, 1]))
Ht1 = Ht @ Htn
xt1 = Ht1[0, 3]
zt1 = Ht1[2, 3]
Ht = Ht1
#####################################################################
# RESULTS
####################################################################
#---------------------------------
# results for created functions
#---------------------------------
result.append([-xt1, zt1])
ax.plot([-xt1], [zt1], 'o')
#---------------------------------
#results for opencv
#---------------------------------
# result.append([-xt1,zt1])
# ax.plot([-xt1],[zt1],'o')
plt.pause(0.01)
frameNumber += 1
print(frameNumber)
cv2.imshow('img2', frame1)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
########################################################################
# SAVE RESULTS
#######################################################################
#------------------------------
# created functions
#------------------------------
np.savetxt("result.csv", result, delimiter=",")
#------------------------------
# opencv
#------------------------------
# np.savetxt("resultcv.csv", result, delimiter=",")
cap.release()
cv2.destroyAllWindows()
import random as rand
import sympy as sp
# =============================================================================================================================================================================================================================== #
# -----> Alias <----- #
# =============================================================================================================================================================================================================================== #
inv = np.linalg.inv
det = np.linalg.det
svd = np.linalg.svd
# =============================================================================================================================================================================================================================== #
# =============================================================================================================================================================================================================================== #
# Get camera data from ReadCameraModel.py
# =============================================================================================================================================================================================================================== #
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel(
'D:/Sudharsan/Academics/Semester 02/ENPM673 - Perception/Projects/Project 05/Data/model'
)
# -----> Intrinsic Matrix of the Camera <----- #
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
K_inv = np.linalg.inv(K)
# =============================================================================================================================================================================================================================== #
# -----> Object for Zhang's Grid <----- #
# =============================================================================================================================================================================================================================== #
class Cells:
def __init__(self):
self.pts = list()
self.pairs = dict()
def rand_pt(self):
def main():
ax = plt.gca()
list_fname = [fname for fname in os.listdir(IMAGES_PATH)]
list_fname.sort()
H = np.eye(4)
p0 = np.array([0, 0, 0, 1])
# Extract the camera params
fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel.ReadCameraModel(
MODELS_PATH)
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
print(
'The extracted camera parameters are fx = {:}, fy = {:}, cx = {:}, cy = {:}, G_camera_image = {:}, LUT = {:}:'
.format(fx, fy, cx, cy, G_camera_image, LUT))
successive_frames = []
itr = 0
points = []
# Iterating through all frames in the video and doing some preprocessing
for index in range(20, len(list_fname)):
print('Image:', index)
frame1 = IMAGES_PATH + "/" + list_fname[index]
frame2 = IMAGES_PATH + "/" + list_fname[index + 1]
img1 = cv2.imread(frame1, 0)
img2 = cv2.imread(frame2, 0)
img1 = cv2.cvtColor(img1, cv2.COLOR_BayerGR2BGR)
img2 = cv2.cvtColor(img2, cv2.COLOR_BayerGR2BGR)
img1 = UndistortImage.UndistortImage(img1, LUT)
img2 = UndistortImage.UndistortImage(img2, LUT)
#img = cv2.resize(img, (0,0),fx=0.5,fy=0.5)
# Get point matches
left_pts, right_pts = featurematch(img1, img2)
#left_inliers, right_inliers, F = ransac(left_pts, right_pts)
E1, _ = cv2.findEssentialMat(left_pts,
right_pts,
focal=fx,
pp=(cx, cy),
method=cv2.RANSAC,
prob=0.999,
threshold=0.05)
#E = essential_matrix(K, F)
#print('Essential matrix:', E)
#print('Essential matrix from Opencv:', E1)
_, R, T, mask = cv2.recoverPose(E1,
left_pts,
right_pts,
focal=fx,
pp=(cx, cy))
H1 = np.hstack((R, T))
H1 = np.vstack((H1, np.array([0, 0, 0, 1])))
H = np.dot(H, H1)
#print('H:', H)
x = H[0][3]
y = H[2][3]
ax.scatter(x, y, color='b')
plt.draw()
plt.pause(0.001)
plt.savefig(r"/home/nalindas9/Desktop/op1/frame" + str(index) + ".png")
#print("(x,y)", (x,y))
points.append((x, y))
successive_frames = []
plot(points)
#cv2.imshow('Color image', img)
#cv2.waitKey(0)
cv2.destroyAllWindows()
