NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-09-21 14:30:28 +02:00
parent 0c13f8cb2e
commit e53dbab406
1 changed files with 15 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
robot/arlo.py
View File

@ -1,6 +1,7 @@
from time import sleep from time import sleep
import cv2 import cv2
import numpy as np
from .robot import Robot from .robot import Robot
@ -12,6 +13,9 @@ FOCAL_LENGTH = 1691
DEFAULT_CALIBRATION_CODE = "40.40-40.40-40.40_ff-ff-ff-ff" 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): def test_forward(arlo, l_power, r_power):
arlo.reset_encoder_counts() arlo.reset_encoder_counts()
arlo.go_diff(l_power, r_power, 1, 1) arlo.go_diff(l_power, r_power, 1, 1)
@ -89,17 +93,20 @@ class Arlo():
self.robot.stop() self.robot.stop()
def estimate_distance(self, bounding_box): def estimate_distance(self, bounding_box):
avg_left = (bounding_box[0][0] + bounding_box[3][0])/2 # avg_left = (bounding_box[0][0] + bounding_box[3][0])/2
avg_right = (bounding_box[1][0] + bounding_box[2][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_up = (bounding_box[0][1] + bounding_box[1][1])/2
avg_down = (bounding_box[2][1] + bounding_box[3][1])/2 # avg_down = (bounding_box[2][1] + bounding_box[3][1])/2
avg_width = avg_right - avg_left # avg_width = avg_right - avg_left
avg_height = avg_down - avg_up # 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): def find_aruco(self, image):
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)