diff --git a/Camera.py b/Camera.py new file mode 100644 index 0000000..edac437 --- /dev/null +++ b/Camera.py @@ -0,0 +1,102 @@ +import matplotlib.pyplot as plt +import numpy as np +from scipy.optimize import least_squares +from skimage import io +from pandas import read_csv + +# local imports +from projective_transform import * +from rotation import * + + +class Camera(object): + + def __init__(self, pose, f_length, image_width, image_height): + self.p = pose + self.f = f_length + self.c = np.array([image_width, image_height]) + + def projective_transform_project(self, pose, X_world): + u,v = projective_transform(self.f, self.c[0], self.c[1], pose, X_world) + U = np.array([u,v]).T + return U + + def rotational_transform(self, pose, X_world): + x,y,z = rotate(pose, X_world) + X = np.array([x,y,z]).T + return X + + def convert_world_to_cam_coords(self, X_world): + pose = self.p.copy() + X_cam = self.rotational_transform(pose, X_world) + X_cam = self.projective_transform_project(pose, X_cam) + return X_cam + + @staticmethod + def residual(pose, self, X_world, X_cam_true): + X_cam_pred = self.rotational_transform(pose, X_world) + X_cam_pred = self.projective_transform_project(pose, X_cam_pred) + return (X_cam_pred - X_cam_true).values.flatten() + + def estimate_pose(self, X_world, X_cam): + pose = self.p + pose_opt = least_squares(self.residual, pose, method = 'lm', args=(self, X_world, X_cam)) + print(pose_opt) + self.p = pose_opt.x + return pose_opt.x + +############################################################################### +############################## MAIN ################################# +############################################################################### + +### Camera specs + +# Frankie's Photo +frankie_photo = { + 'f_length': 23.0, + 'sensor_size': 23.5, + 'width': 6000, + 'length': 4000, + 'pose_guess': [272990.0, 5193880.0, 1109.0, -3.14/2.0, 0, 0], + 'img': 'FrankiePhoto.jpg', + 'gcp': 'FrankiePhoto_GCP_DK.txt', + 'delimiter': '|', + 'header': 0 +} + +# Doug's Photo #1 +doug_photo = { + 'f_length': 27.0, + 'sensor_size': 36.0, + 'width': 3264, + 'length': 2448, + 'pose_guess': [272510.0,5193893.0, 1000.0, 3.14/2.0, 0, 0], + 'pose_true': [272470.0,5193991.0, 985.0, 1.97, 0.214, 0.01], #correct answer verified by Doug + 'img': 'DougPhoto.jpg', + 'gcp': 'DougPhoto_GCP.txt', + 'delimiter': ',', + 'header': None +} + +# Choose Photo Here! +photo = frankie_photo +photo['f_length'] = photo['f_length']/photo['sensor_size']*photo['width'] + +# Known data points +world_coords = read_csv(photo['gcp'], delimiter=photo['delimiter'], header=photo['header']) +X_world = world_coords.iloc[:,2:5] #real world coordinates +X_cam = world_coords.iloc[:,0:2] # pixel coordinates OR OBSERVED VALUES + +# Estimate Camera pose +camera = Camera(photo['pose_guess'], photo['f_length'], photo['width'], photo['length']) +est_pose = camera.estimate_pose(X_world, X_cam) + +# Plot Results +X_cam_pred = camera.convert_world_to_cam_coords(X_world) +print('X_cam_true: \n', X_cam) +print('X_cam_pred: \n', X_cam_pred) +fig, ax = plt.subplots(1, figsize=(12,8)) +ax.imshow(io.imread(photo['img'])) +ax.scatter(X_cam.iloc[:,0],X_cam.iloc[:,1],c="red",s=100) +ax.scatter(X_cam_pred[:, 0], X_cam_pred[:, 1],c="green",s=100) +plt.show() diff --git a/DougPhoto.jpg b/DougPhoto.jpg new file mode 100644 index 0000000..fa139b1 Binary files /dev/null and b/DougPhoto.jpg differ diff --git a/DougPhoto_GCP.txt b/DougPhoto_GCP.txt new file mode 100644 index 0000000..06aca06 --- /dev/null +++ b/DougPhoto_GCP.txt @@ -0,0 +1,6 @@ + +1984.76072662, 1053.33790893, 272558.68, 5193938.07, 1015 +884.77716587, 1854.85032057, 272572.34, 5193981.03, 982 +1202.65008317, 1087.94977937, 273171.31, 5193846.77, 1182 +385.08951175, 1190.83585402, 273183.35, 5194045.24, 1137 +2350.30404406, 1442.35648529, 272556.74, 5193922.02, 998 diff --git a/FrankiePhoto.jpg b/FrankiePhoto.jpg new file mode 100644 index 0000000..c048d6e Binary files /dev/null and b/FrankiePhoto.jpg differ diff --git a/FrankiePhoto_GCP.txt b/FrankiePhoto_GCP.txt new file mode 100644 index 0000000..c13a19b --- /dev/null +++ b/FrankiePhoto_GCP.txt @@ -0,0 +1,6 @@ +u|v|easting|northing|elevation|name +1660|2124|272558.75|5193938.04|1015|University Center +2265|1625|712854.27|5194473.19|1815|Black Mountain +335|2662|272604.24|5193885.80|996|Library - NorthWest Corner +4552|2188|272285.76|5194247.13|998|Buisiness Building - Roof +371|2116|272193.40|5193801.62|999|Some Dorm diff --git a/FrankiePhoto_GCP_DK.txt b/FrankiePhoto_GCP_DK.txt new file mode 100644 index 0000000..ae2fda4 --- /dev/null +++ b/FrankiePhoto_GCP_DK.txt @@ -0,0 +1,6 @@ +u|v|easting|northing|elevation|name +1660|2124|272558.75|5193938.04|1015|Steeple Point +5324|2756|272680.0|5194062.0|998|UC NorthEast Corner +335|2618|272604.24|5193885.80|996|Library - NorthWest Corner +4552|2188|272285.76|5194247.13|997|Buisiness Building - Roof +3124|1986|272190.40|5194109.62|1017|Jesse Hall diff --git a/projective_transform.py b/projective_transform.py new file mode 100644 index 0000000..ef164ae --- /dev/null +++ b/projective_transform.py @@ -0,0 +1,30 @@ +import pandas as pd +import matplotlib.pyplot as plt + +def projective_transform(focal_length, sensor_u, sensor_v, pose, X_world): + + list_u = [] + list_v = [] + #list_I = [] + + for X_point in X_world: + x = X_point[0]/X_point[2] + y = X_point[1]/X_point[2] + + u = x*focal_length + sensor_u/2.0 + v = y*focal_length + sensor_v/2.0 + + list_u.append(u) + list_v.append(v) + #list_I.append(pixel[3]) + return list_u, list_v + +############################################################################### +############################## MAIN ################################# +############################################################################### + +# axes = plt.gca() +# axes.set_xlim(0,sensor_u) +# axes.set_ylim(0,sensor_v) +# plt.scatter(list_u,list_v,c = list_I) +# plt.show() diff --git a/projective_transform.pyc b/projective_transform.pyc new file mode 100644 index 0000000..3504461 Binary files /dev/null and b/projective_transform.pyc differ diff --git a/rotation.py b/rotation.py new file mode 100644 index 0000000..4d2ee49 --- /dev/null +++ b/rotation.py @@ -0,0 +1,72 @@ +import pandas as pd +import numpy as np +import collections +from matplotlib import pyplot as plt +from matplotlib import colors +import matplotlib.cm as cmx +from mpl_toolkits.mplot3d import Axes3D +import pandas as pd + +def Trans(cam_coords): + return np.matrix([[1, 0, 0, -cam_coords[0]], + [0, 1, 0, -cam_coords[1]], + [0, 0, 1, -cam_coords[2]], + [0, 0, 0, 1]]) + +def R_yaw_trans(yaw): + return np.matrix([[np.cos(yaw), -np.sin(yaw), 0, 0], + [np.sin(yaw), np.cos(yaw), 0, 0], + [ 0, 0, 1, 0]]) + +def R_pitch_trans(pitch): + return np.matrix([[1, 0, 0], + [0, np.cos(pitch), np.sin(pitch)], + [0, -np.sin(pitch), np.cos(pitch)]]) + +def R_roll_trans(roll): + return np.matrix([[np.cos(roll), 0, -np.sin(roll)], + [0, 1, 0], + [np.sin(roll), 0, np.cos(roll)]]) + +def R_axis_trans(): + return np.matrix([[1, 0, 0], + [0, 0,-1], + [0, 1, 0]]) + +def rotate(pose, X_world): + #camera coords [easting, northing, elevation, yaw, pitch ,roll] + easting = pose[0] + northing = pose[1] + elevation = pose[2] + cam_coords = np.array([easting, northing, elevation]) + + #camera angle + yaw = pose[3] # radians + pitch = pose[4] # radians + roll = pose[5] # radians + + + T = Trans(cam_coords) + R_yaw = R_yaw_trans(yaw) + R_pitch = R_pitch_trans(pitch) + R_roll = R_roll_trans(roll) + R_axis = R_axis_trans() + C = R_axis.dot(R_roll).dot(R_pitch).dot(R_yaw).dot(T) + x = [] + y = [] + z = [] + I = [] + + for index, coord in X_world.iterrows(): + np_coord = np.array(coord) + np_coord = np.append(coord, 1) + xyz_coords = C.dot(np_coord).T + x.append(xyz_coords.item(0)) + y.append(xyz_coords.item(1)) + z.append(xyz_coords.item(2)) + + return x, y, z + + ############################################################################# + ############################## MAIN ############################### + ############################################################################# diff --git a/rotation.pyc b/rotation.pyc new file mode 100644 index 0000000..e2c57d7 Binary files /dev/null and b/rotation.pyc differ