origin, x and y vector detection working

realWorldGcodeSneder.py

@@ -3,6 +3,10 @@ import numpy as np
 import argparse
 import cv2
 import time
+import math
+
+def dist(p1, p2):
+  return ((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2) ** 0.5
 
 cap = cv2.VideoCapture(1, cv2.CAP_DSHOW) # Set Capture Device, in case of a USB Webcam try 1, or give -1 to get a list of available devices
 cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
@@ -81,15 +85,15 @@ while(True):
     for (x, y, r) in circles:
       # draw the circle in the output image, then draw a rectangle in the image
       # corresponding to the center of the circle
-      print(output[y,x])
+      #print(output[y,x])
 
       masks.append(np.zeros((output.shape[0], output.shape[1]), np.uint8))
       cv2.circle(masks[-1], (x, y), r, (255, 255, 255), -1)
       averageRgbs.append(cv2.mean(output, mask=masks[-1])[::-1])
       blackScores.append(averageRgbs[-1][1] + averageRgbs[-1][2] + averageRgbs[-1][3])
 
-      print("    " + str(averageRgbs[-1]))
-      print("    " + str(blackScores[-1]))
+      #print("    " + str(averageRgbs[-1]))
+      #print("    " + str(blackScores[-1]))
 
     #######################################################################
     # Find circles that are black enough to be considered reference point
@@ -98,13 +102,13 @@ while(True):
     referenceCircles = []
     circleMask = np.zeros((output.shape[0], output.shape[1]), np.uint8)
     for ((x, y, r), blackScore) in zip(circles, blackScores):
-      print("        " + str(blackScore))
+      #print("        " + str(blackScore))
       if blackScore < 300:
         referenceCircles.append((x,y,r))
         cv2.circle(circleMask, (x, y), 30, (255, 255, 255), -1)
     blackWhite = cv2.bitwise_and(blackWhite, blackWhite, mask=circleMask)
     contours, hierarchy = cv2.findContours(blackWhite,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE)
-    print("Contours found: " + str(len(contours)))
+    #print("Contours found: " + str(len(contours)))
 
      
     #######################################################################
@@ -129,7 +133,35 @@ while(True):
           cv2.rectangle(output, (x - 1, y - 1), (x + 1, y + 1), (0, 128, 255), -1)
 
       cv2.drawContours(output, insideContours, -1, (0,255,0), 3)
-      print(centers)
+      #print(centers)
+
+      distToOthers = []
+      distToOthers.append(dist(centers[0], centers[1]) + dist(centers[0], centers[2]))
+      distToOthers.append(dist(centers[1], centers[0]) + dist(centers[1], centers[2]))
+      distToOthers.append(dist(centers[2], centers[0]) + dist(centers[2], centers[1]))
+
+      # sort centers by dist to others, such that shortest distance is first (start of vector)
+      centers = [x for _, x in sorted(zip(distToOthers, centers))]
+      #centers = [(100,0), (100, 100), (0,0)]
+      #centers = [(100,0), (0, 0), (100,100)]
+      #print(centers)
+      #assume x and y vector
+      origin = centers[0]
+      xVector = [centers[1][0] - centers[0][0], centers[1][1] - centers[0][1]]
+      yVector = [centers[2][0] - centers[0][0], centers[2][1] - centers[0][1]]
+      angle = math.atan2(xVector[0] * yVector[1] - xVector[1]*yVector[0], xVector[0]*yVector[0] + xVector[1]*yVector[1]) * 360 / 3.14159 / 2.0
+
+      # Make it so centers are ordered, origin, xAxis, yAxis
+      if angle < 0:
+        temp = xVector
+        xVector = yVector
+        yVector = temp
+      
+      print("origin: " + str(origin))
+      print("xVector: " + str(xVector))
+      print("yVector: " + str(yVector))
+
+
     #cv2.imshow('blackWhite',blackWhite)
 
     #######################################################################