Dump of code

Condence.py

@@ -0,0 +1,263 @@
+import numpy as np
+import cv2 as cv
+import os
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+def capture_images(cam1, cam2, num_images):
+    # Initialize camera objects
+    cap1 = cv.VideoCapture(cam1)
+    cap2 = cv.VideoCapture(cam2)
+
+    # Check if cameras are opened successfully
+    if not (cap1.isOpened() and cap2.isOpened()):
+        print("Error: Could not open cameras")
+        return [], [], [], 0, 0
+
+    objpoints = []  # 3D object points
+    imgpoints1 = []  # 2D image points for camera 1
+    imgpoints2 = []  # 2D image points for camera 2
+
+    # Prepare object points, assuming a chessboard with 9 by 6 squares of 30mm
+    square_size = 30  # in millimeters
+    row, col = 9, 6
+    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:
+        ret1, frame1 = cap1.read()
+        ret2, frame2 = cap2.read()
+
+        height, width, _ = frame1.shape  # Get image dimensions
+
+        if not (ret1 and ret2):
+            print("Error: Could not read frames")
+            break
+
+        gray1 = cv.cvtColor(frame1, cv.COLOR_BGR2GRAY)
+        gray2 = cv.cvtColor(frame2, cv.COLOR_BGR2GRAY)
+        # Detect chessboard corners in both images
+        ret1, corners1 = cv.findChessboardCorners(gray1, (row, col), None)
+        ret2, corners2 = cv.findChessboardCorners(gray2, (row, col), None)
+        if ret1 and ret2:
+            # Refine corner positions
+            corners1 = cv.cornerSubPix(gray1, corners1, (11, 11), (-1, -1), criteria)
+            corners2 = cv.cornerSubPix(gray2, corners2, (11, 11), (-1, -1), criteria)
+
+            objpoints.append(objp)
+            imgpoints1.append(corners1)
+            imgpoints2.append(corners2)
+
+            i += 1
+
+            # Draw and display corners (optional)
+            frame1 = cv.drawChessboardCorners(frame1, (row, col), corners1, ret1)
+            frame2 = cv.drawChessboardCorners(frame2, (row, col), corners2, ret2)
+            stereo_chess = cv.hconcat([frame1, frame2])
+            cv.imshow('stereo', stereo_chess)
+            cv.waitKey(500)
+
+    # Release video captures
+    cap1.release()
+    cap2.release()
+    cv.destroyAllWindows()
+
+    return objpoints, imgpoints1, imgpoints2, width, height
+
+
+cam1 = 1
+cam2 = 2
+# Capture images from both cameras
+objpoints, imgpoints1, imgpoints2, w, h = capture_images(cam1, cam2, num_images=40)
+print('$$$ capture done $$$')
+# Perform stereo calibration
+ret, mtx1, dist1, mtx2, dist2, R, T, E, F = cv.stereoCalibrate(objpoints, imgpoints1, imgpoints2, None, None, None, None, (w, h))
+print('$$$ calibration done $$$')
+print(mtx1, dist1, mtx2, dist2, R, T)
+
+def find_3d_position(mtx1, dist1, mtx2, dist2, R, T):
+    cap1 = cv.VideoCapture(1)
+    cap2 = cv.VideoCapture(2)
+
+    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, False)
+        point_right = detect_cube(frame2, False)
+
+        # 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)
+        plot_3d_points(points_3d)
+
+        stereo_frame = cv.hconcat([frame1, frame2])
+        cv.imshow('Stereo Frames', stereo_frame)
+        cv.waitKey(500)
+
+    return
+
+def cub_cordinate(mtx1, dist1, mtx2, dist2, R, T):
+    cap1 = cv.VideoCapture(1)
+    cap2 = cv.VideoCapture(2)
+
+    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
+        point1 = detect_cube(frame1, False)
+        point2 = detect_cube(frame2, False)
+        """
+        point1 = np.array(point1)
+        point2 = np.array(point2)
+        
+        #RT matrix for C1 is identity.
+        RT1 = np.concatenate([np.eye(3), [[0],[0],[0]]], axis = -1)
+        P1 = mtx1 @ RT1 #projection matrix for C1
+        
+        #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
+
+        # Call the triangulatePoints function
+        points3d_homogeneous = cv.triangulatePoints(P1, P2, point1, point2)
+        # Convert homogeneous coordinates to Euclidean coordinates
+        points3d_homogeneous /= points3d_homogeneous[3]
+        # Extract the 3D points from the homogeneous coordinates
+        points3d = points3d_homogeneous[:3]
+
+        print(points3d_homogeneous)"""
+        
+        #cal_point2 = project_point_to_camera2(point1, mtx1, R, T, mtx2)
+        transform = np.vstack((np.hstack((R, T)), [0, 0, 0, 1]))
+        point_homogeneous = np.array([point1[0], point1[1], 1, 1])
+        cal_point1_homogeneous = np.dot(transform, point_homogeneous)
+        cal_point1 = cal_point1_homogeneous[:2] / cal_point1_homogeneous[3]
+        cal_point1_x, cal_point1_y = cal_point1
+
+        cv.circle(frame1, (int(point1[0]), int(point1[1])), 2, (0, 0, 255), -1)
+        cv.circle(frame2, (int(point2[0]), int(point2[1])), 2, (0, 0, 255), -1)
+        cv.circle(frame2, (int(cal_point1_x), int(cal_point1_y)), 2, (255, 0, 0), -1)
+        cv.circle(frame1, (int(cal_point1_x), int(cal_point1_y)), 2, (255, 0, 0), -1)
+        print(point2, cal_point1)
+
+        stereo_frame = cv.hconcat([frame1, frame2])
+        cv.imshow('Stereo Frames', stereo_frame)
+        cv.waitKey(1)
+
+        # Break the loop on 'q' key press
+        if cv.waitKey(1) & 0xFF == ord('q'): break
+
+    # Release video capture and close windows
+    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)
+
+    # 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, 80, 80])
+    #upper = np.array([60, 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)
+
+def plot_3d_points(points_3d):
+    global ax  # Use the ax defined outside the function
+
+    # Clear the previous plot
+    ax.clear()
+
+    # Set labels
+    ax.set_xlabel('X')
+    ax.set_ylabel('Y')
+    ax.set_zlabel('Z')
+
+    # Scatter plot the new points
+    ax.scatter(points_3d[0], points_3d[1], points_3d[2])
+
+    # Set axis limits to ensure origin is visible
+    ax.set_xlim([-100, 1000])  # Adjust the limits according to your data range
+    ax.set_ylim([-100, 1000])  # Adjust the limits according to your data range
+    ax.set_zlim([-100, 1000])  # Adjust the limits according to your data range
+
+    # Draw and pause to update the plot
+    plt.draw()
+    plt.pause(0.01)
+
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('Z')
+
+# Initialize data
+x_data = []
+y_data = []
+z_data = []
+
+#cub_cordinate(mtx1, dist1, mtx2, dist2, R, T)
+find_3d_position(mtx1, dist1, mtx2, dist2, R, T)

bin.py

@@ -1,6 +1,304 @@
-#!/usr/bin/python3
+import numpy as np
+import cv2 as cv
+import glob
+import os
 
-from pypot.creatures import PoppyErgoJr
+# Here is a litle help:
+# https://temugeb.github.io/opencv/python/2021/02/02/stereo-camera-calibration-and-triangulation.html
 
-jr = PoppyErgoJr()
+def display_depth_map(mtx1, dist1, mtx2, dist2, R, T):
-jr.m3.goal_position = 30
+    # Load stereo rectification parameters
+    R1, R2, P1, P2, Q, _, _ = cv.stereoRectify(mtx1, dist1, mtx2, dist2, (640, 480), R, T)
+    map1x, map1y = cv.initUndistortRectifyMap(mtx1, dist1, R1, P1, (640, 480), cv.CV_32FC1)
+    map2x, map2y = cv.initUndistortRectifyMap(mtx2, dist2, R2, P2, (640, 480), cv.CV_32FC1)
+
+    # Initialize the stereo block matching algorithm
+    stereo = cv.StereoBM_create(numDisparities=16, blockSize=5)
+
+    # Initialize the stereo video capture
+    cap1 = cv.VideoCapture(1)
+    cap2 = cv.VideoCapture(2)
+
+    while True:
+        # Capture stereo images
+        ret1, frame1 = cap1.read()
+        ret2, frame2 = cap2.read()
+
+        if not (ret1 and ret2):
+            break
+
+        # Rectify stereo images
+        rectified_frame1 = cv.remap(frame1, map1x, map1y, cv.INTER_LINEAR)
+        rectified_frame2 = cv.remap(frame2, map2x, map2y, cv.INTER_LINEAR)
+
+        # Convert stereo images to grayscale
+        gray1 = cv.cvtColor(rectified_frame1, cv.COLOR_BGR2GRAY)
+        gray2 = cv.cvtColor(rectified_frame2, cv.COLOR_BGR2GRAY)
+
+        # Compute the disparity map
+        disparity = stereo.compute(gray1, gray2)
+
+        # Convert disparity map to depth map
+        depth_map = cv.convertScaleAbs(disparity, alpha=1.0/8)
+
+        # Display depth map
+        cv.imshow('Depth Map', depth_map)
+        cv.waitKey(1000)
+        # Break the loop when 'q' is pressed
+        if cv.waitKey(1) & 0xFF == ord('q'):
+            break
+
+    # Release video capture and close windows
+    cap1.release()
+    cap2.release()
+    cv.destroyAllWindows()
+def stereo_line(mtx1, dist1, mtx2, dist2):
+    cap1 = cv.VideoCapture(1)
+    cap2 = cv.VideoCapture(2)
+    # Create SIFT detector
+    sift = cv.SIFT_create()
+
+    while True:
+        # Capture stereo images
+        ret1, frame1 = cap1.read()
+        ret2, frame2 = cap2.read()
+        frame1 = cv.undistort(frame1, mtx1, dist1)
+        frame2 = cv.undistort(frame2, mtx2, dist2)
+
+        # Convert stereo images to grayscale
+        frame1 = cv.cvtColor(frame1, cv.COLOR_BGR2GRAY)
+        frame2 = cv.cvtColor(frame2, cv.COLOR_BGR2GRAY)
+
+        # Find keypoints and descriptors
+        kp1, des1 = sift.detectAndCompute(frame1, None)
+        kp2, des2 = sift.detectAndCompute(frame2, None)
+
+        # FLANN parameters
+        FLANN_INDEX_KDTREE = 1
+        index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
+        search_params = dict(checks=50)
+
+        # Create FLANN matcher
+        flann = cv.FlannBasedMatcher(index_params, search_params)
+
+        # Match descriptors
+        matches = flann.knnMatch(des1, des2, k=2)
+
+        # Apply ratio test as per Lowe's paper
+        pts1 = []
+        pts2 = []
+        for m, n in matches:
+            if m.distance < 0.8 * n.distance:
+                pts2.append(kp2[m.trainIdx].pt)
+                pts1.append(kp1[m.queryIdx].pt)
+
+        # Convert points to numpy arrays
+        pts1 = np.int32(pts1)
+        pts2 = np.int32(pts2)
+
+        # Find Fundamental Matrix using RANSAC
+        F, mask = cv.findFundamentalMat(pts1, pts2, cv.FM_RANSAC)
+
+        # Select only inlier points
+        pts1 = pts1[mask.ravel() == 1]
+        pts2 = pts2[mask.ravel() == 1]
+
+        # Find epilines corresponding to points in the right image (second image)
+        lines1 = cv.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F)
+        lines1 = lines1.reshape(-1, 3)
+
+        # Find epilines corresponding to points in the left image (first image)
+        lines2 = cv.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F)
+        lines2 = lines2.reshape(-1, 3)
+
+        # Draw epilines on both images
+        img5, img6 = drawlines(frame1, frame2, lines1, pts1, pts2)
+        img3, img4 = drawlines(frame2, frame1, lines2, pts2, pts1)
+
+        # Display images with epilines
+        cv.imshow('Epilines Left', img5)
+        cv.imshow('Epilines Right', img3)
+        cv.waitKey(1000)
+
+        # Break the loop on 'q' key press
+        if cv.waitKey(1) & 0xFF == ord('q'):
+            break
+
+    # Release video capture and close windows
+    cap1.release()
+    cap2.release()
+    cv.destroyAllWindows()
+def drawlines(img1, img2, lines, pts1, pts2):
+    ''' Draw epilines on the images '''
+    r, c = img1.shape[:2]
+    img1_color = cv.cvtColor(img1, cv.COLOR_GRAY2BGR)
+    img2_color = cv.cvtColor(img2, cv.COLOR_GRAY2BGR)
+    for r, pt1, pt2 in zip(lines, pts1, pts2):
+        color = tuple(np.random.randint(0, 255, 3).tolist())
+        x0, y0 = map(int, [0, -r[2] / r[1]])
+        x1, y1 = map(int, [c, -(r[2] + r[0] * c) / r[1]])
+        img1_color = cv.line(img1_color, (x0, y0), (x1, y1), color, 1)
+        img1_color = cv.circle(img1_color, tuple(pt1), 5, color, -1)
+        img2_color = cv.circle(img2_color, tuple(pt2), 5, color, -1)
+    return img1_color, img2_color
+def finding_gripper(mtx1, dist1):
+    def draw(img, corners, imgpts):
+        corner = tuple(corners[0].ravel().astype(int))
+        imgpt_0 = tuple(imgpts[0].ravel().astype(int))
+        imgpt_1 = tuple(imgpts[1].ravel().astype(int))
+        imgpt_2 = tuple(imgpts[2].ravel().astype(int))
+        img = cv.line(img, corner, imgpt_0, (255,0,0), 5)
+        img = cv.line(img, corner, imgpt_1, (0,255,0), 5)
+        img = cv.line(img, corner, imgpt_2, (0,0,255), 5)
+        return img
+    
+    criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+    row = 3
+    col = 3
+    objp = np.zeros((row*col,3), np.float32)
+    objp[:,:2] = np.mgrid[0:col,0:row].T.reshape(-1,2)
+    
+    axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
+
+    cap = cv.VideoCapture(1)
+    while True:
+        ret, img = cap.read()  # Read a frame from the camera
+        if not ret:
+            print("Failed to capture frame")
+            break
+
+        gray = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
+        ret, corners = cv.findChessboardCorners(gray, (col,row),None)
+        
+        if ret == True:
+            corners2 = cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
+    
+            # Find the rotation and translation vectors.
+            ret,rvecs, tvecs = cv.solvePnP(objp, corners2, mtx1, dist1)
+    
+            # project 3D points to image plane
+            imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, mtx1, dist1)
+            img = draw(img,corners2,imgpts)
+            cv.imshow('img',img)
+            if cv.waitKey(1) & 0xFF == ord('q'):
+                break
+    
+    cv.destroyAllWindows()
+def threeD_cube(mtx1, dist1, mtx2, dist2, R, T):
+    # Define cube vertices in 3D space
+    cube_vertices = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0], [0, 0, -3], [0, 3, -3], [3, 3, -3], [3, 0, -3]])
+    sq_s = 30
+    square_vertices = np.float32([[-sq_s/2, sq_s/2, 0], [sq_s/2, sq_s/2, 0], [sq_s/2, -sq_s/2, 0], [-sq_s/2, -sq_s/2, 0]])
+    row = 8
+    col = 5
+    objp = np.zeros((row*col,3), np.float32)
+    objp[:,:2] = np.mgrid[0:col,0:row].T.reshape(-1,2)
+    # Initialize video capture for both cameras
+    cap1 = cv.VideoCapture(1)  # Assuming camera1 is connected at index 0
+    cap2 = cv.VideoCapture(2)  # Assuming camera2 is connected at index 1
+
+    while True:
+        # Capture frame from camera1
+        ret1, frame1 = cap1.read()
+        if not ret1:
+            break
+
+        # Undistort frame from camera1
+        frame1_undistorted = cv.undistort(frame1, mtx1, dist1)
+        
+        # Detect corners of the chessboard
+        ret_corners, corners = cv.findChessboardCorners(frame1_undistorted, (5, 8), None)
+        cv.drawChessboardCorners(frame1_undistorted, (5,8), corners, ret_corners)
+        if ret_corners:
+            # Estimate pose of the chessboard
+            ret_pose, rvecs, tvecs = cv.solvePnP(objp, corners, mtx1, dist1)
+
+            # Project cube vertices onto image plane of camera1
+            cube_vertices_img1, _ = cv.projectPoints(cube_vertices, rvecs, tvecs, mtx1, dist1)
+
+            # Draw cube on frame from camera1
+            for i in range(4):
+                frame1_undistorted = cv.line(frame1_undistorted, tuple(cube_vertices_img1[i].ravel().astype(int)), tuple(cube_vertices_img1[(i+1) % 4].ravel().astype(int)), (0, 255, 0), 3)
+                frame1_undistorted = cv.line(frame1_undistorted, tuple(cube_vertices_img1[i].ravel().astype(int)), tuple(cube_vertices_img1[i+4].ravel().astype(int)), (0, 255, 0), 3)
+                frame1_undistorted = cv.line(frame1_undistorted, tuple(cube_vertices_img1[i+4].ravel().astype(int)), tuple(cube_vertices_img1[(i+1) % 4 + 4].ravel().astype(int)), (0, 255, 0), 3)
+
+            # Display frame from camera1 with cube
+            cv.imshow('Camera 1', frame1_undistorted)
+            """
+            # Capture frame from camera2
+            ret2, frame2 = cap2.read()
+            if not ret2:
+                break
+
+            # Apply transformation to cube vertices for camera2
+            transformed_vertices = np.dot(cube_vertices, R.T) + T
+
+            # Project transformed cube vertices onto image plane of camera2
+            cube_vertices_img2, _ = cv.projectPoints(transformed_vertices, rvecs, tvecs, mtx1, dist1)
+
+            # Draw transformed cube on frame from camera2
+            for i in range(4):
+                frame2 = cv.line(frame2, tuple(cube_vertices_img2[i].ravel().astype(int)), tuple(cube_vertices_img2[(i+1) % 4].ravel().astype(int)), (0, 255, 0), 3)
+                frame2 = cv.line(frame2, tuple(cube_vertices_img2[i].ravel().astype(int)), tuple(cube_vertices_img2[i+4].ravel().astype(int)), (0, 255, 0), 3)
+                frame2 = cv.line(frame2, tuple(cube_vertices_img2[i+4].ravel().astype(int)), tuple(cube_vertices_img2[(i+1) % 4 + 4].ravel().astype(int)), (0, 255, 0), 3)
+
+            # Display frame from camera2 with transformed cube
+            cv.imshow('Camera 2', frame2)"""
+
+            # Exit on 'q' key press
+            if cv.waitKey(1) & 0xFF == ord('q'):
+                break
+
+    # Release video capture and close windows
+    cap1.release()
+    cap2.release()
+    cv.destroyAllWindows()
+def three_axis(mtx1, dist1, mtx2, dist2, R, T):
+    def draw(img, corners, imgpts):
+        corner = tuple(corners[0].ravel().astype(int))
+        imgpt_0 = tuple(imgpts[0].ravel().astype(int))
+        imgpt_1 = tuple(imgpts[1].ravel().astype(int))
+        imgpt_2 = tuple(imgpts[2].ravel().astype(int))
+        img = cv.line(img, corner, imgpt_0, (255,0,0), 5)
+        img = cv.line(img, corner, imgpt_1, (0,255,0), 5)
+        img = cv.line(img, corner, imgpt_2, (0,0,255), 5)
+        return img
+    
+    criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+    row = 8
+    col = 5
+    objp = np.zeros((row*col,3), np.float32)
+    objp[:,:2] = np.mgrid[0:col,0:row].T.reshape(-1,2)
+    
+    axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
+
+    cap = cv.VideoCapture(1)
+    while True:
+        ret, img = cap.read()  # Read a frame from the camera
+        if not ret:
+            print("Failed to capture frame")
+            break
+
+        gray = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
+        ret, corners = cv.findChessboardCorners(gray, (col,row),None)
+        
+        if ret == True:
+            corners2 = cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
+    
+            # Find the rotation and translation vectors.
+            ret,rvecs, tvecs = cv.solvePnP(objp, corners2, mtx1, dist1)
+    
+            # project 3D points to image plane
+            imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, mtx1, dist1)
+            img = draw(img,corners2,imgpts)
+            cv.imshow('img',img)
+            if cv.waitKey(1) & 0xFF == ord('q'):
+                break
+    
+    cv.destroyAllWindows()
+
+#display_depth_map(mtx1, dist1, mtx2, dist2, R, T)
+#stereo_line(mtx1, dist1, mtx2, dist2, R, T)
+#finding_gripper(mtx1, dist1)
+#threeD_cube(mtx1, dist1, mtx2, dist2, R, T)
+#three_axis(mtx1, dist1, mtx2, dist2, R, T)

test.py

@@ -1,44 +1,112 @@
-#!/usr/bin/python3
-
-import cv2
 import numpy as np
+import cv2
+import os
 
-# Open two video capture objects for each camera
+def generate_point_cloud(img_left_path, img_right_path, output_path='out.ply'):
-cap_left = cv2.VideoCapture(1)  # Adjust the index if needed
+    # Load images
-cap_right = cv2.VideoCapture(2)     # Adjust the index if needed
+    print('loading images...')
+    imgL = cv2.pyrDown(cv2.imread(img_left_path))  # downscale images for faster processing
+    imgR = cv2.pyrDown(cv2.imread(img_right_path))
 
-# Check if the cameras opened successfully
+    # Disparity parameters tuned for 'aloe' image pair
-if not cap_left.isOpened() or not cap_right.isOpened():
+    window_size = 3
-    print("Error: Couldn't open one or both cameras.")
+    min_disp = 16
-    exit()
+    num_disp = 112 - min_disp
+    stereo = cv2.StereoSGBM(
+        minDisparity=min_disp,
+        numDisparities=num_disp,
+        SADWindowSize=window_size,
+        uniquenessRatio=10,
+        speckleWindowSize=100,
+        speckleRange=32,
+        disp12MaxDiff=1,
+        P1=8 * 3 * window_size**2,
+        P2=32 * 3 * window_size**2,
+        fullDP=False
+    )
 
-# Set the width and height of the video capture (adjust as needed)
+    # Compute disparity
-cap_left.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+    print('computing disparity...')
-cap_left.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+    disp = stereo.compute(imgL, imgR).astype(np.float32) / 16.0
-cap_right.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
-cap_right.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
 
-while True:
+    # Generate 3D point cloud
-    # Read frames from both cameras
+    print('generating 3d point cloud...')
-    ret_left, frame_left = cap_left.read()
+    h, w = imgL.shape[:2]
-    ret_right, frame_right = cap_right.read()
+    f = 0.8 * w  # Guess for focal length
+    Q = np.float32([[1, 0, 0, -0.5 * w],
+                    [0, -1, 0, 0.5 * h],  # Turn points 180 deg around x-axis
+                    [0, 0, 0, -f],  # so that y-axis looks up
+                    [0, 0, 1, 0]])
+    points = cv2.reprojectImageTo3D(disp, Q)
+    colors = cv2.cvtColor(imgL, cv2.COLOR_BGR2RGB)
+    mask = disp > disp.min()
+    out_points = points[mask]
+    out_colors = colors[mask]
 
-    # Break the loop if either of the cameras fails to read a frame
+    # Write point cloud to file
-    if not ret_left or not ret_right:
+    ply_header = '''ply
-       print("Error: Couldn't read frames from one or both cameras.")
+    format ascii 1.0
-       break
+    element vertex %(vert_num)d
+    property float x
+    property float y
+    property float z
+    property uchar red
+    property uchar green
+    property uchar blue
+    end_header
+    '''
 
-    # Display the frames side by side for stereo effect
+    def write_ply(fn, verts, colors):
-    stereo_frame = cv2.hconcat([frame_left, frame_right])
+        verts = verts.reshape(-1, 3)
+        colors = colors.reshape(-1, 3)
+        verts = np.hstack([verts, colors])
+        with open(fn, 'w') as f:
+            f.write(ply_header % dict(vert_num=len(verts)))
+            np.savetxt(f, verts, '%f %f %f %d %d %d')
 
-    # Display the stereo frame
+    write_ply(output_path, out_points, out_colors)
-    cv2.imshow('Stereo Camera Feed', stereo_frame)
+    print('%s saved' % output_path)
 
-    # Break the loop if 'q' key is pressed
+    # Display results
-    if cv2.waitKey(1) & 0xFF == ord('q'):
+    cv2.imshow('left', imgL)
-        break
+    cv2.imshow('disparity', (disp - min_disp) / num_disp)
-
+    cv2.waitKey()
-# Release the video capture objects and close the OpenCV window
-cap_left.release()
-cap_right.release()
     cv2.destroyAllWindows()
+
+def capture_stereo_images(cam1_id, cam2_id, num_images=10, output_folder='captured_images'):
+    # Initialize camera objects
+    cap1 = cv2.VideoCapture(cam1_id)
+    cap2 = cv2.VideoCapture(cam2_id)
+
+    # Check if cameras are opened successfully
+    if not (cap1.isOpened() and cap2.isOpened()):
+        print("Error: Could not open cameras")
+        return
+
+    # Create output folder if it doesn't exist
+    if not os.path.exists(output_folder):
+        os.makedirs(output_folder)
+
+    # Capture stereo image pairs
+    for i in range(num_images):
+        ret1, frame1 = cap1.read()
+        ret2, frame2 = cap2.read()
+
+        if not (ret1 and ret2):
+            print("Error: Could not read frames")
+            break
+
+        # Save images
+        cv2.imwrite(f"{output_folder}/left_{i}.jpg", frame1)
+        cv2.imwrite(f"{output_folder}/right_{i}.jpg", frame2)
+
+        print(f"Captured pair {i + 1}/{num_images}")
+
+    # Release video captures
+    cap1.release()
+    cap2.release()
+
+
+capture_stereo_images(1, 2, num_images=10, output_folder='captured_images')
+
+generate_point_cloud('path_to_left_img.jpg', 'path_to_right_img.jpg')

vision.py

@@ -3,9 +3,6 @@ import cv2 as cv
 import glob
 import os
 
-# Here is a litle help:
-# https://temugeb.github.io/opencv/python/2021/02/02/stereo-camera-calibration-and-triangulation.html
-
 def find_camera(find_flag):
     if find_flag:
         cam_available = []
@@ -27,6 +24,7 @@ def find_camera(find_flag):
     else:
         cam1 = 1
         cam2 = 2
+    print(f"Cameras number used : {cam1} & {cam2}")
     return cam1, cam2
 def img_capture(camera_num):
     # Create a directory to save captured images
@@ -42,7 +40,7 @@ def img_capture(camera_num):
         exit()
     i = 0
     # Capture and save 12 images
-    while i < 12:
+    while i < 6:
         # Capture a frame from the camera
         ret, frame = cap.read()
 
@@ -52,7 +50,7 @@ def img_capture(camera_num):
             break
 
         # Display the captured image
-        cv.imshow('Captured Image', frame)
+        cv.imshow('Capture Image', frame)
 
         # Save the captured image if the 's' key is pressed
         key = cv.waitKey(5) & 0xFF
@@ -61,27 +59,24 @@ def img_capture(camera_num):
             cv.imwrite(img_path, frame)
             print(f"Image {i+1} saved: {img_path}")
             i += 1
-
         # If 'q' key is pressed, exit the loop
-        elif key == ord('q'):
+        elif key == ord('q'): break
-            break
-    
     # Release the camera and close all OpenCV windows
     cap.release()
     cv.destroyAllWindows()
     print("Image capture complete.")
-
     return
 def single_calibration(camera_num, img_cap):
-    if img_cap:
+    if img_cap: img_capture(camera_num)
-        img_capture(camera_num)
     # Termination criteria
     criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
     # Prepare object points, assuming a chessboard with 9 by 6 squares of 30mm
     square_size = 30  # in millimeters
-    objp = np.zeros((5 * 8, 3), np.float32)
+    row = 9
-    objp[:, :2] = np.mgrid[0:8, 0:5].T.reshape(-1, 2) * square_size
+    col = 6
+    objp = np.zeros((row * col, 3), np.float32)
+    objp[:, :2] = np.mgrid[0:row, 0:col].T.reshape(-1, 2) * square_size
 
     # Arrays to store object points and image points from all the images.
     objpoints = []  # 3D point in real-world space
@@ -91,9 +86,8 @@ def single_calibration(camera_num, img_cap):
     for frame in images:
         img = cv.imread(frame)
         gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
-
         # Find the chessboard corners
-        ret, corners = cv.findChessboardCorners(gray, (8, 5), None)
+        ret, corners = cv.findChessboardCorners(gray, (row, col), None)
 
         # If found, add object points, image points (after refining them)
         if ret == True:
@@ -101,37 +95,30 @@ def single_calibration(camera_num, img_cap):
             corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
             imgpoints.append(corners2)
             # Draw and display the corners
-            cv.drawChessboardCorners(img, (8, 5), corners2, ret)
+            cv.drawChessboardCorners(img, (row, col), corners2, ret)
             cv.imshow('img', img)
-            cv.waitKey(400)
+            cv.waitKey(1)
 
     cv.destroyAllWindows()
     ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, (gray.shape[1], gray.shape[0]), None, None)
-
     return mtx, dist
 
 def stereo_capture():
     # Open two video capture objects for each camera
-    cap_left = cv.VideoCapture(1)  # Adjust the index if needed
+    cap_left = cv.VideoCapture(cam1)  # Adjust the index if needed
-    cap_right = cv.VideoCapture(2)  # Adjust the index if needed
+    cap_right = cv.VideoCapture(cam2)  # Adjust the index if needed
 
     # Check if the cameras opened successfully
     if not cap_left.isOpened() or not cap_right.isOpened():
         print("Error: Couldn't open one or both cameras.")
         exit()
     
-    # Set the width and height of the video capture (adjust as needed)
-    cap_left.set(cv.CAP_PROP_FRAME_WIDTH, 640)
-    cap_left.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
-    cap_right.set(cv.CAP_PROP_FRAME_WIDTH, 640)
-    cap_right.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
-    
     # Create a directory to save images
     output_dir = 'stereo_images'
     os.makedirs(output_dir, exist_ok=True)
 
     frame_counter = 0
-    while frame_counter < 12:
+    while frame_counter < 6:
         # Read frames from both cameras
         ret_left, frame_left = cap_left.read()
         ret_right, frame_right = cap_right.read()
@@ -145,9 +132,8 @@ def stereo_capture():
         stereo_frame = cv.hconcat([frame_left, frame_right])
         cv.imshow('Stereo Camera Feed', stereo_frame)
 
-
-        # Save the captured image if the 's' key is pressed
         key = cv.waitKey(5) & 0xFF
+        # Save the captured image if the 's' key is pressed
         if key == ord('s'):
             # Save the frames from both cameras
             frame_counter += 1
@@ -155,25 +141,21 @@ def stereo_capture():
             img_path_right = os.path.join(output_dir, f'{frame_counter}_right_image.jpg')
             cv.imwrite(img_path_left, frame_left)
             cv.imwrite(img_path_right, frame_right)
-            print(f"Image {frame_counter+1} saved")
+            print(f"Image {frame_counter} saved")
-
         # Break the loop if 'q' key is pressed
-        if cv.waitKey(1) & 0xFF == ord('q'):
+        if key == ord('q'): break
-            break
-
     # Release the video capture objects and close the OpenCV window
     cap_left.release()
     cap_right.release()
     cv.destroyAllWindows()
-
+    return
-#stereo_capture()
+def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder, stereo_capture_flag):
-
+    if stereo_capture_flag: stereo_capture()
-def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder):
+    # Read the synched frames
-    #read the synched frames
     images_names = glob.glob(frames_folder)
     images_names = sorted(images_names)
-    c1_images_names = images_names[:len(images_names)//2]
+    c1_images_names = images_names[0::2]
-    c2_images_names = images_names[len(images_names)//2:]
+    c2_images_names = images_names[1::2]
  
     c1_images = []
     c2_images = []
@@ -185,11 +167,11 @@ def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder):
         c2_images.append(_im)
  
     #change this if stereo calibration not good.
-    criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 100, 0.0001)
+    criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.0001)
  
-    rows = 5 #number of checkerboard rows.
+    rows = 6 #number of checkerboard rows.
-    columns = 8 #number of checkerboard columns.
+    columns = 9 #number of checkerboard columns.
-    world_scaling = 1. #change this to the real world square size. Or not.
+    world_scaling = 30 #change this to the real world square size. Or not.
  
     #coordinates of squares in the checkerboard world space
     objp = np.zeros((rows*columns,3), np.float32)
@@ -210,34 +192,168 @@ def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder):
     for frame1, frame2 in zip(c1_images, c2_images):
         gray1 = cv.cvtColor(frame1, cv.COLOR_BGR2GRAY)
         gray2 = cv.cvtColor(frame2, cv.COLOR_BGR2GRAY)
-        c_ret1, corners1 = cv.findChessboardCorners(gray1, (5, 8), None)
+        c_ret1, corners1 = cv.findChessboardCorners(gray1, (rows, columns), None)
-        c_ret2, corners2 = cv.findChessboardCorners(gray2, (5, 8), None)
+        c_ret2, corners2 = cv.findChessboardCorners(gray2, (rows, columns), None)
  
         if c_ret1 == True and c_ret2 == True:
             corners1 = cv.cornerSubPix(gray1, corners1, (11, 11), (-1, -1), criteria)
             corners2 = cv.cornerSubPix(gray2, corners2, (11, 11), (-1, -1), criteria)
  
-            cv.drawChessboardCorners(frame1, (5,8), corners1, c_ret1)
+            cv.drawChessboardCorners(frame1, (rows, columns), corners1, c_ret1)
-            cv.imshow('img', frame1)
+            #cv.imshow('img', frame1)
  
-            cv.drawChessboardCorners(frame2, (5,8), corners2, c_ret2)
+            cv.drawChessboardCorners(frame2, (rows, columns), corners2, c_ret2)
-            cv.imshow('img2', frame2)
+            #cv.imshow('img2', frame2)
-            k = cv.waitKey(500)
+            stereo_chess = cv.hconcat([frame1, frame2])
+            cv.imshow('stereo', stereo_chess)
+            cv.waitKey(1)
  
             objpoints.append(objp)
             imgpoints_left.append(corners1)
             imgpoints_right.append(corners2)
  
     stereocalibration_flags = cv.CALIB_FIX_INTRINSIC
-    ret, CM1, dist1, CM2, dist2, R, T, E, F = cv.stereoCalibrate(objpoints, imgpoints_left, imgpoints_right, mtx1, dist1, mtx2, dist2, (width, height), criteria = criteria, flags = stereocalibration_flags)
+    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(ret)
     return R, T
 
-#R, T = stereo_calibration(mtx1, dist1, mtx2, dist2, 'stereo_images/*')
+def cub_cordinate(mtx1, dist1, mtx2, dist2, R, T):
+    cap1 = cv.VideoCapture(1)
+    cap2 = cv.VideoCapture(2)
 
+    while True:
+        # Capture stereo images
+        ret1, frame1 = cap1.read()
+        ret2, frame2 = cap2.read()
+        if not ret1 and not ret2 : break
 
-cam1, cam2 = find_camera(find_flag = True)
+        frame1 = cv.undistort(frame1, mtx1, dist1)
+        frame2 = cv.undistort(frame2, mtx2, dist2)
         
-mtx1, dist1 = single_calibration(camera_num = cam1, img_cap = False)
+        # Detect red cube in both images
-mtx2, dist2 = single_calibration(camera_num = cam2, img_cap = False)
+        point1 = detect_cube(frame1, False)
+        point2 = detect_cube(frame2, False)
+        """
+        point1 = np.array(point1)
+        point2 = np.array(point2)
+        
+        #RT matrix for C1 is identity.
+        RT1 = np.concatenate([np.eye(3), [[0],[0],[0]]], axis = -1)
+        P1 = mtx1 @ RT1 #projection matrix for C1
+        
+        #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
+
+        # Call the triangulatePoints function
+        points3d_homogeneous = cv.triangulatePoints(P1, P2, point1, point2)
+        # Convert homogeneous coordinates to Euclidean coordinates
+        points3d_homogeneous /= points3d_homogeneous[3]
+        # Extract the 3D points from the homogeneous coordinates
+        points3d = points3d_homogeneous[:3]
+
+        print(points3d_homogeneous)"""
+        
+        #cal_point2 = project_point_to_camera2(point1, mtx1, R, T, mtx2)
+        transform = np.vstack((np.hstack((R, T)), [0, 0, 0, 1]))
+        point_homogeneous = np.array([point1[0], point1[1], 1, 1])
+        cal_point1_homogeneous = np.dot(transform, point_homogeneous)
+        cal_point1 = cal_point1_homogeneous[:2] / cal_point1_homogeneous[3]
+        cal_point1_x, cal_point1_y = cal_point1
+
+        cv.circle(frame1, (int(point1[0]), int(point1[1])), 2, (0, 0, 255), -1)
+        cv.circle(frame2, (int(point2[0]), int(point2[1])), 2, (0, 0, 255), -1)
+        cv.circle(frame2, (int(cal_point1_x), int(cal_point1_y)), 2, (255, 0, 0), -1)
+        print(point2, cal_point1)
+
+        stereo_frame = cv.hconcat([frame1, frame2])
+        cv.imshow('Stereo Frames', stereo_frame)
+        cv.waitKey(1)
+
+        # Break the loop on 'q' key press
+        if cv.waitKey(1) & 0xFF == ord('q'): break
+
+    # Release video capture and close windows
+    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)
+
+    # 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, 80, 80])
+    #upper = np.array([60, 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)
+
+def triangulate(mtx1, mtx2, R, T):
+ 
+    uvs1 = [[458, 86]]
+    uvs2 = [[540, 311]]
+ 
+    uvs1 = np.array(uvs1)
+    uvs2 = np.array(uvs2)
+ 
+    #RT matrix for C1 is identity.
+    RT1 = np.concatenate([np.eye(3), [[0],[0],[0]]], axis = -1)
+    P1 = mtx1 @ RT1 #projection matrix for C1
+ 
+    #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))
+    # Step 2: Transform world coordinates to camera 2 coordinate system
+    point_world_cam2 = np.dot(R, point_world) + T
+    # Step 3: Project world coordinates onto image plane of camera 2
+    point_cam2_homogeneous = np.dot(mtx2, point_world_cam2)
+    point_cam2_homogeneous /= point_cam2_homogeneous[2]  # Convert to homogeneous coordinates
+    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)
+
+cub_cordinate(mtx1, dist1, mtx2, dist2, R, T)
+
+print("$$$ Code Done $$$")