Completed Project #1

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()

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

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

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

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()

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     ###############################
+  #############################################################################