def detect_marker_positions(self, img):
# Perform detection
corners, ids, rejected = cv2.aruco.detectMarkers(
img, self.aruco_dict, parameters=self.aruco_params)
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
corners, self.marker_length, self.camera_matrix,
self.distortion_params)
if ids is None:
return [], img
# Compute the marker positions
measurements = []
seen_ids = []
for i in range(len(ids)):
idi = ids[i, 0]
# Some markers appear multiple times but should only be handled once.
if idi in seen_ids:
continue
else:
seen_ids.append(idi)
lm_tvecs = tvecs[ids == idi].T
lm_bff2d = np.block([[lm_tvecs[2, :]], [-lm_tvecs[0, :]]])
lm_bff2d = np.mean(lm_bff2d, axis=1).reshape(-1, 1)
lm_measurement = Measurements.MarkerMeasurement(lm_bff2d, idi)
measurements.append(lm_measurement)
# Draw markers on image copy
img_marked = img.copy()
cv2.aruco.drawDetectedMarkers(img_marked, corners, ids)
return measurements, img_marked
def control(self):
dt = 0
if self.previous_time == -1:
dt == 0
else:
dt = time.time() - self.previous_time
lv, rv = self.keyboard.latest_drive_signal()
if not self.data is None:
self.data.write_keyboard(lv, rv)
drive_meas = Measurements.DriveMeasurement(lv, rv, dt)
self.previous_time = time.time()
self.slam.predict(drive_meas)
def control(self):
# Import teleoperation control signals
lv, rv = self.keyboard.latest_drive_signal()
drive_meas = Measurements.DriveMeasurement(lv, rv, dt=0.3)
self.slam.predict(drive_meas)
def control(self):
lv, rv = self.keyboard.latest_drive_signal()
if not self.data is None:
self.data.write_keyboard(lv, rv)
drive_meas = Measurements.DriveMeasurement(lv, rv, dt=0.3)
self.slam.predict(drive_meas)
def control(self, lv, rv, dt):
# Import teleoperation control signals
drive_meas = Measurements.DriveMeasurement(lv, rv, dt)
self.slam.predict(drive_meas)