diff --git a/Condence.py b/Condence.py new file mode 100644 index 0000000..a4780fe --- /dev/null +++ b/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) \ No newline at end of file diff --git a/bin.py b/bin.py index 72a7a89..bf09849 100644 --- a/bin.py +++ b/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() -jr.m3.goal_position = 30 +def display_depth_map(mtx1, dist1, mtx2, dist2, R, T): + # 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) \ No newline at end of file diff --git a/camera1_images/image_1.jpg b/camera1_images/image_1.jpg index 2bf4e08..6132861 100644 Binary files a/camera1_images/image_1.jpg and b/camera1_images/image_1.jpg differ diff --git a/camera1_images/image_10.jpg b/camera1_images/image_10.jpg deleted file mode 100644 index 79a36d4..0000000 Binary files a/camera1_images/image_10.jpg and /dev/null differ diff --git a/camera1_images/image_11.jpg b/camera1_images/image_11.jpg deleted file mode 100644 index 71a2da3..0000000 Binary files a/camera1_images/image_11.jpg and /dev/null differ diff --git a/camera1_images/image_12.jpg b/camera1_images/image_12.jpg deleted file mode 100644 index 1dd7e51..0000000 Binary files a/camera1_images/image_12.jpg and /dev/null differ diff --git a/camera1_images/image_2.jpg b/camera1_images/image_2.jpg index 89f1c96..1634876 100644 Binary files a/camera1_images/image_2.jpg and b/camera1_images/image_2.jpg differ diff --git a/camera1_images/image_3.jpg b/camera1_images/image_3.jpg index 59022af..03067ad 100644 Binary files a/camera1_images/image_3.jpg and b/camera1_images/image_3.jpg differ diff --git a/camera1_images/image_4.jpg b/camera1_images/image_4.jpg index 9aedfca..1a0c4db 100644 Binary files a/camera1_images/image_4.jpg and b/camera1_images/image_4.jpg differ diff --git a/camera1_images/image_5.jpg b/camera1_images/image_5.jpg index c0b8a91..4da1401 100644 Binary files a/camera1_images/image_5.jpg and b/camera1_images/image_5.jpg differ diff --git a/camera1_images/image_6.jpg b/camera1_images/image_6.jpg index c65b97f..739fef4 100644 Binary files a/camera1_images/image_6.jpg and b/camera1_images/image_6.jpg differ diff --git a/camera1_images/image_7.jpg b/camera1_images/image_7.jpg deleted file mode 100644 index 4b8c84b..0000000 Binary files a/camera1_images/image_7.jpg and /dev/null differ diff --git a/camera1_images/image_8.jpg b/camera1_images/image_8.jpg deleted file mode 100644 index 914e49c..0000000 Binary files a/camera1_images/image_8.jpg and /dev/null differ diff --git a/camera1_images/image_9.jpg b/camera1_images/image_9.jpg deleted file mode 100644 index 116a09d..0000000 Binary files a/camera1_images/image_9.jpg and /dev/null differ diff --git a/camera2_images/image_1.jpg b/camera2_images/image_1.jpg index 0224a16..f1d4dcd 100644 Binary files a/camera2_images/image_1.jpg and b/camera2_images/image_1.jpg differ diff --git a/camera2_images/image_10.jpg b/camera2_images/image_10.jpg deleted file mode 100644 index 60d4670..0000000 Binary files a/camera2_images/image_10.jpg and /dev/null differ diff --git a/camera2_images/image_11.jpg b/camera2_images/image_11.jpg deleted file mode 100644 index 5a755cc..0000000 Binary files a/camera2_images/image_11.jpg and /dev/null differ diff --git a/camera2_images/image_12.jpg b/camera2_images/image_12.jpg deleted file mode 100644 index 26f78a1..0000000 Binary files a/camera2_images/image_12.jpg and /dev/null differ diff --git a/camera2_images/image_2.jpg b/camera2_images/image_2.jpg index 7a27232..f175d0f 100644 Binary files a/camera2_images/image_2.jpg and b/camera2_images/image_2.jpg differ diff --git a/camera2_images/image_3.jpg b/camera2_images/image_3.jpg index f5f32e3..4b29674 100644 Binary files a/camera2_images/image_3.jpg and b/camera2_images/image_3.jpg differ diff --git a/camera2_images/image_4.jpg b/camera2_images/image_4.jpg index 49a2c64..22e864b 100644 Binary files a/camera2_images/image_4.jpg and b/camera2_images/image_4.jpg differ diff --git a/camera2_images/image_5.jpg b/camera2_images/image_5.jpg index f1cc7c4..5ee6307 100644 Binary files a/camera2_images/image_5.jpg and b/camera2_images/image_5.jpg differ diff --git a/camera2_images/image_6.jpg b/camera2_images/image_6.jpg index d2134ac..9322376 100644 Binary files a/camera2_images/image_6.jpg and b/camera2_images/image_6.jpg differ diff --git a/camera2_images/image_7.jpg b/camera2_images/image_7.jpg deleted file mode 100644 index 0bd7789..0000000 Binary files a/camera2_images/image_7.jpg and /dev/null differ diff --git a/camera2_images/image_8.jpg b/camera2_images/image_8.jpg deleted file mode 100644 index 6cd025e..0000000 Binary files a/camera2_images/image_8.jpg and /dev/null differ diff --git a/camera2_images/image_9.jpg b/camera2_images/image_9.jpg deleted file mode 100644 index 513ee4f..0000000 Binary files a/camera2_images/image_9.jpg and /dev/null differ diff --git a/captured_images/left_0.jpg b/captured_images/left_0.jpg new file mode 100644 index 0000000..6867d29 Binary files /dev/null and b/captured_images/left_0.jpg differ diff --git a/captured_images/left_1.jpg b/captured_images/left_1.jpg new file mode 100644 index 0000000..1e30403 Binary files /dev/null and b/captured_images/left_1.jpg differ diff --git a/captured_images/left_2.jpg b/captured_images/left_2.jpg new file mode 100644 index 0000000..a4ce572 Binary files /dev/null and b/captured_images/left_2.jpg differ diff --git a/captured_images/left_3.jpg b/captured_images/left_3.jpg new file mode 100644 index 0000000..ad39037 Binary files /dev/null and b/captured_images/left_3.jpg differ diff --git a/captured_images/left_4.jpg b/captured_images/left_4.jpg new file mode 100644 index 0000000..ee2beb0 Binary files /dev/null and b/captured_images/left_4.jpg differ diff --git a/captured_images/left_5.jpg b/captured_images/left_5.jpg new file mode 100644 index 0000000..e735faa Binary files /dev/null and b/captured_images/left_5.jpg differ diff --git a/captured_images/left_6.jpg b/captured_images/left_6.jpg new file mode 100644 index 0000000..e999af4 Binary files /dev/null and b/captured_images/left_6.jpg differ diff --git a/captured_images/left_7.jpg b/captured_images/left_7.jpg new file mode 100644 index 0000000..e81fc19 Binary files /dev/null and b/captured_images/left_7.jpg differ diff --git a/captured_images/left_8.jpg b/captured_images/left_8.jpg new file mode 100644 index 0000000..7d3c7c5 Binary files /dev/null and b/captured_images/left_8.jpg differ diff --git a/captured_images/left_9.jpg b/captured_images/left_9.jpg new file mode 100644 index 0000000..27f5024 Binary files /dev/null and b/captured_images/left_9.jpg differ diff --git a/captured_images/right_0.jpg b/captured_images/right_0.jpg new file mode 100644 index 0000000..d09294d Binary files /dev/null and b/captured_images/right_0.jpg differ diff --git a/captured_images/right_1.jpg b/captured_images/right_1.jpg new file mode 100644 index 0000000..27554b7 Binary files /dev/null and b/captured_images/right_1.jpg differ diff --git a/captured_images/right_2.jpg b/captured_images/right_2.jpg new file mode 100644 index 0000000..4e2e807 Binary files /dev/null and b/captured_images/right_2.jpg differ diff --git a/captured_images/right_3.jpg b/captured_images/right_3.jpg new file mode 100644 index 0000000..56f1ab9 Binary files /dev/null and b/captured_images/right_3.jpg differ diff --git a/captured_images/right_4.jpg b/captured_images/right_4.jpg new file mode 100644 index 0000000..9308d81 Binary files /dev/null and b/captured_images/right_4.jpg differ diff --git a/captured_images/right_5.jpg b/captured_images/right_5.jpg new file mode 100644 index 0000000..8a6e7d2 Binary files /dev/null and b/captured_images/right_5.jpg differ diff --git a/captured_images/right_6.jpg b/captured_images/right_6.jpg new file mode 100644 index 0000000..de4cce6 Binary files /dev/null and b/captured_images/right_6.jpg differ diff --git a/captured_images/right_7.jpg b/captured_images/right_7.jpg new file mode 100644 index 0000000..34d3bfc Binary files /dev/null and b/captured_images/right_7.jpg differ diff --git a/captured_images/right_8.jpg b/captured_images/right_8.jpg new file mode 100644 index 0000000..afbb51e Binary files /dev/null and b/captured_images/right_8.jpg differ diff --git a/captured_images/right_9.jpg b/captured_images/right_9.jpg new file mode 100644 index 0000000..a191735 Binary files /dev/null and b/captured_images/right_9.jpg differ diff --git a/stereo_images/10_left_image.jpg b/stereo_images/10_left_image.jpg deleted file mode 100644 index fae4f45..0000000 Binary files a/stereo_images/10_left_image.jpg and /dev/null differ diff --git a/stereo_images/10_right_image.jpg b/stereo_images/10_right_image.jpg deleted file mode 100644 index ac0ee03..0000000 Binary files a/stereo_images/10_right_image.jpg and /dev/null differ diff --git a/stereo_images/11_left_image.jpg b/stereo_images/11_left_image.jpg deleted file mode 100644 index 88f4c01..0000000 Binary files a/stereo_images/11_left_image.jpg and /dev/null differ diff --git a/stereo_images/11_right_image.jpg b/stereo_images/11_right_image.jpg deleted file mode 100644 index a08c539..0000000 Binary files a/stereo_images/11_right_image.jpg and /dev/null differ diff --git a/stereo_images/12_left_image.jpg b/stereo_images/12_left_image.jpg deleted file mode 100644 index 07ae03e..0000000 Binary files a/stereo_images/12_left_image.jpg and /dev/null differ diff --git a/stereo_images/12_right_image.jpg b/stereo_images/12_right_image.jpg deleted file mode 100644 index 4d9fe14..0000000 Binary files a/stereo_images/12_right_image.jpg and /dev/null differ diff --git a/stereo_images/1_left_image.jpg b/stereo_images/1_left_image.jpg index 3631a0f..2726ce5 100644 Binary files a/stereo_images/1_left_image.jpg and b/stereo_images/1_left_image.jpg differ diff --git a/stereo_images/1_right_image.jpg b/stereo_images/1_right_image.jpg index 19c7baf..3a5d758 100644 Binary files a/stereo_images/1_right_image.jpg and b/stereo_images/1_right_image.jpg differ diff --git a/stereo_images/2_left_image.jpg b/stereo_images/2_left_image.jpg index d5aabff..bc3e2e4 100644 Binary files a/stereo_images/2_left_image.jpg and b/stereo_images/2_left_image.jpg differ diff --git a/stereo_images/2_right_image.jpg b/stereo_images/2_right_image.jpg index a40ea1f..5850f95 100644 Binary files a/stereo_images/2_right_image.jpg and b/stereo_images/2_right_image.jpg differ diff --git a/stereo_images/3_left_image.jpg b/stereo_images/3_left_image.jpg index ce3f991..7239c31 100644 Binary files a/stereo_images/3_left_image.jpg and b/stereo_images/3_left_image.jpg differ diff --git a/stereo_images/3_right_image.jpg b/stereo_images/3_right_image.jpg index 2b12b21..88d7c3d 100644 Binary files a/stereo_images/3_right_image.jpg and b/stereo_images/3_right_image.jpg differ diff --git a/stereo_images/4_left_image.jpg b/stereo_images/4_left_image.jpg index e2b6240..570e116 100644 Binary files a/stereo_images/4_left_image.jpg and b/stereo_images/4_left_image.jpg differ diff --git a/stereo_images/4_right_image.jpg b/stereo_images/4_right_image.jpg index fc63405..6e5d99a 100644 Binary files a/stereo_images/4_right_image.jpg and b/stereo_images/4_right_image.jpg differ diff --git a/stereo_images/5_left_image.jpg b/stereo_images/5_left_image.jpg index 131129b..066bcfb 100644 Binary files a/stereo_images/5_left_image.jpg and b/stereo_images/5_left_image.jpg differ diff --git a/stereo_images/5_right_image.jpg b/stereo_images/5_right_image.jpg index 26c7000..6c9c2ee 100644 Binary files a/stereo_images/5_right_image.jpg and b/stereo_images/5_right_image.jpg differ diff --git a/stereo_images/6_left_image.jpg b/stereo_images/6_left_image.jpg index 61c71eb..a3b3c80 100644 Binary files a/stereo_images/6_left_image.jpg and b/stereo_images/6_left_image.jpg differ diff --git a/stereo_images/6_right_image.jpg b/stereo_images/6_right_image.jpg index 299b05f..6540ecb 100644 Binary files a/stereo_images/6_right_image.jpg and b/stereo_images/6_right_image.jpg differ diff --git a/stereo_images/7_left_image.jpg b/stereo_images/7_left_image.jpg deleted file mode 100644 index 0c8bad1..0000000 Binary files a/stereo_images/7_left_image.jpg and /dev/null differ diff --git a/stereo_images/7_right_image.jpg b/stereo_images/7_right_image.jpg deleted file mode 100644 index d0d4abe..0000000 Binary files a/stereo_images/7_right_image.jpg and /dev/null differ diff --git a/stereo_images/8_left_image.jpg b/stereo_images/8_left_image.jpg deleted file mode 100644 index 775068e..0000000 Binary files a/stereo_images/8_left_image.jpg and /dev/null differ diff --git a/stereo_images/8_right_image.jpg b/stereo_images/8_right_image.jpg deleted file mode 100644 index 2b0f43c..0000000 Binary files a/stereo_images/8_right_image.jpg and /dev/null differ diff --git a/stereo_images/9_left_image.jpg b/stereo_images/9_left_image.jpg deleted file mode 100644 index 9bfadc2..0000000 Binary files a/stereo_images/9_left_image.jpg and /dev/null differ diff --git a/stereo_images/9_right_image.jpg b/stereo_images/9_right_image.jpg deleted file mode 100644 index f42533f..0000000 Binary files a/stereo_images/9_right_image.jpg and /dev/null differ diff --git a/test.py b/test.py index 814f88a..dcb81d4 100644 --- a/test.py +++ b/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 -cap_left = cv2.VideoCapture(1) # Adjust the index if needed -cap_right = cv2.VideoCapture(2) # Adjust the index if needed +def generate_point_cloud(img_left_path, img_right_path, output_path='out.ply'): + # Load images + 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 -if not cap_left.isOpened() or not cap_right.isOpened(): - print("Error: Couldn't open one or both cameras.") - exit() + # Disparity parameters tuned for 'aloe' image pair + window_size = 3 + min_disp = 16 + 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) -cap_left.set(cv2.CAP_PROP_FRAME_WIDTH, 640) -cap_left.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) -cap_right.set(cv2.CAP_PROP_FRAME_WIDTH, 640) -cap_right.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) + # Compute disparity + print('computing disparity...') + disp = stereo.compute(imgL, imgR).astype(np.float32) / 16.0 -while True: - # Read frames from both cameras - ret_left, frame_left = cap_left.read() - ret_right, frame_right = cap_right.read() - - # 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.") - break + # Generate 3D point cloud + print('generating 3d point cloud...') + h, w = imgL.shape[:2] + 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] - # Display the frames side by side for stereo effect - stereo_frame = cv2.hconcat([frame_left, frame_right]) + # Write point cloud to file + ply_header = '''ply + format ascii 1.0 + 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 stereo frame - cv2.imshow('Stereo Camera Feed', stereo_frame) + def write_ply(fn, verts, colors): + 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') - # Break the loop if 'q' key is pressed - if cv2.waitKey(1) & 0xFF == ord('q'): - break + write_ply(output_path, out_points, out_colors) + print('%s saved' % output_path) -# Release the video capture objects and close the OpenCV window -cap_left.release() -cap_right.release() -cv2.destroyAllWindows() \ No newline at end of file + # Display results + cv2.imshow('left', imgL) + cv2.imshow('disparity', (disp - min_disp) / num_disp) + cv2.waitKey() + 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') \ No newline at end of file diff --git a/vision.py b/vision.py index a9ebfbd..09b351f 100644 --- a/vision.py +++ b/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'): - break - + elif key == ord('q'): 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: - img_capture(camera_num) + if img_cap: 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) - objp[:, :2] = np.mgrid[0:8, 0:5].T.reshape(-1, 2) * square_size + row = 9 + 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_right = cv.VideoCapture(2) # Adjust the index if needed + cap_left = cv.VideoCapture(cam1) # 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'): - break - + if key == ord('q'): break # Release the video capture objects and close the OpenCV window cap_left.release() cap_right.release() cv.destroyAllWindows() - -#stereo_capture() - -def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder): - #read the synched frames + return +def stereo_calibration(mtx1, dist1, mtx2, dist2, frames_folder, stereo_capture_flag): + if stereo_capture_flag: stereo_capture() + # Read the synched frames images_names = glob.glob(frames_folder) images_names = sorted(images_names) - c1_images_names = images_names[:len(images_names)//2] - c2_images_names = images_names[len(images_names)//2:] + c1_images_names = images_names[0::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. - columns = 8 #number of checkerboard columns. - world_scaling = 1. #change this to the real world square size. Or not. + rows = 6 #number of checkerboard rows. + columns = 9 #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 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_ret2, corners2 = cv.findChessboardCorners(gray2, (5, 8), None) + c_ret1, corners1 = cv.findChessboardCorners(gray1, (rows, columns), 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.imshow('img', frame1) + cv.drawChessboardCorners(frame1, (rows, columns), corners1, c_ret1) + #cv.imshow('img', frame1) - cv.drawChessboardCorners(frame2, (5,8), corners2, c_ret2) - cv.imshow('img2', frame2) - k = cv.waitKey(500) + cv.drawChessboardCorners(frame2, (rows, columns), corners2, c_ret2) + #cv.imshow('img2', frame2) + 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) - - print(ret) + 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() return R, T + +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) + 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): -#R, T = stereo_calibration(mtx1, dist1, mtx2, dist2, 'stereo_images/*') + 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 = True) +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) -mtx1, dist1 = single_calibration(camera_num = cam1, img_cap = False) -mtx2, dist2 = single_calibration(camera_num = cam2, img_cap = False) \ No newline at end of file +cub_cordinate(mtx1, dist1, mtx2, dist2, R, T) + +print("$$$ Code Done $$$") \ No newline at end of file