import pickle
import cv2
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.image as mpimg

# Read in the saved objpoints and imgpoints
dist_pickle = pickle.load( open( "wide_dist_pickle.p", "rb" ) )
objpoints = dist_pickle["objpoints"]
imgpoints = dist_pickle["imgpoints"]

#objpoints = [] # 3D points in real world space
#imgpoints = [] # 2D points in image plane
#objp = np.zeros((6*8,3), np.float32)
#objp[:,:2] = np.mgrid[0:8,0:6].T.reshape(-1,2) # x, y coordinates
#imgpoints = []

# Read in an image
img = cv2.imread('test_image.png')
#img = cv2.imread('calibration_test.png')

# TODO: Write a function that takes an image, object points, and image points
# performs the camera calibration, image distortion correction and
# returns the undistorted image
def cal_undistort(img, objpoints, imgpoints):
    # Use cv2.calibrateCamera() and cv2.undistort()
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[1:], None, None)
    undist = cv2.undistort(img, mtx, dist, None, mtx)
    return undist


undistorted = cal_undistort(img, objpoints, imgpoints)

f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(undistorted)
ax2.set_title('Undistorted Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
plt.show()

import pickle
import cv2
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.image as mpimg

# Read in the saved objpoints and imgpoints
dist_pickle = pickle.load( open( "wide_dist_pickle.p", "rb" ) )
objpoints = dist_pickle["objpoints"]
imgpoints = dist_pickle["imgpoints"]

#objpoints = [] # 3D points in real world space
#imgpoints = [] # 2D points in image plane
#objp = np.zeros((6*8,3), np.float32)
#objp[:,:2] = np.mgrid[0:8,0:6].T.reshape(-1,2) # x, y coordinates
#imgpoints = []

# Read in an image
img = cv2.imread('test_image.png')
#img = cv2.imread('calibration_test.png')

# TODO: Write a function that takes an image, object points, and image points
# performs the camera calibration, image distortion correction and
# returns the undistorted image
def cal_undistort(img, objpoints, imgpoints):
    # Use cv2.calibrateCamera() and cv2.undistort()
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img.shape[1:], None, None)
    undist = cv2.undistort(img, mtx, dist, None, mtx)
    return undist


undistorted = cal_undistort(img, objpoints, imgpoints)

f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(undistorted)
ax2.set_title('Undistorted Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
plt.show()

# Read in the saved objpoints and imgpoints #objpoints = [] # 3D points in real world space #imgpoints = [] # 2D points in image plane #objp = np.zeros((6*8,3), np.float32) #objp[:,:2] = np.mgrid[0:8,0:6].T.reshape(-1,2) # x, y coordinates #imgpoints = [] # Read in an image #img = cv2.imread('calibration_test.png') # TODO: Write a function that takes an image, object points, and image points # performs the camera calibration, image distortion correction and # returns the undistorted image # Use cv2.calibrateCamera() and cv2.undistort()