first commit

.gitignore

@@ -0,0 +1,2 @@
+.venv/
+__pycache__/

README.md

@@ -0,0 +1,65 @@
+# LANDING_SPACE
+
+Thuật toán hạ cánh và điều hướng drone dựa trên ArUco marker
+
+## Tải xuống git này và cài đặt các tài nguyên cần thiết
+
+```bash
+git clone https://git.vnai.xyz/haminhtien99/landing_space
+cd landing_space
+pip install -r requirements.txt
+```
+
+## Cài đặt các giá trị ban đầu cho marker
+
+Di chuyển tới file [`default_config.py`](default_config.py), thay đổi các giá trị sau
+
+```python
+MARKER_SIZE=5
+set_resolution='original'
+```
+
+
+
+## Hiệu chuẩn camera:
+
+Để thực hiện hiệu chuẩn, cần phải chụp ảnh bàn cờ `chessboard.jpg` được in trên giấy với nhiều góc độ khác nhau
+
+`python camera_calibration/capture_images.py`
+
+- Nhấn phím "s" để chụp ảnh, nhấn "q" để kết thúc.
+- Ảnh sẽ được lưu vào thư mục con của thư mục `camera_calibration/images`, tương ứng với giá trị `set_resolution` được đặt ban đầu.
+
+`python camera_calibration/capture_images_without_showing.py`, tương tự bên trên
+
+- Ảnh sẽ được tự động lưu vào thư mục `camera_calibration/images` sau mỗi 5 giây.
+- Nhấn tổ hợp phím `Ctrl` + `C` để thoát chương trình
+
+`python camera_calibration/calibration.py`
+
+- Thông số hiệu chuẩn của camera sẽ được lưu vào thư mục `camera_calibration/calib_data`.
+
+## Dữ liệu hiệu chuẩn
+Dữ liệu hiệu chuẩn camera lưu tại tệp .npz. Dữ liệu này bao gồm:
+
+- camMatrix: Ma trận camera, chứa các tham số nội tại của camera.
+- distCoef: Các hệ số biến dạng của camera.
+- rVector: Vector xoay của từng ảnh dùng để hiệu chuẩn.
+- tVector: Vector tịnh tiến của từng ảnh hiệu chuẩn.
+
+## Tạo mã Aruco:
+
+- Tạo tùy ý 20 mã marker. Chạy `python generate_markers/generate.py`.
+- Có thể thay đổi loại marker tùy ý, trong bài toán này sử dụng loại 7x7 (`aruco.DICT_7X7_50`)
+
+
+## Ước lượng vị trí marker so với camera:
+- Chạy `distance_estimation/distance_single.py` để in kết quả đầu ra kèm kết quả hình ảnh;
+- Hoặc chạy `distance_estimation/distance_single_without_showing.py` để in kết quả đầu ra.
+
+## Ước lượng vị trí camera (Hiện không khả dụng):
+`python distance_estimation/camera_position.py`
+- Ược lượng vị trí camera trong bản đồ tạo bởi các marker được xác định trước
+- Phương pháp này tính toán vị trí camera từ vector tVec, không sử dụng ma trận nghịch đảo của ma trận xoay (vốn yêu cầu mặt phẳng camera luôn song song với mặt phẳng của marker).
+- Các script này sẽ hiển thị vị trí tương đối của camera so với marker_2 đặt ở trung tâm vùng hạ cánh. `x, y, z, roll, pitch, yaw` sẽ được hiển thị trên màn hình.
+- Nhấn phím `q` để kết thúc, thoát video

camera_calibration/calibration.py

@@ -0,0 +1,67 @@
+import cv2
+import os
+import numpy as np
+
+
+
+# LỰa chọn độ phân giải bằng cách thay đổi set_resolution
+resolution = 'original'
+
+# Checker board size
+CHESS_BOARD_DIM = (9, 6)
+
+# The size of Square in the checker board.
+SQUARE_SIZE = 24  # millimeters
+
+# termination criteria
+criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+
+calib_data_path = f"camera_calibration/calib_data/{resolution}"
+if not os.path.exists(calib_data_path):
+    os.makedirs(calib_data_path)
+
+# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
+obj_3D = np.zeros((CHESS_BOARD_DIM[0] * CHESS_BOARD_DIM[1], 3), np.float32)
+
+obj_3D[:, :2] = np.mgrid[0 : CHESS_BOARD_DIM[0], 0 : CHESS_BOARD_DIM[1]].T.reshape(
+    -1, 2
+)
+obj_3D *= SQUARE_SIZE
+print(obj_3D)
+
+# Arrays to store object points and image points from all the images.
+obj_points_3D = []  # 3d point in real world space
+img_points_2D = []  # 2d points in image plane.
+
+# The images directory path
+image_dir_path = f"camera_calibration/images/{resolution}"
+files = os.listdir(image_dir_path)
+for file in files:
+    print(file)
+    imagePath = os.path.join(image_dir_path, file)
+
+    image = cv2.imread(imagePath)
+    grayScale = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+    ret, corners = cv2.findChessboardCorners(image, CHESS_BOARD_DIM, None)
+    if ret == True:
+        obj_points_3D.append(obj_3D)
+        corners2 = cv2.cornerSubPix(grayScale, corners, (3, 3), (-1, -1), criteria)
+        img_points_2D.append(corners2)
+
+        img = cv2.drawChessboardCorners(image, CHESS_BOARD_DIM, corners2, ret)
+
+cv2.destroyAllWindows()
+ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
+    obj_points_3D, img_points_2D, grayScale.shape[::-1], None, None
+)
+print("calibrated")
+
+np.savez(
+    f"{calib_data_path}/MultiMatrix",
+    camMatrix=mtx,
+    distCoef=dist,
+    rVector=rvecs,
+    tVector=tvecs,
+)
+print(f'saved to {calib_data_path}/MultiMatrix')

camera_calibration/capture_images.py

@@ -0,0 +1,71 @@
+import cv2
+import os
+import sys
+
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+from default_config import *
+
+
+visualization = True
+CHESS_BOARD_DIM = (9, 6)
+criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+# checking if  images dir is exist not, if not then create images directory
+image_dir_path = f"CAMERA_CALIBRATION/images/{set_resolution}"
+if not os.path.exists(image_dir_path):
+    os.makedirs(image_dir_path)
+
+
+def detect_checker_board(image, grayImage, criteria, boardDimension):
+    ret, corners = cv2.findChessboardCorners(grayImage, boardDimension)
+    if ret == True:
+        corners1 = cv2.cornerSubPix(grayImage, corners, (3, 3), (-1, -1), criteria)
+        image = cv2.drawChessboardCorners(image, boardDimension, corners1, ret)
+
+    return image, ret
+
+
+cap = cv2.VideoCapture(0)
+
+# Thay đổi độ phân giải phù hợp với yêu cầu ban đầu
+new_height = RESOLUTION_STANDARDS[set_resolution]
+new_width, new_height = change_resolution(cap,new_height)    
+print(f'Resolution camera {new_width}x{new_height}')
+
+n = 0  # image_counter
+while True:
+    _, frame = cap.read()
+    copyFrame = frame.copy()
+    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+    image, board_detected = detect_checker_board(frame, gray, criteria, CHESS_BOARD_DIM)
+
+    if visualization:
+        cv2.putText(
+            frame,
+            f"saved_img : {n}",
+            (30, 40),
+            cv2.FONT_HERSHEY_PLAIN,
+            1.4,
+            (0, 255, 0),
+            2,
+            cv2.LINE_AA,
+        )
+
+        cv2.imshow("frame", frame)
+        cv2.imshow("copyFrame", copyFrame)
+
+    key = cv2.waitKey(1)
+
+    if key == ord("q"):
+        break
+    if key == ord("s") and board_detected == True:
+        # storing the checker board image
+        cv2.imwrite(f"{image_dir_path}/image{n}.png", copyFrame)
+
+        print(f"saved image number {n}")
+        n += 1  # incrementing the image counter
+cap.release()
+cv2.destroyAllWindows()
+
+print("Total saved Images:", n)

camera_calibration/capture_images_without_showing.py

@@ -0,0 +1,53 @@
+import cv2
+import os
+import sys
+import time
+
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+from default_config import *
+
+CHESS_BOARD_DIM = (9, 6)
+criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+image_dir_path = f"CAMERA_CALIBRATION/images/{set_resolution}"
+os.makedirs(image_dir_path, exist_ok=True)
+
+def detect_checker_board(image, grayImage, criteria, boardDimension):
+    ret, corners = cv2.findChessboardCorners(grayImage, boardDimension)
+    if ret:
+        corners1 = cv2.cornerSubPix(grayImage, corners, (3, 3), (-1, -1), criteria)
+        image = cv2.drawChessboardCorners(image, boardDimension, corners1, ret)
+    return image, ret
+
+cap = cv2.VideoCapture(0)
+
+# Thay đổi độ phân giải video theo cài đặt ở file default_config.py
+new_height = RESOLUTION_STANDARDS[set_resolution]
+new_width, new_height = change_resolution(cap, new_height)    
+print(f'Resolution camera {new_width}x{new_height}')
+
+n = 0  # image counter
+MAX_IMAGES = 20  # Limit for automatic saving
+
+while True:
+    ret, frame = cap.read()
+    if not ret:
+        break
+
+    copyFrame = frame.copy()
+    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+    _, board_detected = detect_checker_board(frame, gray, criteria, CHESS_BOARD_DIM)
+
+    if board_detected:
+        save_path = f"{image_dir_path}/image{n}.png"
+        cv2.imwrite(save_path, copyFrame)
+        print(f"[INFO] Saved image {n} at {save_path}")
+        n += 1
+
+        if n >= MAX_IMAGES:
+            break
+        time.sleep(5)
+
+cap.release()
+print("Total saved images:", n)

default_config.py

@@ -0,0 +1,25 @@
+import cv2
+import os
+
+os.environ["QT_QPA_PLATFORM"] = "xcb"
+
+MARKER_SIZE = 5  # centimeters
+
+# LỰa chọn độ phân giải bằng cách thay đổi set_resolution
+set_resolution = 'original'
+RESOLUTION_STANDARDS = {'540p': 540, '720p': 720, '1080p': 1080, 'original': None}
+
+
+# Thay đổi độ phân giải phù hợp với yêu cầu ban đầu
+def change_resolution(capture: cv2.VideoCapture, new_height):
+    old_height = int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
+    old_width = int(capture.get(cv2.CAP_PROP_FRAME_WIDTH))
+
+    if new_height is not None:
+        new_width = int(old_width* new_height/old_height)
+        capture.set(cv2.CAP_PROP_FRAME_WIDTH, new_width)
+        capture.set(cv2.CAP_PROP_FRAME_HEIGHT, new_height)
+    else:
+        new_width = old_width
+        new_height = old_height
+    return (new_width, new_height)

distance_estimation/camera_position.py

@@ -0,0 +1,131 @@
+''' Camera position estimation from center marker '''
+
+import cv2
+from cv2 import aruco
+import numpy as np
+import math
+import sys
+import os
+
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+
+from default_config import *
+
+
+
+calib_data_path = f"camera_calibration/calib_data/{set_resolution}/MultiMatrix.npz"
+calib_data = np.load(calib_data_path)
+print('resolution:', set_resolution)
+
+cam_mat = calib_data["camMatrix"]
+dist_coef = calib_data["distCoef"]
+
+
+marker_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_50)
+param_markers = aruco.DetectorParameters()
+
+cap = cv2.VideoCapture(0)
+
+# Thay đổi độ phân giải phù hợp với yêu cầu ban đầu
+new_height = RESOLUTION_STANDARDS[set_resolution]
+new_width, new_height = change_resolution(cap,new_height)    
+print(f'Resolution camera {new_width}x{new_height}')
+
+fps = cap.get(cv2.CAP_PROP_FPS)
+print(f"Frames per second: {fps}")
+
+# convert rotation matrix to euler angles (degrees)
+def rotationMatrixEulerAngles(rotMatrix):
+    assert rotMatrix.shape == (3, 3), "Ma trận quay phải có kích thước 3x3"
+    sy = math.sqrt(rotMatrix[0, 0]* rotMatrix[0, 0] + rotMatrix[1, 0]*rotMatrix[1, 0])
+    singular = sy < 1e-6
+    if not singular:
+        x = math.atan2(rotMatrix[2, 1], rotMatrix[2, 2])
+        x = np.round(np.degrees(x))
+        y = math.atan2(-rotMatrix[2, 0], sy)
+        y = np.round(np.degrees(y))
+        z = math.atan2(rotMatrix[1, 0], rotMatrix[0, 0])
+        z = np.round(np.degrees(z))
+    else:
+        x = math.atan2(rotMatrix[1, 0], rotMatrix[0, 0])
+        x = np.round(np.degrees(x))
+        y = math.atan2(-rotMatrix[2, 0], sy)
+        y = np.round(np.degrees(y))
+        z = 0
+    return np.array([x, y, z])
+
+while True:
+    ret, frame = cap.read()
+
+    if not ret:
+        break
+    gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    marker_corners, marker_IDs, reject = aruco.detectMarkers(
+        gray_frame, marker_dict, parameters=param_markers
+    )
+    if marker_corners:
+        rVec, tVec, _ = aruco.estimatePoseSingleMarkers(
+            marker_corners, MARKER_SIZE, cam_mat, dist_coef
+        )
+        total_markers = range(0, marker_IDs.size)
+        markers = {}
+        for ids, corners, i in zip(marker_IDs, marker_corners, total_markers):
+            corners = corners.reshape(4, 2)
+            corners = corners.astype(int)
+            top_right = corners[0].ravel()
+            top_left = corners[1].ravel()
+            bottom_right = corners[2].ravel()
+            bottom_left = corners[3].ravel()
+    
+            # Chuyển đổi rVec thành ma trận quay R
+            R, _ = cv2.Rodrigues(rVec[i])
+        
+            # Tính toán tọa độ các điểm của marker so với camera
+            object_points = np.array([
+                [-MARKER_SIZE / 2, MARKER_SIZE / 2, 0],
+                [MARKER_SIZE / 2, MARKER_SIZE / 2, 0],
+                [MARKER_SIZE / 2, -MARKER_SIZE / 2, 0],
+                [-MARKER_SIZE / 2, -MARKER_SIZE / 2, 0]
+            ])
+            marker_points_camera = R @ object_points.T + tVec[i].reshape(3, 1)
+            marker_points_camera = marker_points_camera.T
+
+            distance = np.sqrt(
+                tVec[i][0][2] ** 2 + tVec[i][0][0] ** 2 + tVec[i][0][1] ** 2
+            )
+            # Draw the pose of the marker
+            # point = cv2.drawFrameAxes(frame, cam_mat, dist_coef, rVec[i], tVec[i], 4, 4)
+            x = -tVec[i][0][0]
+            y = tVec[i][0][1]
+            z = tVec[i][0][2]
+            pos = np.array([x, y, z])
+            angles = rotationMatrixEulerAngles(R.T)
+            if ids == 2:
+                markers['marker 2'] = np.hstack((pos,angles))
+            if ids == 0:
+                pos += np.array([-12.3, 9.1, 0])
+                markers['marker 0'] = np.hstack((pos,angles))
+            if ids == 1:
+                pos += np.array([12.3, 9.1, 0.])
+                markers['marker 1'] = np.hstack((pos,angles))
+            if ids == 3:
+                pos += np.array([-7.2, -9.1, 0.])
+                markers['marker 3'] = np.hstack((pos,angles))
+            if ids == 4:
+                pos += np.array([12.3, -5., 0.])
+                markers['marker 4'] = np.hstack((pos,angles))
+        if 'marker 2' in markers.keys():
+            x, y, z, roll, pitch, yaw = np.round(markers['marker 2'], 1)
+        else:
+            cam_position = np.array([0., 0., 0., 0., 0., 0.])
+            for v in markers.values():
+                cam_position += v
+            cam_position /= len(markers)
+            x, y, z, roll, pitch, yaw = np.round(cam_position, 1)
+        print(x, y, z, roll, pitch, yaw)
+    # cv2.imshow("frame", frame)
+    key = cv2.waitKey(1)
+    if key == ord("q"):
+        break
+cap.release()
+cv2.destroyAllWindows()

distance_estimation/distance_single.py

@@ -0,0 +1,96 @@
+import cv2
+from cv2 import aruco
+import numpy as np
+import os
+import sys
+
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+from default_config import *
+
+visualzation = True
+
+
+calib_data_path = f"camera_calibration/calib_data/{set_resolution}/MultiMatrix.npz"
+calib_data = np.load(calib_data_path)
+print('resolution:', set_resolution)
+
+cam_mat = calib_data["camMatrix"]
+dist_coef = calib_data["distCoef"]
+
+
+marker_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_50)
+param_markers = aruco.DetectorParameters()
+
+cap = cv2.VideoCapture(0)
+
+# Thay đổi độ phân giải phù hợp với yêu cầu ban đầu
+new_height = RESOLUTION_STANDARDS[set_resolution]
+new_width, new_height = change_resolution(cap,new_height)    
+print(f'Resolution camera {new_width}x{new_height}')
+
+fps = cap.get(cv2.CAP_PROP_FPS)
+print(f"Frames per second: {fps}")
+
+while True:
+    ret, frame = cap.read()
+
+    if not ret:
+        break
+    gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    detector = cv2.aruco.ArucoDetector(marker_dict, param_markers)
+    marker_corners, marker_IDs, reject = detector.detectMarkers(gray_frame)
+    if marker_corners:
+        rVec, tVec, _ = aruco.estimatePoseSingleMarkers(
+            marker_corners, MARKER_SIZE, cam_mat, dist_coef
+        )
+        total_markers = range(0, marker_IDs.size)
+        for ids, corners, i in zip(marker_IDs, marker_corners, total_markers):
+            if visualzation:
+                cv2.polylines(
+                    frame, [corners.astype(np.int32)], True, (0, 255, 255), 4, cv2.LINE_AA
+                )
+            corners = corners.reshape(4, 2)
+            corners = corners.astype(int)
+            top_right = corners[0].ravel()
+            top_left = corners[1].ravel()
+            bottom_right = corners[2].ravel()
+            bottom_left = corners[3].ravel()
+
+            distance = np.linalg.norm(tVec[i][0])
+            x = tVec[i][0][0]
+            y = tVec[i][0][1]
+            z = tVec[i][0][2]
+
+            # Draw the pose of the marker
+            if visualzation:
+                point = cv2.drawFrameAxes(frame, cam_mat, dist_coef, rVec[i], tVec[i], 4, 4)
+                cv2.putText(
+                    frame,
+                    f"id: {ids[0]} Dist: {distance: .2f}",
+                    top_right,
+                    cv2.FONT_HERSHEY_PLAIN,
+                    1.3,
+                    (0, 0, 255),
+                    2,
+                    cv2.LINE_AA,
+                )
+                cv2.putText(
+                    frame,
+                    f"x:{x: .1f} y: {y: .1f} ",
+                    bottom_right,
+                    cv2.FONT_HERSHEY_PLAIN,
+                    1.0,
+                    (0, 0, 255),
+                    2,
+                    cv2.LINE_AA,
+                )
+            print(f"Marker {ids[0]}: x = {x: .1f}, y = {y: .1f} , z = {z: .1f}, distance = {distance: .2f}")
+    if visualzation:
+        cv2.imshow("frame", frame)
+        key = cv2.waitKey(1)
+        if key == ord("q"):
+            break
+cap.release()
+
+if visualzation:
+    cv2.destroyAllWindows()

generate_markers/generate.py

@@ -0,0 +1,11 @@
+import cv2 as cv
+from cv2 import aruco
+
+marker_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_50)
+
+MARKER_SIZE = 400  # pixels
+
+for id in range(5):
+    marker_image = aruco.generateImageMarker(marker_dict, id, MARKER_SIZE)
+    cv.imwrite(f"markers/marker_{id}.png", marker_image)
+