Detecteer hoek

src/helpers/template_extraction/script.py

@@ -2,6 +2,7 @@ import cv2
 import numpy as np
 import sys
 import os
+import math as m
 
 # command:
 # python template.py input_folder output_folder
@@ -31,36 +32,40 @@ if(not os.path.isdir(output_folder)):
     print("Output folder not found")
     exit()
 
+scale_percentage = 25
+
 # Aruco params
 detector_params = cv2.aruco.DetectorParameters()
 dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_ARUCO_ORIGINAL)  
 detector = cv2.aruco.ArucoDetector(dictionary, detector_params)
 
 for file in os.listdir(input_folder):
+    # Laad een plaatje
     filename = input_folder + '/' + file
     out_filename = output_folder + '/' + file
     img = cv2.imread(filename, 0)  
-    small_img = cv2.resize(img, (750, 750))
+    
+    # Rescaling 
+    x_scale = int(img.shape[0]/100*scale_percentage)
+    y_scale = int(img.shape[1]/100*scale_percentage)
+    # print("X_scale:", x_scale)
+    # print("Y_scale:", y_scale)
+    
+    # Detecteer markers
+    small_img = cv2.resize(img, (x_scale, y_scale))
     (corners, ids, rejected) = detector.detectMarkers(small_img)
     new_image = cv2.aruco.drawDetectedMarkers(
         small_img, 
         corners, 
         ids)
     
-    # Debug
-    for id in ids:
-        print(id)
-        print(corners[id[0]])
-        print()
+    assert len(ids) == 4, "Not enough markers found"
 
+    # Zoek de "binneste" hoekjes
     i = 0
     x = np.zeros(4, dtype=np.int32)
     y = np.zeros(4, dtype=np.int32)
     for corner in corners:
-        # print(ids[i])
-        # print(corner[0])
-        # print()
-        
         x[i] = corner[0][getCorner(ids[i])][0]
         y[i] = corner[0][getCorner(ids[i])][1]
         cv2.circle(
@@ -69,22 +74,42 @@ for file in os.listdir(input_folder):
             radius=5, 
             color=(255, 100, 255),
             thickness=-1)
-        
         print("x[%d]: %d"%(i, x[i]))
         print("y[%d]: %d"%(i, y[i]))
         print()
         i += 1
 
-    offset = 20
+    # Draai plaatje om 0 en 1 gelijk te trekken
+    x0 = x[np.where(ids == 0)[0][0]]
+    x1 = x[np.where(ids == 1)[0][0]]
+    x_dif = x0-x1
+
+    y0 = y[np.where(ids == 0)[0][0]]
+    y1 = y[np.where(ids == 1)[0][0]]
+    y_dif = y0 - y1
+
+    angle_error_rad = np.arctan(y_dif/x_dif)
+    angle_error_deg = angle_error_rad * 180.0 / m.pi
+    print("Hoek deg", angle_error_deg)
+
+    # Calibreer afstand tov afstand tussen 0 en 1
+
+
+    # Teken rode balken
+
+
+    # Crop
+    offset = 180
     crop_img = new_image[
         np.min(x+offset):np.max(x-offset),
         np.min(y+offset):np.max(y-offset)
     ]
 
-    print("imshape[0]: " + str(small_img.shape[0]))
-    print("imshape[1]: " + str(small_img.shape[1]))
+    # Check voor rode balken
     cv2.imshow('window', new_image)
     cv2.imshow('cropped', crop_img)
+
+    # Keur af of sla op
     # cv2.imwrite(out_filename, new_image) 
 
     cv2.waitKey(0)