Add DLT_calib and epipolar line

Author: cjh1995-ros

scripts/DLT_calib.py

@@ -0,0 +1,205 @@
+from copy import deepcopy
+import cv2
+import numpy as np
+from math import sqrt
+
+class CustomCalibrator():
+    def __init__(self, points_3d):
+        self.points_2d = []
+        self.img = None
+        self.clean_img = None
+        self.video = '/dev/video2'
+        self.points_3d = points_3d    
+
+    def take_points(self, event, x, y, flags, param):
+        if event == cv2.EVENT_LBUTTONDOWN:
+            self.points_2d.append((x, y))
+            self.img = cv2.circle(self.img, (x, y), 3, (0, 0, 255), 3)
+
+    def find_homography(self):
+        """Find homography matrix"""
+        # check point's length
+        if len(self.points_2d) != 6:
+            raise Exception("Not enough points!")
+        
+        self.points_2d = np.array(self.points_2d, dtype=np.float32)
+        self.points_3d = np.array(self.points_3d, dtype=np.float32)
+        
+        # Make A, B matrix
+        A = None
+        B = None
+        for i in range(len(self.points_2d)):
+            rx, ry, rz = self.points_3d[i]
+            u, v = self.points_2d[i]
+            A_matrix = np.array([[rx, ry, rz, 1, 0, 0, 0, 0, -u * rx, -u * ry, -u * rz], 
+                                 [0, 0, 0, 0, rx, ry, rz, 1, -v * rx, -v * ry, -v * rz]], 
+                                dtype=np.float32)
+            B_matrix = np.array([[u, v]], dtype=np.float32)
+            if i == 0:
+                A = A_matrix.copy()
+                B = B_matrix.copy()
+            else:
+                A = np.vstack((A, A_matrix))
+                B = np.hstack((B, B_matrix))
+        
+        # Before pseudo inverse, Check det(A.T @ A) 
+        det = np.linalg.det((A.T @ A))
+        if det == 0:
+            # raise Exception("(A.T @ A) is Singular Matrix!!!")
+            print(A.T @ A)
+
+        # Do pseudo inverse
+        V = np.linalg.inv(A.T @ A) @ A.T @ B.T
+        V = V.T
+        return V
+
+    def calibrate_homography(self, V):
+        # decompose matrix items
+        h11, h12, h13, h14, h21, h22, h23, h24, h31, h32, h33 = V[0].tolist() # squeeze를 쓴다면 더 세련될 것.
+        print(h11, h12, h13, h14, h21, h22, h23, h24, h31, h32, h33)
+        # 1) calculate tz
+        tz = 1.0 / sqrt(h31 ** 2 + h32 ** 2 + h33 ** 2)
+        print(tz)
+        # 2) calculate R3
+        R3 = tz * np.array([[h31, h32, h33]])
+        # 3) calculate u0
+        u0 = tz * np.array([[h11, h12, h13]]) @ R3.T 
+        # 4) calculate v0
+        v0 = tz * np.array([[h21, h22, h23]]) @ R3.T
+        # 5) calculate fx
+        fx = tz * np.array([[h11, h12, h13]]) - u0 * R3
+        fx = np.linalg.norm(fx)
+        # 6) calculate fy
+        fy = tz * np.array([[h21, h22, h23]]) - v0 * R3
+        fy = np.linalg.norm(fy)
+        # 7) ca;culate R1
+        R1 = tz / fx * np.array([[h11, h12, h13]]) - u0 / fx * R3
+        # 8) calculate R2
+        R2 = tz / fy * np.array([[h21, h22, h23]]) - v0 / fy * R3
+        # 9) calculate tx
+        tx = tz / fx * (h14 - u0)
+        # 10) calculate ty
+        ty = tz / fy * (h24 - v0)
+        
+        # Return Homography, Intrinsic and Extrinsic matrix
+        R = np.vstack((np.vstack((R1, R2)), R3))
+        t = np.array([[tx, ty, tz]], dtype=object)
+        H1 = np.array([[h11, h12, h13, h14]])
+        H2 = np.array([[h21, h22, h23, h24]])
+        H3 = np.array([[h31, h32, h33, 1]])
+        homography_matrix = np.vstack((np.vstack((H1, H2)), H3))
+        extrinsic_matrix = np.hstack((R, t.T))
+        intrinsic_matrix = np.array([[fx, 0, u0], [0, fy, v0], [0, 0, 1]], dtype=np.float32)
+        # test_R = R.T
+        # test_T = -R.T @ t
+        return homography_matrix, intrinsic_matrix, extrinsic_matrix # , test_R, test_T
+
+    def reprojection(self, homography_matrix):
+        # Get Reprojected points
+        reprojected_points = []
+        for point in self.points_3d:
+            point = np.append(point, 1)
+            reprojected_point = homography_matrix @ point.T
+            reprojected_point[0] /= reprojected_point[2]
+            reprojected_point[1] /= reprojected_point[2]
+            reprojected_points.append(reprojected_point.T[:2])
+        
+        
+        # Show 2d proj positions
+        show_points = np.array(reprojected_points, dtype=np.int32)
+        for reproj in show_points:
+            self.img = cv2.circle(self.img, tuple(reproj), 3, (0, 255, 0), 3)
+        cv2.imwrite(f"saved_proj_img.png", self.img)
+
+        # Calc Error
+        point_2d = self.points_2d.astype(np.float32)
+        err = np.linalg.norm(point_2d - reprojected_points)
+        print("points_2d = ", point_2d)
+        print("reproj 2d = ", reprojected_points)
+        return err
+
+    def pipeline(self, **mode):
+        cap = cv2.VideoCapture(self.video)
+        cv2.namedWindow('img')
+        cv2.setMouseCallback('img', self.take_points)
+        n_save = 0
+        FIX_IMAGE = False
+        print(mode)
+        # get points_2d
+        if mode['mode'] == 'using_video':
+            GET_CLEAN_IMG = True
+            while True:
+                if not FIX_IMAGE:
+                    ret, self.img = cap.read()
+                    if ret != None and GET_CLEAN_IMG:
+                        self.clean_img = deepcopy(self.img)
+                        GET_CLEAN_IMG = False
+
+                # Add info to IMG
+                h, w, _ = self.img.shape
+
+                # View Image
+                cv2.imshow("img", self.img)
+                
+                # key event
+                key = cv2.waitKey(1)
+                if key == ord('q'):
+                    break
+                elif key == ord('s'):
+                    FIX_IMAGE = True
+                elif key == ord('e'):
+                    n_save += 1
+                    cv2.imwrite(f"saved_img_{n_save}.png", self.img)
+            cap.release()
+
+        elif mode['mode'] == 'image_cal':
+            self.img = cv2.imread(mode['img'])
+            self.clean_img = deepcopy(self.img)
+            while True:
+                h, w, _ = self.img.shape
+
+                # View Image
+                cv2.imshow("img", self.img)
+                
+                # key event
+                key = cv2.waitKey(1)
+                if key == ord('q'):
+                    break
+                elif key == ord('s'):
+                    FIX_IMAGE = True
+                elif key == ord('e'):
+                    n_save += 1
+                    cv2.imwrite(f"saved_img_{n_save}.png", self.img)
+            cap.release()
+
+        elif mode['mode'] == 'done_image':
+            self.points_2d = [(402, 324), (434, 300), (451,113), (414, 124), (270,172), (208, 315)]
+
+        # find homography items
+        V = self.find_homography()
+        
+        # From homography, find intrinsic and extrinsic
+        homography, intrinsic, extrinsic= self.calibrate_homography(V) # , test_R, test_T 
+        print("homography matrix\n", homography)
+        print("intrinsic matrix\n", intrinsic)
+        print("extrinsic matrix, and shape\n", extrinsic.shape, extrinsic)
+        
+        # reproject points to image
+        err = self.reprojection(homography)
+        
+        # Show projected image
+        if mode['mode'] != 'done_image':
+            self.img = cv2.putText(self.img, f"err = {err:.3f}", (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
+            cv2.imshow("img", self.img)
+            cv2.waitKey(0)
+
+        return homography, intrinsic, extrinsic, self.clean_img
+
+def main():
+    # cm 단위로 했음. m단위로 바꾸기.
+    points_3d = [(3,0,0), (5,0,0), (5,0,6), (3,0,6), (0,2,5), (0,5,0)]
+    cc = CustomCalibrator(points_3d=points_3d)
+    cc.pipeline()
+
+if __name__ == "__main__":
+    main()

scripts/DLT_calib_done.png

@@ -0,0 +1,152 @@
+from logging import raiseExceptions
+import cv2
+import numpy as np
+from copy import deepcopy
+from math import sqrt
+from Homework_02 import CustomCalibrator
+
+def screw(vec1x3):
+    return np.array([[0, -vec1x3[2], vec1x3[1]], [vec1x3[2], 0, -vec1x3[0]], [-vec1x3[1], vec1x3[0], 0]])
+
+def inv_intrinsic(intri):
+    fx, cx, fy, cy = intri[0, 0], intri[0, 2], intri[1, 1], intri[1, 2]
+    return np.array([[1./fx, 0., -cx/fx], [0, 1./fy, -cy/fy], [0., 0., 1.]])
+
+class CustomEpipolarFinder():
+    def __init__(self, s1_datas, s2_datas):
+        """
+        Get scene_1, scene_2 intrinsic, extrinsic datas, original img. Make Camera Matrix Inverse for each scene.
+        """
+        self.points_2d = []
+        if len(s1_datas) == 3:
+            s1_Intrinsic = s1_datas[0]
+            s1_Extrinsic = s1_datas[1]
+            self.s1_img = s1_datas[2]
+            self.clean_img_1 = deepcopy(self.s1_img)
+        else:
+            raise Exception("Short datas. May be no images?")
+
+        if len(s2_datas) == 3:
+            s2_Intrinsic = s2_datas[0]
+            s2_Extrinsic = s2_datas[1]
+            self.s2_img = s2_datas[2]
+            self.clean_img_2 = deepcopy(self.s2_img)
+        else:
+            raise Exception("Short datas. May be no images?")
+
+        # Check Intrinsic Similarity and Get Inv Intrinsic mat
+        self.s1_inv_intrinsic = inv_intrinsic(s1_Intrinsic)
+        self.s2_inv_intrinsic = inv_intrinsic(s2_Intrinsic)
+        
+        # Get rotation and translation inform
+        c1_M_r = np.vstack((s1_Extrinsic, np.array([[0.0, 0.0, 0.0, 1.0]]))).astype(float)
+        c2_M_r = np.vstack((s2_Extrinsic, np.array([[0.0, 0.0, 0.0, 1.0]]))).astype(float)
+        c1_M_c2 = c1_M_r @ np.linalg.inv(c2_M_r)
+        self.rot_2to1 = c1_M_c2[:3, :3]
+        trans_2to1 = c1_M_c2[:3, 3]
+        self.trans_2to1 = screw(trans_2to1) # 3x3 screw sym matrix
+
+    def take_points(self, event, x, y, flags, param):
+        """
+        L = Take interesting point on img!
+        R = Delete point before chosen.
+        """
+        if event == cv2.EVENT_LBUTTONDOWN:
+            print("left")
+            self.points_2d.append((x, y))
+            cv2.circle(self.s1_img, (x, y), 3, (0, 0, 255), 3)
+
+        elif event == cv2.EVENT_RBUTTONDOWN:
+            print("right")
+            self.point_2d[0], self.point_2d[1] = 0, 0
+            self.points_2d.pop()
+            cv2.circle(self.s1_img, (x, y), 3, (0, 0, 255), 3)
+
+    def get_epipolarline(self):
+        """
+        Find Epipolar line.
+        """
+        # Make point 2d shape (u, v, 1)
+        self.points_2d = np.array([(u, v, 1) for (u, v) in self.points_2d])
+
+        # Calculate Fundamental Matrix
+        Fundamental = self.s1_inv_intrinsic.T @ self.trans_2to1.T @ self.rot_2to1 @ self.s2_inv_intrinsic
+        
+        # Calculate coefficient of point_2d
+        coeffs = np.array([p2d @ Fundamental for p2d in self.points_2d])
+
+        # Draw Line on Img
+        x_range = np.linspace(0, self.s2_img.shape[1] - 1, 50)
+        ys_range = []
+        for coeff in coeffs:
+            y_range = -coeff[0] / coeff[1] * x_range - coeff[2] / coeff[1]
+            ys_range.append(y_range)
+
+        # 1) Filter Outlier
+        good_results = []
+        for y_range in ys_range:
+            good_result = []
+            for i, y in enumerate(y_range):
+                if y < 0 or y > self.s1_img.shape[0]:
+                    continue
+                else:
+                    good_result.append((x_range[i], y))
+            good_results.append(good_result)
+
+        # 2) Use left and right value of results and draw it.
+        for good_result in good_results:
+            # get data from it
+            left, right = good_result[0], good_result[-1]
+            # Make it tuple
+            left, right = (int(left[0]), int(left[1])), (int(right[0]), int(right[1]))
+            cv2.line(self.s2_img, left, right, (0, 255, 0), 3)
+
+        return coeffs
+
+    def pipeline(self):
+        """
+        Get images and check point which I want to find epipolar line.
+        """
+        # From s1_img, pick interesting 2d point.
+        cv2.namedWindow('img')
+        cv2.setMouseCallback('img', self.take_points)
+        while True:
+            cv2.imshow("img", self.s1_img)
+            if cv2.waitKey(0) == ord('q'):
+                break
+        # Find epipolar line on s2_img
+        coeffs = self.get_epipolarline()
+        # Check Correspondence
+        print(f"coeffs of epipolar line = \n{coeffs}")
+        # Show Img
+        img = np.hstack((self.s1_img, self.s2_img))
+        cv2.imshow("end", img)
+        cv2.waitKey(0)
+        # Save Img
+        cv2.imwrite("good_epipolar_img.png", img)
+        
+        return
+
+
+def main():
+    points_3d = [(1,0,0), (3,0,1.9), (5,0,0), (0,1, 1.9), (0, 3, 0), (0, 5, 1.9)]
+    mode1 = {'mode': 'image_cal', 'img': 'img1.png'}
+    mode2 = {'mode': 'image_cal', 'img': 'img2.png'}
+
+    cc1 = CustomCalibrator(points_3d=points_3d)
+    cc2 = CustomCalibrator(points_3d=points_3d)
+
+
+    _, s1_Intrinsic, s1_Extrinsic, s1_img = cc1.pipeline(**mode1)
+    s1_datas = (s1_Intrinsic, s1_Extrinsic, s1_img)
+    if s1_datas != None: print("s1 success!")
+    
+    _, s2_Intrinsic, s2_Extrinsic, s2_img = cc2.pipeline(**mode2)
+    s2_datas = (s2_Intrinsic, s2_Extrinsic, s2_img)
+    if s2_datas != None: print("s2 success!")
+
+    cef = CustomEpipolarFinder(s1_datas=s1_datas, s2_datas=s2_datas)
+    cef.pipeline()
+
+if __name__ == "__main__":
+    main()