2D revision

2dVision.py

@@ -0,0 +1,130 @@
+import cv2 as cv
+import numpy as np
+
+def Planer_Calibration(cam, num_images):
+    # Initialize camera objects
+    cap = cv.VideoCapture(cam)
+
+    # Check if cameras are opened successfully
+    if not (cap.isOpened()):
+        print("Error: Could not open cameras")
+        return
+
+    objpoints = []  # 3D object points
+    imgpoints = []  # 2D image points for camera 1
+    # Prepare object points, assuming a chessboard with 9 by 6 squares of 30mm
+    square_size = 30  # in millimeters
+    row, col = 8, 5
+    objp = np.zeros((row * col, 3), np.float32)
+    objp[:, :2] = np.mgrid[0:row, 0:col].T.reshape(-1, 2) * square_size
+
+    criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+    i = 0
+    while i < num_images:
+        ret, frame = cap.read()
+
+        height, width, _ = frame.shape  # Get image dimensions
+
+        if not (ret):
+            print("Error: Could not read frames")
+            break
+
+        gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
+        # Detect chessboard corners in both images
+        ret, corners = cv.findChessboardCorners(gray, (row, col), None)
+        if ret:
+            # Refine corner positions
+            corners = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
+            objpoints.append(objp)
+            imgpoints.append(corners)
+
+            i += 1
+
+            # Draw and display corners (optional)
+            frame = cv.drawChessboardCorners(frame, (row, col), corners, ret)
+            cv.imshow('Calibrationg', frame)
+            cv.waitKey(500)
+        else:
+            cv.imshow('No Board', frame)
+            cv.waitKey(1)
+
+    return objpoints, imgpoints, width, height
+
+
+def findPositions(cam, duration):
+    cap = cv.VideoCapture(cam)
+    i = 0
+    while i < duration:
+        ret, frame = cap.read()
+        frame = cv.undistort(frame, mtx, dist)
+        point = detect_cube(frame, show_flag = True)
+        cv.circle(frame, (int(point[0]), int(point[1])), 2, (0, 0, 255), -1)
+        cv.imshow('Frame', frame)
+        cv.waitKey(1)
+
+        key = cv.waitKey(5) & 0xFF
+        if key == ord('s'):
+            if i == 0:
+                if offset == 0:
+                    offset = point
+                else:
+                    appended_array = np.vstack((offset, point))
+                    # Calculate the average along the first axis (axis=0)
+                    offset = np.mean(appended_array, axis=0)
+                
+                i += 1
+    return
+
+
+def detect_cube(image, show_flag): 
+    # Convert image to HSV color space
+    hsv = cv.cvtColor(image, cv.COLOR_BGR2HSV)
+
+    # Define lower and upper bounds for red color in HSV
+    # Red range
+    #lower = np.array([0, 100, 100])
+    #upper = np.array([5, 255, 255])
+    # Yellow range
+    #lower = np.array([25, 100, 100])
+    #upper = np.array([35, 255, 255])
+    # Green range
+    lower = np.array([40, 50, 50])
+    upper = np.array([75, 255, 255])
+    # Blue range
+    #lower = np.array([100, 100, 100])
+    #upper = np.array([110, 255, 255])
+
+    # Threshold the HSV image to get only red colors
+    mask = cv.inRange(hsv, lower, upper)
+    # Find non-zero pixel coordinates
+    non_zero_pixels = cv.findNonZero(mask)
+
+    # Check if non-zero pixels are found
+    if non_zero_pixels is not None:
+        # Calculate the average position and extract x and y coordinates of the average position
+        average_position = np.mean(non_zero_pixels, axis=0)
+        avg_x, avg_y = average_position[0]
+    else: avg_x, avg_y = 0, 0
+
+    if show_flag :
+        # Apply the mask to the original image
+        masked_image = cv.bitwise_and(image, image, mask=mask)
+        cv.circle(masked_image, (int(avg_x), int(avg_y)), 2, (0, 0, 255), -1)
+        cv.imshow('Remaining Image', masked_image)
+        cv.waitKey(1)
+
+    if 0: # Calculate the average value for each channel (Hue, Saturation, Value) across non-zero pixels
+        non_zero_indices = np.nonzero(mask)
+        non_zero_pixel_values = hsv[non_zero_indices]
+        avg = np.mean(non_zero_pixel_values, axis=0)
+        print(avg)
+    return (avg_x, avg_y)
+
+cam = 1
+
+objpoints, imgpoints, width, height = Planer_Calibration(cam, num_images = 20)
+ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, (width, height), None, None)
+print(mtx, dist)
+
+findPositions(cam, duration = 10)

vision.py

@@ -2,6 +2,8 @@ import numpy as np
 import cv2 as cv
 import glob
 import os
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
 
 def find_camera(find_flag):
     if find_flag:
@@ -23,7 +25,7 @@ def find_camera(find_flag):
             cam2 = cam_available[1]
     else:
         cam1 = 1
-        cam2 = 2
+        cam2 = 0
     print(f"Cameras number used : {cam1} & {cam2}")
     return cam1, cam2
 def img_capture(camera_num):
@@ -40,7 +42,7 @@ def img_capture(camera_num):
         exit()
     i = 0
     # Capture and save 12 images
-    while i < 6:
+    while i < 15:
         # Capture a frame from the camera
         ret, frame = cap.read()
 
@@ -73,8 +75,8 @@ def single_calibration(camera_num, img_cap):
 
     # Prepare object points, assuming a chessboard with 9 by 6 squares of 30mm
     square_size = 30  # in millimeters
-    row = 9
-    col = 6
+    row = 8
+    col = 5
     objp = np.zeros((row * col, 3), np.float32)
     objp[:, :2] = np.mgrid[0:row, 0:col].T.reshape(-1, 2) * square_size
 
@@ -101,9 +103,10 @@ def single_calibration(camera_num, img_cap):
 
     cv.destroyAllWindows()
     ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, (gray.shape[1], gray.shape[0]), None, None)
+    print(mtx, dist)
     return mtx, dist
 
-def stereo_capture():
+def stereo_capture(mtx1, dist1, mtx2, dist2):
     # Open two video capture objects for each camera
     cap_left = cv.VideoCapture(cam1)  # Adjust the index if needed
     cap_right = cv.VideoCapture(cam2)  # Adjust the index if needed
@@ -118,11 +121,14 @@ def stereo_capture():
     os.makedirs(output_dir, exist_ok=True)
 
     frame_counter = 0
-    while frame_counter < 6:
+    while frame_counter < 15:
         # Read frames from both cameras
         ret_left, frame_left = cap_left.read()
         ret_right, frame_right = cap_right.read()
 
+        frame_left = cv.undistort(frame_left, mtx1, dist1)
+        frame_right = cv.undistort(frame_right, mtx2, dist2)
+
         # Break the loop if either of the cameras fails to read a frame
         if not ret_left or not ret_right:
             print("Error: Couldn't read frames from one or both cameras.")
@@ -150,7 +156,7 @@ def stereo_capture():
     cv.destroyAllWindows()
     return
 def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder, stereo_capture_flag):
-    if stereo_capture_flag: stereo_capture()
+    if stereo_capture_flag: stereo_capture(mtx1, dist1, mtx2, dist2)
     # Read the synched frames
     images_names = glob.glob(frames_folder)
     images_names = sorted(images_names)
@@ -169,8 +175,8 @@ def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder, stereo_capture_f
     #change this if stereo calibration not good.
     criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.0001)
  
-    rows = 6 #number of checkerboard rows.
-    columns = 9 #number of checkerboard columns.
+    rows = 5 #number of checkerboard rows.
+    columns = 8 #number of checkerboard columns.
     world_scaling = 30 #change this to the real world square size. Or not.
  
     #coordinates of squares in the checkerboard world space
@@ -215,11 +221,12 @@ def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder, stereo_capture_f
     stereocalibration_flags = cv.CALIB_FIX_INTRINSIC
     ret, CM1, dist1_bis, CM2, dist2_bis, R, T, E, F = cv.stereoCalibrate(objpoints, imgpoints_left, imgpoints_right, mtx1, dist1, mtx2, dist2, (width, height), criteria = criteria, flags = stereocalibration_flags)
     cv.destroyAllWindows()
+    print(R, T)
     return R, T
 
-def cub_cordinate(mtx1, dist1, mtx2, dist2, R, T):
-    cap1 = cv.VideoCapture(1)
-    cap2 = cv.VideoCapture(2)
+def cub_cordinate(cam1, cam2, mtx1, dist1, mtx2, dist2, R, T):
+    cap1 = cv.VideoCapture(cam1)
+    cap2 = cv.VideoCapture(cam2)
 
     while True:
         # Capture stereo images
@@ -277,7 +284,6 @@ def cub_cordinate(mtx1, dist1, mtx2, dist2, R, T):
     cap1.release()
     cap2.release()
     cv.destroyAllWindows()
-
 def detect_cube(image, show_flag): 
     # Convert image to HSV color space
     hsv = cv.cvtColor(image, cv.COLOR_BGR2HSV)
@@ -290,11 +296,11 @@ def detect_cube(image, show_flag):
     #lower = np.array([25, 100, 100])
     #upper = np.array([35, 255, 255])
     # Green range
-    #lower = np.array([40, 80, 80])
-    #upper = np.array([60, 255, 255])
+    lower = np.array([40, 50, 50])
+    upper = np.array([75, 255, 255])
     # Blue range
-    lower = np.array([100, 100, 100])
-    upper = np.array([110, 255, 255])
+    #lower = np.array([100, 100, 100])
+    #upper = np.array([110, 255, 255])
 
     # Threshold the HSV image to get only red colors
     mask = cv.inRange(hsv, lower, upper)
@@ -321,7 +327,6 @@ def detect_cube(image, show_flag):
         avg = np.mean(non_zero_pixel_values, axis=0)
         print(avg)
     return (avg_x, avg_y)
-
 def triangulate(mtx1, mtx2, R, T):
  
     uvs1 = [[458, 86]]
@@ -337,7 +342,6 @@ def triangulate(mtx1, mtx2, R, T):
     #RT matrix for C2 is the R and T obtained from stereo calibration.
     RT2 = np.concatenate([R, T], axis = -1)
     P2 = mtx2 @ RT2 #projection matrix for C2
-
 def project_point_to_camera2(point_cam1, mtx1, R, T, mtx2):
     # Step 1: Convert point coordinates to world coordinates in camera 1
     point_world = np.dot(np.linalg.inv(mtx1), np.append(point_cam1, 1))
@@ -349,11 +353,52 @@ def project_point_to_camera2(point_cam1, mtx1, R, T, mtx2):
     point_cam2 = point_cam2_homogeneous[:2]  # Extract (x, y) coordinates
     return point_cam2
 
-cam1, cam2 = find_camera(find_flag = False)
-mtx1, dist1 = single_calibration(camera_num = cam1, img_cap = True)
-mtx2, dist2 = single_calibration(camera_num = cam2, img_cap = True)
-R, T = stereo_calibration(mtx1, dist1, mtx2, dist2, 'stereo_images/*', stereo_capture_flag = True)
+def find_3d_position(mtx1, dist1, mtx2, dist2, R, T):
+    cap1 = cv.VideoCapture(cam1)
+    cap2 = cv.VideoCapture(cam2)
 
-cub_cordinate(mtx1, dist1, mtx2, dist2, R, T)
+    while True:
+        # Capture stereo images
+        ret1, frame1 = cap1.read()
+        ret2, frame2 = cap2.read()
+        if not ret1 and not ret2 : break
+        
+        frame1 = cv.undistort(frame1, mtx1, dist1)
+        frame2 = cv.undistort(frame2, mtx2, dist2)
+
+        # Detect red cube in both images
+        point_left = detect_cube(frame1, True)
+        point_right = detect_cube(frame2, True)
+
+        # Convert 2D points to homogeneous coordinates
+        point_left = np.array([point_left[0], point_left[1]])
+        point_right = np.array([point_right[0], point_right[1]])
+
+        # Triangulate 3D point
+        P1 = np.hstack((np.eye(3), np.zeros((3, 1))))
+        P2 = np.hstack((R, T))
+        print(point_left.T, point_right.T)
+        points_4d = cv.triangulatePoints(P1, P2, point_left.T, point_right.T)
+
+        # Convert homogeneous coordinates to Cartesian coordinates
+        points_3d = points_4d[:3] / points_4d[3]
+
+        cv.circle(frame1, (int(point_left[0]), int(point_left[1])), 2, (0, 0, 255), -1)
+        cv.circle(frame2, (int(point_right[0]), int(point_right[1])), 2, (0, 0, 255), -1)
+        print(points_3d)
+
+        stereo_frame = cv.hconcat([frame1, frame2])
+        cv.imshow('Stereo Frames', stereo_frame)
+        cv.waitKey(500)
+
+    return
+
+cam1, cam2 = find_camera(find_flag = False)
+mtx1, dist1 = single_calibration(camera_num = cam1, img_cap = False)
+mtx2, dist2 = single_calibration(camera_num = cam2, img_cap = False)
+R, T = stereo_calibration(mtx1, dist1, mtx2, dist2, 'stereo_images/*', stereo_capture_flag = False)
+
+#cub_cordinate(cam1, cam2, mtx1, dist1, mtx2, dist2, R, T)
+find_3d_position(mtx1, dist1, mtx2, dist2, R, T)
 
 print("$$$ Code Done $$$")