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)
 

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.roll = min(np.radians(5), max(np.radians(-5), rot[0]))
-        self.pitch = rot[1]
+        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
 

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,

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)