diff --git a/main.py b/main.py index 219782f..1ae372c 100644 --- a/main.py +++ b/main.py @@ -146,6 +146,16 @@ def run(name: str, map_name: str, ref_min_distance: float): chunk.pos = data['chunk_positions'][i] / online_map.pixel_ratio chunks.append(chunk) + r = 0 + for i in range(len(data['points']) - 1): + r += np.hypot( + data['points'][i][0] - data['points'][i+1][0], + data['points'][i][1] - data['points'][i+1][1] + ) + print("R: ", r) + + return + points = data['points'] / online_map.pixel_ratio print("READ POINTS:", points) diff --git a/map.jpg b/map.jpg index cd15f72..8d424c1 100644 Binary files a/map.jpg and b/map.jpg differ diff --git a/position.py b/position.py index 6b9b184..4b165cf 100644 --- a/position.py +++ b/position.py @@ -146,14 +146,14 @@ class Position: R = R[best_id] rot = Rotation.from_matrix(R).as_euler('XYZ').flatten() - self.roll = rot[0] - self.pitch = rot[1] + self.roll = min(np.radians(5), max(np.radians(-5), rot[0])) + self.pitch = min(np.radians(5), max(np.radians(-5), rot[1])) self.yaw = rot[2] t = t[best_id].flatten() self.x -= T[0] * constants._K_FOCUS_DISTANCE self.y += T[1] * constants._K_FOCUS_DISTANCE - self.z = 1 + T[2] + self.z = max(0.7, min(1.3, 1 + T[2])) T[0] *= constants._K_FOCUS_DISTANCE T[1] *= constants._K_FOCUS_DISTANCE diff --git a/test_projection.ipynb b/test_projection.ipynb index a390c75..72e3eba 100644 --- a/test_projection.ipynb +++ b/test_projection.ipynb @@ -876,6 +876,37 @@ "\n" ] }, + { + "cell_type": "code", + "execution_count": 2, + "id": "78eb2e53", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([1.41421356, 5. ])" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "import numpy as np\n", + "a = np.array([[1, 1], [3, 4]])\n", + "np.hypot(a[:, 0], a[:, 1])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6f726e54", + "metadata": {}, + "outputs": [], + "source": [] + }, { "cell_type": "code", "execution_count": null, diff --git a/vision_chunk.py b/vision_chunk.py index 8192fe0..e86b98c 100644 --- a/vision_chunk.py +++ b/vision_chunk.py @@ -10,7 +10,7 @@ from timer import Timer from typing import Literal, Optional, Tuple FeatureMethod = Literal["orb", "sift", "akaze", "brisk"] -DEFAULT_METHOD = "brisk" +DEFAULT_METHOD = "orb" @dataclass class VisionChunk: @@ -81,11 +81,11 @@ class VisionChunk: gray = img_np # Гауссовское размытие для подавления шума и мелких различий - blurred = cv2.GaussianBlur(gray, (5, 5), 1.0) + # blurred = cv2.GaussianBlur(gray, (5, 5), 1.0) # CLAHE для выравнивания контраста между снимками clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8)) - enhanced = clahe.apply(blurred) + enhanced = clahe.apply(gray) # Опционально: нормализация гистограммы для устранения различий в освещении normalized = cv2.normalize(enhanced, None, 0, 255, cv2.NORM_MINMAX)