263 lines
9.0 KiB
Python
263 lines
9.0 KiB
Python
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) |