commit 8fb3aa8feccd126f35e8e9ba84cb7424ee24d99d Author: Jens Ahrensfeld Date: Mon Nov 24 09:31:24 2025 +0100 Initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fbca225 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +results/ diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/cam_pose.iml b/.idea/cam_pose.iml new file mode 100644 index 0000000..c49f1cb --- /dev/null +++ b/.idea/cam_pose.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..6fdcddb --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..92f5545 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..a56aadb --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +https://docs.opencv.org/4.x/d6/d55/tutorial_table_of_content_calib3d.html +https://docs.opencv.org/4.x/d9/db7/tutorial_py_table_of_contents_calib3d.html \ No newline at end of file diff --git a/cam_calib.py b/cam_calib.py new file mode 100644 index 0000000..4bfe6bd --- /dev/null +++ b/cam_calib.py @@ -0,0 +1,77 @@ +import numpy as np +import cv2 as cv +import glob + +def main(): + # termination criteria + criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) + objp = np.zeros((6 * 7, 3), np.float32) + objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2) + + # Arrays to store object points and image points from all the images. + objpoints = [] # 3d point in real world space + imgpoints = [] # 2d points in image plane. + + images = glob.glob('*.jpg') + + for fname in images: + img = cv.imread(fname) + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + + # Find the chess board corners + ret, corners = cv.findChessboardCorners(gray, (7, 6), None) + + # If found, add object points, image points (after refining them) + if ret: + objpoints.append(objp) + + corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + imgpoints.append(corners2) + + # Draw and display the corners + cv.drawChessboardCorners(img, (7, 6), corners2, ret) + cv.imshow('img', img) + cv.waitKey(500) + + # Calibration + ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) + + # write to file + np.savez("results/cam.npz", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs) + + # Undistortion + img = cv.imread('checker_cam.jpg') + h, w = img.shape[:2] + newcameramtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h)) + + # 1. Using cv.undistort() + dst = cv.undistort(img, mtx, dist, None, newcameramtx) + + # crop the image + x, y, w, h = roi + dst = dst[y:y + h, x:x + w] + cv.imwrite('results/calibresult_1.png', dst) + + # 2. Undistort using remapping + mapx, mapy = cv.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5) + dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR) + + # crop the image + x, y, w, h = roi + dst = dst[y:y + h, x:x + w] + cv.imwrite('results/calibresult_2.png', dst) + + # Re-projection Error + mean_error = 0 + for i in range(len(objpoints)): + imgpoints2, _ = cv.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist) + error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2) / len(imgpoints2) + mean_error += error + + print("total error: {}".format(mean_error / len(objpoints))) + cv.destroyAllWindows() + +if __name__ == '__main__': + main() diff --git a/cam_pose.py b/cam_pose.py new file mode 100644 index 0000000..a6dcf93 --- /dev/null +++ b/cam_pose.py @@ -0,0 +1,99 @@ +import numpy as np +import cv2 as cv +import glob + +def draw_gizmo(img, corners, imgpts): + corner = tuple(corners[0].ravel().astype("int32")) + imgpts = imgpts.astype("int32") + img = cv.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5) + img = cv.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5) + img = cv.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5) + return img + + +def draw_cube(img, corners, imgpts): + imgpts = np.int32(imgpts).reshape(-1, 2) + + # draw ground floor in green + img = cv.drawContours(img, [imgpts[:4]], -1, (0, 255, 0), -3) + + # draw pillars in blue color + for i, j in zip(range(4), range(4, 8)): + img = cv.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255), 3) + + # draw top layer in red color + img = cv.drawContours(img, [imgpts[4:]], -1, (0, 0, 255), 3) + + return img + + +def main(): + process_video() + +def process_video(): + with np.load('results/cam.npz') as X: + mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')] + + criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) + objp = np.zeros((6 * 7, 3), np.float32) + objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2) + + axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3) + + # Open camera + video = cv.VideoCapture(0) + if not video.isOpened(): + print("Cannot open camera") + exit() + + while True: + # Read a new frame + ok, img = video.read() + if not ok: + return + process_img(img, criteria, objp, axis, mtx, dist) + k = cv.waitKey(1) & 0xFF + if k == ord('q'): + break + +def process_still(): + with np.load('results/cam.npz') as X: + mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')] + + criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) + objp = np.zeros((6 * 7, 3), np.float32) + objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2) + + axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3) + + for fname in glob.glob('check*.jpg'): + img = cv.imread(fname) + process_img(img, criteria, objp, axis, mtx, dist) + k = cv.waitKey(1) & 0xFF + if k == ord('q'): + break + + +def process_img(img, criteria, objp, axis, mtx, dist): # Load previously saved data + pattern_size = (7, 6) + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + ret, corners = cv.findChessboardCorners(gray, pattern_size, corners=None, flags=cv.CALIB_CB_ADAPTIVE_THRESH+cv.CALIB_CB_NORMALIZE_IMAGE) + cv.drawChessboardCorners(img, corners=corners, patternSize=pattern_size, patternWasFound=ret) + + if ret: + corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + + # Find the rotation and translation vectors. + ret, rvecs, tvecs = cv.solvePnP(objp, corners2, mtx, dist) + + # project 3D points to image plane + imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, mtx, dist) + + img = draw_gizmo(img, corners2, imgpts) + + cv.imshow('img', img) + + +if __name__ == '__main__': + main() + cv.destroyAllWindows() diff --git a/checker_cam.jpg b/checker_cam.jpg new file mode 100644 index 0000000..a68b675 Binary files /dev/null and b/checker_cam.jpg differ