add serialize & deserialzie methods for Calibration and repalce .pkl …

…with .yml

calib_params.yml

@@ -0,0 +1,31 @@
+essential_matrix:
+- [-0.017127011243210456, 0.06099748886356814, -1.8919197115950048]
+- [0.7545609778175904, 2.5650380217065742, 92.29790541975109]
+- [1.37112631169174, -92.31044245530454, 2.5518530453622352]
+fundamental_matrix:
+- [-9.779393985712206e-07, 3.482910516406645e-06, -0.06192919902090507]
+- [4.308486158814036e-05, 0.00014646173256028242, 2.9610298427112283]
+- [0.035760906006510794, -3.0456239992821517, 1.0]
+left_camera_matrix:
+- [570.0976536080545, 0.0, 344.7959814265909]
+- [0.0, 570.0976536080545, 195.33762661869207]
+- [0.0, 0.0, 1.0]
+left_distortion_coefficients:
+- [-0.41768432007095896, 0.21049421555652553, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+  0.0, 0.0, 0.0, 0.0]
+right_camera_matrix:
+- [571.6510518719991, 0.0, 287.9213500551263]
+- [0.0, 571.6510518719991, 215.28170270393227]
+- [0.0, 0.0, 1.0]
+right_distortion_coefficients:
+- [-0.4160474716070939, 0.19746555657692402, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+  0.0, 0.0, 0.0, 0.0]
+rms: 0.4455502018268381
+rotation_matrix:
+- [0.9999400510564399, -0.005872707123126066, -0.009241515259430181]
+- [0.005613526875871187, 0.9995970323017617, -0.02782555173801323]
+- [0.009401202543194587, 0.02777200613129998, 0.9995700741149591]
+translation_vector:
+- [-92.33655419332558]
+- [-1.8895733182055152]
+- [-0.11352047572711924]

calibration.py

@@ -4,7 +4,7 @@
 import cv2
 import csv
 import numpy as np
-import pickle
+import yaml
 
 class StereoPair():
     """
@@ -22,7 +22,7 @@ def __str__(self):
 
 class Calibration():
     """
-    Stereo calibration results
+    Object that hold and manage the stereo calibration parameters
     """
     def __init__(self):
         self.camera_mat = StereoPair()
@@ -31,22 +31,42 @@ def __init__(self):
         self.translation_vec = None
         self.essential_mat = None
         self.fundamental_mat = None
-        self.img_size = None
-        self.undistort_map = StereoPair()
-        self.rectify_map = StereoPair()
         self.rms = None
-        self.Q = None
-        self.r1 = None
-        self.r2 = None
-        self.p1 = None
-        self.p2 = None
 
     def __str__(self):
         return '\n'.join('%s: %s' % item for item in vars(self).items())
 
-    def save(self, file_name):
-        pickle.dump(self, open(file_name, "wb"))
-        print 'calibration result saved into %s' % file_name
+    def write(self, file_name):
+        """ Write the attributes (stereo params) to a yaml file """
+        stereo_params = {
+             "left_camera_matrix": self.camera_mat.left.tolist(),
+             "right_camera_matrix": self.camera_mat.right.tolist(),
+             "left_distortion_coefficients": self.distortion_coeffs.left.tolist(),
+             "right_distortion_coefficients": self.distortion_coeffs.right.tolist(),
+             "rotation_matrix": self.rotation_mat.tolist(),
+             "translation_vector": self.translation_vec.tolist(),
+             "essential_matrix": self.essential_mat.tolist(),
+             "fundamental_matrix": self.fundamental_mat.tolist(),
+             "rms": self.rms
+            }
+        with open(file_name, 'w') as f:
+            yaml.dump(stereo_params, f)
+
+        print 'calibration parameters saved into %s' % file_name
+
+    def read(self, file_name):
+        """ Read the attributes (stereo params) to a yaml file """
+        with open(file_name, 'r') as f:
+            stereo_params = yaml.load(f)
+
+            self.camera_mat = StereoPair(np.array(stereo_params["left_camera_matrix"]), np.array(stereo_params["right_camera_matrix"]))
+            self.distortion_coeffs = StereoPair(np.array(stereo_params["left_distortion_coefficients"]), np.array(stereo_params["right_distortion_coefficients"]))
+            self.rotation_mat = np.array(stereo_params["rotation_matrix"])
+            self.translation_vec = np.array(stereo_params["translation_vector"])
+            self.essential_mat = np.array(stereo_params["essential_matrix"])
+            self.fundamental_mat = np.array(stereo_params["fundamental_matrix"])
+            self.rms = stereo_params["rms"]
+
 
 def stereo_calibrate(obj_points, img_points, img_size):
     """
@@ -60,7 +80,6 @@ def stereo_calibrate(obj_points, img_points, img_size):
         Calibration instance with calibrated results or None if calibration failed
     """
     calib = Calibration()
-    calib.img_size = img_size
 
     criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 1000, 1e-5)
 #    flags = 0
@@ -200,8 +219,6 @@ def show_corners(input_dir, file_format, rows, cols, img_points):
 
         cv2.waitKey(0)
 
-
-
 def draw_epipolarlines(img_left, img_points_left, img_points_right, fundamental_mat, flag):
     lines = cv2.computeCorrespondEpilines(img_points_right.reshape(-1, 2), flag, fundamental_mat)
     lines = lines.reshape(-1, 3)
@@ -224,50 +241,67 @@ def draw_epipolarlines(img_left, img_points_left, img_points_right, fundamental_
     return ep_img
 
 
-def stereo_rectify(calib):
-    print 'perform rectification and undistortion...'
+def get_rectify_map(calib, img_size):
+    """
+    Get the undistortion and rectification map given calibrated parameters and image's size
+
+    Args:
+        calib: the Calibration object
+        img_size: a tuple of image's size (width, height)
 
-    (r1, r2, p1, p2, calib.Q, roi1, roi2) = cv2.stereoRectify(calib.camera_mat.left, 
+    Returns:
+        a tuple of maps: (left_map1, left_map2, right_map1, right_map2)
+    """
+    (r1, r2, p1, p2, Q, roi1, roi2) = cv2.stereoRectify(calib.camera_mat.left, 
                                                               calib.distortion_coeffs.left, 
                                                               calib.camera_mat.right, 
                                                               calib.distortion_coeffs.right,
-                                                              calib.img_size,
+                                                              img_size,
                                                               calib.rotation_mat,
                                                               calib.translation_vec,
                                                               flags=0,
                                                               alpha=0
                                                               )
 
-    calib.r1 = r1
-    calib.r2 = r2
-    calib.p1 = p1
-    calib.p2 = p2
+    (left_map1, left_map2) = cv2.initUndistortRectifyMap(calib.camera_mat.left, calib.distortion_coeffs.left,
+                                                                        r1, p1, img_size, cv2.CV_32FC1)
 
+    (right_map1, right_map2) = cv2.initUndistortRectifyMap(calib.camera_mat.right, calib.distortion_coeffs.right,
+                                                                        r2, p2, img_size, cv2.CV_32FC1)
 
-    (calib.undistort_map.left, calib.rectify_map.left) = cv2.initUndistortRectifyMap(calib.camera_mat.left, calib.distortion_coeffs.left,
-                                                                        r1, p1, calib.img_size, cv2.CV_32FC1)
+    return (left_map1, left_map2, right_map1, right_map2)
 
-    (calib.undistort_map.right, calib.rectify_map.right) = cv2.initUndistortRectifyMap(calib.camera_mat.right, calib.distortion_coeffs.right,
-                                                                        r2, p2, calib.img_size, cv2.CV_32FC1)
+def undistort_rectify(calib, img1, img2):
+    """
+    Undistort and rectify a stereo image pair to make epipolar lines horizontal.
+    
+    Args:
+        calib: the Calibration object
+        img1: left image
+        img2: right image
 
-    print '[SUCCESS] rectification and undistortion done'
+    Returns:
+        a tuple of the undistored and rectified stereo images
+    """
+    if img1.shape != img2.shape:
+        raise ValueError('left and right images size not matched')
+    height, width = img1.shape
+    img_size = (width, height)
 
-def rectify(calib, img_left, img_right):
-    img_left = cv2.remap(img_left, calib.undistort_map.left, calib.rectify_map.left, cv2.INTER_LINEAR)
-    img_right = cv2.remap(img_right, calib.undistort_map.right, calib.rectify_map.right, cv2.INTER_LINEAR)
-    return (img_left, img_right)
+    (left_map1, left_map2, right_map1, right_map2) = get_rectify_map(calib, img_size)
+    img1 = cv2.remap(img1, left_map1, left_map2, cv2.INTER_LINEAR)
+    img2 = cv2.remap(img2, right_map1, right_map2, cv2.INTER_LINEAR)
+    return (img1, img2)
 
 def stereo_remap(input_dir, file_format, rows, cols, calib, img_num):
     for x in range(0, img_num):
         path_left = os.path.join(input_dir, file_format.left.format(idx=x))
         path_right = os.path.join(input_dir, file_format.right.format(idx=x))
 
-        img_left_ = cv2.imread(path_left, 0)
-        img_right_ = cv2.imread(path_right, 0)
-
-        img_left = cv2.remap(img_left_, calib.undistort_map.left, calib.rectify_map.left, cv2.INTER_LINEAR)
-        img_right = cv2.remap(img_right_, calib.undistort_map.right, calib.rectify_map.right, cv2.INTER_LINEAR)
+        img_left = cv2.imread(path_left, 0)
+        img_right = cv2.imread(path_right, 0)
 
+        (img_left, img_right) = undistort_rectify(calib, img_left, img_right)
 
         found_left, img_points_left = cv2.findChessboardCorners(img_left, (rows, cols))
         found_right, img_points_right = cv2.findChessboardCorners(img_right, (rows, cols))
@@ -343,16 +377,9 @@ def draw_horizontal_lines(img):
     img_size = (640, 480)# (weight, height) or (cols, row)
     calib = stereo_calibrate(obj_points_all, StereoPair(img_points_left, img_points_right), img_size)
 
-    show_corners(input_dir, file_format, rows, cols, StereoPair(img_points_left, img_points_right))
-    stereo_rectify(calib)
-    stereo_remap(input_dir, file_format, rows, cols, calib, img_num)
+#    show_corners(input_dir, file_format, rows, cols, StereoPair(img_points_left, img_points_right))
+#    stereo_remap(input_dir, file_format, rows, cols, calib, img_num)
 
     print calib
-    calib.save('calib_result.pkl')
-
-#     image_points = StereoPair(img_points_left, img_points_right)
-#     pickle.dump(image_points, open("image_points.pkl", "wb"))
-#     pickle.dump(obj_points_all, open("object_points.pkl", "wb"))
+    calib.write('calib_params.yml')
 
-#    calib = pickle.load(open('calib_result.pkl', 'rb'))
-#    stereo_remap(input_dir, rows, cols, calib, img_num)

show_rectified.py

@@ -3,31 +3,33 @@
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 import calibration
 from calibration import Calibration, StereoPair
-import pickle
 import cv2
 import numpy as np
 import argparse
 
 if __name__ == "__main__":
     """
-    Undistort and rectify a stereo image pair given a calibration parameter file.
-    Horizontal lines will be drawn on the rectified pair to verify the result.
+    Undistort and rectify a stereo image pair given the calibration parameters.
+    Horizontal lines will be drawn on the undistorted & rectified pair to verify the result.
 
     Example usage:
-    python show_rectified.py calib_result.pkl stereo_images/0L.jpg stereo_images/0R.jpg
+    python show_rectified.py calib_params.yml stereo_images/0L.jpg stereo_images/0R.jpg
     """
-    parser = argparse.ArgumentParser(description='Show undistorted & rectified stereo image pair. The input left img1 and right img2 is the original distorted and unrectified images. This program undistort and rectify img1 and img2 based on the given calibration parameter file.')
-    parser.add_argument('calibration_file', help='the file containing the calibration pararmeters (.pkl)')
+
+    parser = argparse.ArgumentParser(description='Show undistorted & rectified stereo image pair. The input left img1 and right img2 is the original distorted and unrectified images. This program undistort and rectify img1 and img2 using calibration parameters read from a yaml file.')
+    parser.add_argument('calibration_file', help='the yaml file containing the calibration pararmeters (.yml)')
     parser.add_argument('img1', help='left image')
     parser.add_argument('img2', help='right image')
     args = parser.parse_args()
 
-    calib = pickle.load(open(args.calibration_file, 'rb'))
+    calib = Calibration()
+
+    calib.read(args.calibration_file)
 
     img1 = cv2.imread(args.img1, 0)
     img2 = cv2.imread(args.img2, 0)
 
-    (img1, img2) = calibration.rectify(calib, img1, img2)
+    (img1, img2) = calibration.undistort_rectify(calib, img1, img2)
 
     img1 = calibration.draw_horizontal_lines(img1)
     img2 = calibration.draw_horizontal_lines(img2)