From e53dbab406c80a6ac7e262ee7e51b3fd8c03b03c Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Wed, 21 Sep 2022 14:30:28 +0200 Subject: [PATCH] :sparkles: --- robot/arlo.py | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/robot/arlo.py b/robot/arlo.py index 08b196e..8902af1 100644 --- a/robot/arlo.py +++ b/robot/arlo.py @@ -1,6 +1,7 @@ from time import sleep import cv2 +import numpy as np from .robot import Robot @@ -12,6 +13,9 @@ FOCAL_LENGTH = 1691 DEFAULT_CALIBRATION_CODE = "40.40-40.40-40.40_ff-ff-ff-ff" +CAMERA_MATRIX = np.array([[FOCAL_LENGTH,0,512],[0,FOCAL_LENGTH,360][0,0,1]]) +DIST_COEF = [0,0,0,0] + def test_forward(arlo, l_power, r_power): arlo.reset_encoder_counts() arlo.go_diff(l_power, r_power, 1, 1) @@ -89,17 +93,20 @@ class Arlo(): self.robot.stop() def estimate_distance(self, bounding_box): - avg_left = (bounding_box[0][0] + bounding_box[3][0])/2 - avg_right = (bounding_box[1][0] + bounding_box[2][0])/2 - avg_up = (bounding_box[0][1] + bounding_box[1][1])/2 - avg_down = (bounding_box[2][1] + bounding_box[3][1])/2 + # avg_left = (bounding_box[0][0] + bounding_box[3][0])/2 + # avg_right = (bounding_box[1][0] + bounding_box[2][0])/2 + # avg_up = (bounding_box[0][1] + bounding_box[1][1])/2 + # avg_down = (bounding_box[2][1] + bounding_box[3][1])/2 - avg_width = avg_right - avg_left - avg_height = avg_down - avg_up + # avg_width = avg_right - avg_left + # avg_height = avg_down - avg_up - avg_size = (avg_width + avg_height)/2 + # avg_size = (avg_width + avg_height)/2 - return (FOCAL_LENGTH * 14.5)/avg_size + # return (FOCAL_LENGTH * 14.5)/avg_size + return cv2.aruco.estimatePoseSingleMarkers( + bounding_box, 14.5, CAMERA_MATRIX, DIST_COEF + ) def find_aruco(self, image): aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)