988 lines
30 KiB
Plaintext
988 lines
30 KiB
Plaintext
{
|
||
"cells": [
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 57,
|
||
"id": "d2631d69",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"name": "stdout",
|
||
"output_type": "stream",
|
||
"text": [
|
||
"Homography: [[ 1.45631545 -0.00167508 62.16857114]\n",
|
||
" [ 0.02150464 1.44269413 -56.2912924 ]\n",
|
||
" [ 0.00003243 0.00000175 1. ]]\n",
|
||
"4 [[[ 0.88083101 -0.05835966 0.46982006]\n",
|
||
" [-0.05373063 0.97363845 0.2216781 ]\n",
|
||
" [-0.47037193 -0.22050467 0.85447524]]\n",
|
||
"\n",
|
||
" [[ 0.88083101 -0.05835966 0.46982006]\n",
|
||
" [-0.05373063 0.97363845 0.2216781 ]\n",
|
||
" [-0.47037193 -0.22050467 0.85447524]]\n",
|
||
"\n",
|
||
" [[ 0.99993156 -0.0053206 -0.0104192 ]\n",
|
||
" [ 0.00528968 0.99998153 -0.00299237]\n",
|
||
" [ 0.01043493 0.00293705 0.99994124]]\n",
|
||
"\n",
|
||
" [[ 0.99993156 -0.0053206 -0.0104192 ]\n",
|
||
" [ 0.00528968 0.99998153 -0.00299237]\n",
|
||
" [ 0.01043493 0.00293705 0.99994124]]] [[[ 0.14085899]\n",
|
||
" [ 0.06967199]\n",
|
||
" [ 0.54817935]]\n",
|
||
"\n",
|
||
" [[-0.14085899]\n",
|
||
" [-0.06967199]\n",
|
||
" [-0.54817935]]\n",
|
||
"\n",
|
||
" [[-0.4413341 ]\n",
|
||
" [-0.20542618]\n",
|
||
" [ 0.2970191 ]]\n",
|
||
"\n",
|
||
" [[ 0.4413341 ]\n",
|
||
" [ 0.20542618]\n",
|
||
" [-0.2970191 ]]] [[[ 0.87244355]\n",
|
||
" [ 0.40302658]\n",
|
||
" [-0.27642691]]\n",
|
||
"\n",
|
||
" [[-0.87244355]\n",
|
||
" [-0.40302658]\n",
|
||
" [ 0.27642691]]\n",
|
||
"\n",
|
||
" [[-0.00858977]\n",
|
||
" [-0.00845358]\n",
|
||
" [-0.99992737]]\n",
|
||
"\n",
|
||
" [[ 0.00858977]\n",
|
||
" [ 0.00845358]\n",
|
||
" [ 0.99992737]]]\n",
|
||
"int32\n",
|
||
"[0 3]\n",
|
||
"[[1 2]]\n",
|
||
"Truth T: [[[ 1.26688695 -0.07978837 19.1537688 ]\n",
|
||
" [ -0.07694267 1.4040634 8.53253905]\n",
|
||
" [ 0.69082299 0.3191267 1.22072533]]\n",
|
||
"\n",
|
||
" [[ 1.26688695 -0.07978837 19.1537688 ]\n",
|
||
" [ -0.07694267 1.4040634 8.53253905]\n",
|
||
" [ 0.69082299 0.3191267 1.22072533]]\n",
|
||
"\n",
|
||
" [[ 1.44503941 0.0053461 221.32627361]\n",
|
||
" [ 0.00250041 1.44206795 101.8745381 ]\n",
|
||
" [ -0.00373716 -0.0036779 1.00456832]]\n",
|
||
"\n",
|
||
" [[ 1.44503941 0.0053461 221.32627361]\n",
|
||
" [ 0.00250041 1.44206795 101.8745381 ]\n",
|
||
" [ -0.00373716 -0.0036779 1.00456832]]]\n",
|
||
"Possible Solutions: [0 3]\n",
|
||
"[[[ 0.88083101 -0.05835966 0.46982006]\n",
|
||
" [-0.05373063 0.97363845 0.2216781 ]\n",
|
||
" [-0.47037193 -0.22050467 0.85447524]]\n",
|
||
"\n",
|
||
" [[ 0.88083101 -0.05835966 0.46982006]\n",
|
||
" [-0.05373063 0.97363845 0.2216781 ]\n",
|
||
" [-0.47037193 -0.22050467 0.85447524]]\n",
|
||
"\n",
|
||
" [[ 0.99993156 -0.0053206 -0.0104192 ]\n",
|
||
" [ 0.00528968 0.99998153 -0.00299237]\n",
|
||
" [ 0.01043493 0.00293705 0.99994124]]\n",
|
||
"\n",
|
||
" [[ 0.99993156 -0.0053206 -0.0104192 ]\n",
|
||
" [ 0.00528968 0.99998153 -0.00299237]\n",
|
||
" [ 0.01043493 0.00293705 0.99994124]]]\n",
|
||
"Translations: [[[ 49.3006458 ]\n",
|
||
" [ 24.38519785]\n",
|
||
" [ 191.86277366]]\n",
|
||
"\n",
|
||
" [[ -49.3006458 ]\n",
|
||
" [ -24.38519785]\n",
|
||
" [-191.86277366]]\n",
|
||
"\n",
|
||
" [[-154.46693485]\n",
|
||
" [ -71.89916133]\n",
|
||
" [ 103.95668379]]\n",
|
||
"\n",
|
||
" [[ 154.46693485]\n",
|
||
" [ 71.89916133]\n",
|
||
" [-103.95668379]]]\n",
|
||
"normals: [[[ 0.87244355]\n",
|
||
" [ 0.40302658]\n",
|
||
" [-0.27642691]]\n",
|
||
"\n",
|
||
" [[-0.87244355]\n",
|
||
" [-0.40302658]\n",
|
||
" [ 0.27642691]]\n",
|
||
"\n",
|
||
" [[-0.00858977]\n",
|
||
" [-0.00845358]\n",
|
||
" [-0.99992737]]\n",
|
||
"\n",
|
||
" [[ 0.00858977]\n",
|
||
" [ 0.00845358]\n",
|
||
" [ 0.99992737]]]\n",
|
||
"[[-14.54372108 28.02261667 3.79060488]\n",
|
||
" [-14.54372108 28.02261667 3.79060488]\n",
|
||
" [ 0.17145973 -0.59698726 0.30486582]\n",
|
||
" [ 0.17145973 -0.59698726 0.30486582]]\n"
|
||
]
|
||
},
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"np.float64(17262.208134164426)"
|
||
]
|
||
},
|
||
"execution_count": 57,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"from pathlib import Path\n",
|
||
"import math\n",
|
||
"import cv2\n",
|
||
"\n",
|
||
"from scipy.spatial.transform import Rotation\n",
|
||
"import numpy as np\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"\n",
|
||
"from PIL import Image\n",
|
||
"\n",
|
||
"from autopilot import AutoPilot\n",
|
||
"from vision_chunk import VisionChunk\n",
|
||
"from simulator import Simulator\n",
|
||
"from position import Position\n",
|
||
"from utility import cv2_to_pil\n",
|
||
"from visualization import VisualizationManager\n",
|
||
"\n",
|
||
"chunk = VisionChunk.load_image(Path('images') / 'photo_1.png')\n",
|
||
"image_width = chunk.image.size[0]\n",
|
||
"center = image_width / 2\n",
|
||
"\n",
|
||
"focus_length = image_width / 2\n",
|
||
"K = np.array([\n",
|
||
" [focus_length, 0, center],\n",
|
||
" [0, focus_length, center],\n",
|
||
" [0, 0, 1],\n",
|
||
"])\n",
|
||
"np.set_printoptions(suppress=True)\n",
|
||
"\n",
|
||
"def get_image(chunk: VisionChunk, geo: Position) -> tuple[Image.Image, np.ndarray]:\n",
|
||
" H = geo.get_homography_matrix(K)\n",
|
||
" # R = np.array(geo.get_rotation_matrix())\n",
|
||
" # Z = np.eye(3)\n",
|
||
" # Z[2, 2] = geo.z\n",
|
||
" # T = np.eye(3)\n",
|
||
" # T[0, 2] = geo.x\n",
|
||
" # T[1, 2] = geo.y\n",
|
||
" # H = K @ Z @ R @ np.linalg.inv(K)\n",
|
||
" warped = cv2.warpPerspective(np.array(chunk.image), H, (image_width, image_width), flags=cv2.INTER_LINEAR)\n",
|
||
" plt.imshow(warped)\n",
|
||
" plt.show()\n",
|
||
" return cv2_to_pil(warped), H\n",
|
||
"\n",
|
||
"\n",
|
||
"x = 150 / focus_length\n",
|
||
"y = 70 / focus_length\n",
|
||
"prev_geo = Position(0, 0, 1, math.radians(0), math.radians(0), math.radians(0))\n",
|
||
"cur_geo = Position(x, y, 0.7, math.radians(0), math.radians(0), math.radians(0))\n",
|
||
"image1, H1 = get_image(chunk, prev_geo)\n",
|
||
"image2, H2 = get_image(chunk, cur_geo)\n",
|
||
"\n",
|
||
"chunk1 = VisionChunk(image1, 'orb')\n",
|
||
"chunk2 = VisionChunk(image2, 'orb')\n",
|
||
"\n",
|
||
"autopilot = AutoPilot()\n",
|
||
"# pts_src, pts_dst = autopilot.calculate_optical_flow(chunk1, chunk2)\n",
|
||
"pts_src, pts_dst, *_ = chunk1.detect_and_match_keypoints(chunk2)\n",
|
||
"\n",
|
||
"\n",
|
||
"\n",
|
||
"# print(pts_src.shape)\n",
|
||
"H, _ = cv2.findHomography(pts_src, pts_dst, cv2.RANSAC, 0.5, maxIters=2000)\n",
|
||
"H = H @ H1\n",
|
||
"# H = np.linalg.inv(H)\n",
|
||
"print(\"Homography:\", H)\n",
|
||
"# print(\"Rev Homography:\", np.linalg.inv(H))\n",
|
||
"# num, R, t, n\n",
|
||
"num, R, t, n = cv2.decomposeHomographyMat(H, K) \n",
|
||
"R = np.array(R)\n",
|
||
"t = np.array(t)\n",
|
||
"n = np.array(n)\n",
|
||
"print(num, R, t, n)\n",
|
||
"\n",
|
||
"# print(\"Inversed decompose:\", cv2.decomposeHomographyMat(np.linalg.inv(H), K))\n",
|
||
"\n",
|
||
"inds = cv2.filterHomographyDecompByVisibleRefpoints(R, n, pts_src, pts_dst)\n",
|
||
"if inds is not None:\n",
|
||
" inds = inds.flatten()\n",
|
||
" print(inds.dtype)\n",
|
||
" print(inds.flatten())\n",
|
||
" # R = R[inds]\n",
|
||
" # t = t[inds]\n",
|
||
" # n = n[inds]\n",
|
||
"else:\n",
|
||
" print(\"[INDS]: None :(\")\n",
|
||
"\n",
|
||
"print(np.array([\n",
|
||
" [1, 2],\n",
|
||
" [3 ,4]\n",
|
||
"])[[0]])\n",
|
||
"\n",
|
||
"T = np.linalg.inv(R) @ np.linalg.inv(K) @ H @ K\n",
|
||
"T[:, :2, 2] *= focus_length\n",
|
||
"print(\"Truth T: \", T)\n",
|
||
"\n",
|
||
"print(\"Possible Solutions:\", inds)\n",
|
||
"print(R)\n",
|
||
"print(\"Translations:\", t * focus_length)\n",
|
||
"print(\"normals:\", n)\n",
|
||
"\n",
|
||
"# vm = VisualizationManager()\n",
|
||
"# vm.update_homography_grid(np.array(chunk2.image), H, 20)\n",
|
||
"\n",
|
||
"# print(H1)\n",
|
||
"# print(H2)\n",
|
||
"# print(\"Translation\", t)\n",
|
||
"print(Rotation.from_matrix(R).as_euler('XYZ', degrees=True))\n",
|
||
"\n",
|
||
"pts_src = pts_src.reshape((-1, 2))\n",
|
||
"pts_dst = pts_dst.reshape((-1, 2))\n",
|
||
"pts_src_3 = np.c_[np.array(pts_src), np.ones(np.array(pts_src).shape[0])]\n",
|
||
"pts_dst_3 = np.c_[np.array(pts_dst), np.ones(np.array(pts_dst).shape[0])]\n",
|
||
"Z = H2.copy()\n",
|
||
"# Z[:, 2] = [0, 0, 1]\n",
|
||
"# Z = np.linalg.inv(Z)\n",
|
||
"# Z[:, 2] = [0, 22, 1]\n",
|
||
"err = (pts_dst - (Z @ pts_src_3.T).T[:, :2]) ** 2\n",
|
||
"err.mean()"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 77,
|
||
"id": "573e9f36",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"array([4, 1])"
|
||
]
|
||
},
|
||
"execution_count": 77,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"np.argpartition(np.array([4, 1, 2, 3, 0]), 2)[:2]"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "af4a38dc",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"array([[[1., 0., 0., 0.],\n",
|
||
" [1., 0., 0., 0.]],\n",
|
||
"\n",
|
||
" [[0., 1., 0., 0.],\n",
|
||
" [0., 1., 0., 0.]]])"
|
||
]
|
||
},
|
||
"execution_count": 79,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": []
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 60,
|
||
"id": "bf6f822d",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"array([[1., 0., 0.],\n",
|
||
" [0., 1., 0.],\n",
|
||
" [0., 0., 1.]])"
|
||
]
|
||
},
|
||
"execution_count": 60,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"from numpy.linalg import inv\n",
|
||
"\n",
|
||
"inv(np.eye(3))"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 59,
|
||
"id": "b35aab86",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"-0.29428571428571426"
|
||
]
|
||
},
|
||
"execution_count": 59,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"-103 / 350"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 53,
|
||
"id": "8bf8c3e2",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"array([155.93773004, 78.15418433, 2.49470517])"
|
||
]
|
||
},
|
||
"execution_count": 53,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"A = np.array([[ 0.81113805 , 0.00010604, 126.46549906],\n",
|
||
" [ 0.00012962, 0.81093256 , 63.38304349],\n",
|
||
" [ 0.00086776, 0.00035579 , 2.02320589]])\n",
|
||
"\n",
|
||
"P = np.array([0, 0, 1]) / 0.811\n",
|
||
"A @ P"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 5,
|
||
"id": "1ced4915",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"(1,\n",
|
||
" (array([[ 1.00000000e+00, 5.42096647e-17, -3.33066907e-16],\n",
|
||
" [ 5.41763505e-17, 1.00000000e+00, -7.77156117e-16],\n",
|
||
" [-5.76312734e-18, 9.51009073e-17, 1.00000000e+00]]),),\n",
|
||
" (array([[0.],\n",
|
||
" [0.],\n",
|
||
" [0.]]),),\n",
|
||
" (array([[0.],\n",
|
||
" [0.],\n",
|
||
" [0.]]),))"
|
||
]
|
||
},
|
||
"execution_count": 5,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"import cv2\n",
|
||
"import numpy as np\n",
|
||
"H = np.array([[ 1.00000000e+00, 1.49310572e-16, -1.25807618e-13],\n",
|
||
" [ 4.84132232e-17, 1.00000000e+00, -1.66095704e-13],\n",
|
||
" [-1.64660781e-20, 2.71716878e-19, 1.00000000e+00]])\n",
|
||
"cv2.decomposeHomographyMat(H, np.array([\n",
|
||
" [350, 0, 350],\n",
|
||
" [0, 350, 350],\n",
|
||
" [0, 0, 1]\n",
|
||
"]))"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 3,
|
||
"id": "fad63f09",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"np.int64(0)"
|
||
]
|
||
},
|
||
"execution_count": 3,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"ss = cv2.decomposeHomographyMat(np.eye(3), np.eye(3))\n",
|
||
"G: np.ndarray = ((ss[1] - cur_geo.get_rotation_matrix()) ** 2)\n",
|
||
"G.mean((1, 2)).argmin()"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "6665f057",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"3.141592653589793"
|
||
]
|
||
},
|
||
"execution_count": 9,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"\n",
|
||
"# ------------------------------------------------------------\n",
|
||
"\n",
|
||
"# E, mask = cv2.findEssentialMat(pts_src, pts_dst, K, method=cv2.RANSAC, prob=0.999, threshold=1.0)\n",
|
||
"\n",
|
||
"# # Извлечение поворота и трансляции\n",
|
||
"# _, R, t, mask = cv2.recoverPose(E, pts_src, pts_dst, K)\n",
|
||
"\n",
|
||
"# rotation_euler = Rotation.from_matrix(R).as_euler('ZYX', degrees=True)\n",
|
||
"# print(\"Rotation:\", rotation_euler)\n",
|
||
"\n",
|
||
"# vm = VisualizationManager()\n",
|
||
"# vm.update_homography_grid(np.array(chunk2.image), H, 20)\n",
|
||
"\n",
|
||
"# print(H1)\n",
|
||
"# print(H2)\n",
|
||
"# print(\"Translation\", t)\n",
|
||
"# Rotation.from_matrix(R).as_euler('ZYX', degrees=True)\n",
|
||
"\n",
|
||
"# ------------------------------------------------------------\n",
|
||
"\n",
|
||
"def estimate_rotation_from_homography(H, K, H_prev_estimate=None):\n",
|
||
" \"\"\"\n",
|
||
" Извлечение вращения из гомографии, игнорируя компоненту трансляции\n",
|
||
" \"\"\"\n",
|
||
" # Шаг 1: Преобразование в пространство нормализованных координат\n",
|
||
" K_inv = np.linalg.inv(K)\n",
|
||
" R_approx = K_inv @ H @ K\n",
|
||
" \n",
|
||
" # Шаг 2: SVD-ортогонализация для получения корректной матрицы вращения\n",
|
||
" U, S, Vt = np.linalg.svd(R_approx)\n",
|
||
" \n",
|
||
" # Проверка знака определителя\n",
|
||
" R = U @ Vt\n",
|
||
" if np.linalg.det(R) < 0:\n",
|
||
" # Инвертируем последний столбец U\n",
|
||
" U[:, -1] *= -1\n",
|
||
" R = U @ Vt\n",
|
||
" \n",
|
||
" return R\n",
|
||
"\n",
|
||
"# Использование:\n",
|
||
"# H, _ = cv2.findHomography(pts_src, pts_dst, cv2.RANSAC, 0.5, maxIters=2000)\n",
|
||
"# R_estimated = estimate_rotation_from_homography(H, K)\n",
|
||
"# rotation_euler = Rotation.from_matrix(R_estimated).as_euler('ZYX', degrees=True)\n",
|
||
"# print(\"Estimated rotation:\", rotation_euler)\n",
|
||
"\n",
|
||
"\n",
|
||
"\n",
|
||
"# im = bilinear_quad_to_square(np.array(chunk.image), [[0, 0], [100, 0], [100, 100], [0, 100]], 500, 500)\n",
|
||
"# cv2_to_pil(im)\n",
|
||
"\n",
|
||
"print(H1, H2)"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 6,
|
||
"id": "a38f9786",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"name": "stdout",
|
||
"output_type": "stream",
|
||
"text": [
|
||
"[[0.34560856 0.37318193 1. ]\n",
|
||
" [0.41831377 0.29715173 1. ]\n",
|
||
" [0.88572301 0.58160404 1. ]\n",
|
||
" [0.85899758 0.77806988 1. ]] [[-0.50714718 0.03888142 1. ]\n",
|
||
" [-0.49275814 0.1430908 1. ]\n",
|
||
" [-1.00607478 0.33253878 1. ]\n",
|
||
" [-1.14339579 0.18951426 1. ]]\n",
|
||
"[[-6.21408324e-01 -7.83486931e-01 7.05631752e-09]\n",
|
||
" [ 7.83486932e-01 -6.21408338e-01 -2.89870073e-09]\n",
|
||
" [ 8.26329402e-09 3.79110206e-08 1.00000000e+00]]\n",
|
||
"[[-0.62140831 -0.78348689 0. ]\n",
|
||
" [ 0.78348689 -0.62140831 0. ]\n",
|
||
" [ 0. 0. 1. ]]\n"
|
||
]
|
||
}
|
||
],
|
||
"source": [
|
||
"s = np.random.random((50, 3))\n",
|
||
"s[:, 2] = 1\n",
|
||
"yaw = np.degrees(3)\n",
|
||
"Z = np.array([\n",
|
||
" [np.cos(yaw), -np.sin(yaw), 0],\n",
|
||
" [np.sin(yaw), np.cos(yaw), 0],\n",
|
||
" [0, 0, 1],\n",
|
||
"])\n",
|
||
"p = (Z @ s.copy().T).T\n",
|
||
"print(s[:4], p[:4])\n",
|
||
"s = s[:, :2]\n",
|
||
"p = p[:, :2]\n",
|
||
"\n",
|
||
"print(cv2.findHomography(s, p)[0])\n",
|
||
"print(Z)"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"id": "4d13da7c",
|
||
"metadata": {},
|
||
"source": [
|
||
"## Откуда берётся смещение при повороте?\n",
|
||
"Ответ: ошибка содержится в определении местоположении ключевых точек"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "55ba9c48",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"from pathlib import Path\n",
|
||
"import math\n",
|
||
"import cv2\n",
|
||
"\n",
|
||
"from scipy.spatial.transform import Rotation\n",
|
||
"import numpy as np\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"\n",
|
||
"from PIL import Image\n",
|
||
"\n",
|
||
"from autopilot import AutoPilot\n",
|
||
"from vision_chunk import VisionChunk\n",
|
||
"from simulator import Simulator\n",
|
||
"from geolocation import Geolocation\n",
|
||
"from utility import cv2_to_pil\n",
|
||
"from visualization import VisualizationManager\n",
|
||
"\n",
|
||
"chunk = VisionChunk.load_image(Path('images') / 'photo_1.png')\n",
|
||
"image_width = chunk.image.size[0]\n",
|
||
"center = image_width / 2\n",
|
||
"\n",
|
||
"focus_length = 130\n",
|
||
"HEIGHT: float = 130\n",
|
||
"K = np.array([\n",
|
||
" [focus_length, 0, center],\n",
|
||
" [0, focus_length, center],\n",
|
||
" [0, 0, 1],\n",
|
||
"])\n",
|
||
"\n",
|
||
"\n",
|
||
"def bilinear_quad_to_square(img, pts_src, width, height):\n",
|
||
" \"\"\"\n",
|
||
" pts_src: массив 4 точек [[x0,y0], [x1,y1], [x2,y2], [x3,y3]]\n",
|
||
" в порядке: левый-верх, правый-верх, правый-низ, левый-низ\n",
|
||
" \"\"\"\n",
|
||
" pts = np.float32(pts_src)\n",
|
||
" \n",
|
||
" # Создаём сетку для результата\n",
|
||
" map_x = np.zeros((height, width), dtype=np.float32)\n",
|
||
" map_y = np.zeros((height, width), dtype=np.float32)\n",
|
||
"\n",
|
||
" for y in range(height):\n",
|
||
" v = y / (height - 1) if height > 1 else 0\n",
|
||
" for x in range(width):\n",
|
||
" u = x / (width - 1) if width > 1 else 0\n",
|
||
" \n",
|
||
" # Билинейная интерполяция углов четырёхугольника\n",
|
||
" px = (1 - u)*(1 - v)*pts[0,0] + u*(1 - v)*pts[1,0] + \\\n",
|
||
" (1 - u)*v*pts[3,0] + u*v*pts[2,0]\n",
|
||
" py = (1 - u)*(1 - v)*pts[0,1] + u*(1 - v)*pts[1,1] + \\\n",
|
||
" (1 - u)*v*pts[3,1] + u*v*pts[2,1]\n",
|
||
" \n",
|
||
" map_x[y, x] = px\n",
|
||
" map_y[y, x] = py\n",
|
||
"\n",
|
||
" # Применяем remap с билинейной интерполяцией цвета\n",
|
||
" result = cv2.remap(img, map_x, map_y, cv2.INTER_LINEAR)\n",
|
||
" return result\n",
|
||
"\n",
|
||
"def get_image(chunk: VisionChunk, geo: Geolocation) -> tuple[Image.Image, np.ndarray]:\n",
|
||
" R = np.array(geo.get_rotation_matrix())\n",
|
||
"\n",
|
||
" T = np.array([\n",
|
||
" [1, 0, -math.tan(geo.pitch)],\n",
|
||
" [0, 1, -math.tan(geo.roll)],\n",
|
||
" [0, 0, 1],\n",
|
||
" ])\n",
|
||
" \n",
|
||
" T = np.array([\n",
|
||
" [1, 0, 0],\n",
|
||
" [0, 1, 0],\n",
|
||
" [0, 0, 1],\n",
|
||
" ])\n",
|
||
"\n",
|
||
"\n",
|
||
" # R[:, 2] = [0, 0, 1]\n",
|
||
" H = K @ T @ R @ np.linalg.inv(K)\n",
|
||
" # print(\"R:\", R)\n",
|
||
" # print(\"H:\", H)\n",
|
||
" # print(H @ np.array([[center, center, 1]]).T)\n",
|
||
" # print(center)\n",
|
||
"\n",
|
||
" corners = np.array([\n",
|
||
" [center - focus_length, center - focus_length, 1],\n",
|
||
" [center - focus_length, center + focus_length, 1],\n",
|
||
" [center + focus_length, center + focus_length, 1],\n",
|
||
" [center + focus_length, center - focus_length, 1],\n",
|
||
" ])\n",
|
||
"\n",
|
||
" corners = ((H) @ corners.T).T\n",
|
||
" corners[:, 0] /= corners[:, 2]\n",
|
||
" corners[:, 1] /= corners[:, 2]\n",
|
||
" corners = corners[:, :2]\n",
|
||
" # print(corners)\n",
|
||
" # plt.imshow(chunk.image)\n",
|
||
" # points = corners.tolist() + [corners[0]]\n",
|
||
" # plt.plot(*zip(*points), color='red')\n",
|
||
" # plt.show()\n",
|
||
"\n",
|
||
" width = focus_length * 2\n",
|
||
" pts_src = corners.astype('float32')\n",
|
||
" pts_dst = np.float32([\n",
|
||
" [0, 0],\n",
|
||
" [width - 1, 0],\n",
|
||
" [width - 1, width - 1],\n",
|
||
" [0, width - 1],\n",
|
||
" ])\n",
|
||
"\n",
|
||
" # print(pts_src, pts_dst)\n",
|
||
" M = cv2.getPerspectiveTransform(pts_src, pts_dst)\n",
|
||
" warped = cv2.warpPerspective(np.array(chunk.image), M, (width, width), flags=cv2.INTER_LINEAR)\n",
|
||
" # warped = cv2.warpAffine(np.array(chunk.image), H, (width, width), flags=cv2.INTER_LINEAR)\n",
|
||
" # warped = bilinear_quad_to_square(np.array(chunk.image), pts_src, width, width)\n",
|
||
" # plt.imshow(warped)\n",
|
||
" # plt.show()\n",
|
||
" return cv2_to_pil(warped), H\n",
|
||
"\n",
|
||
"\n",
|
||
"prev_geo = Geolocation(0, 0, 1, math.radians(0), math.radians(0), math.radians(0))\n",
|
||
"cur_geo = Geolocation(0, 0, 1, math.radians(10), math.radians(0), math.radians(0))\n",
|
||
"image1, H1 = get_image(chunk, prev_geo)\n",
|
||
"image2, H2 = get_image(chunk, cur_geo)\n",
|
||
"\n",
|
||
"chunk1 = VisionChunk(image1, 'akaze')\n",
|
||
"chunk2 = VisionChunk(image2, 'akaze')\n",
|
||
"\n",
|
||
"# autopilot = AutoPilot()\n",
|
||
"# # pts_src, pts_dst = autopilot.calculate_optical_flow(chunk1, chunk2)\n",
|
||
"pts_src, pts_dst, *_ = chunk1.detect_and_match_keypoints(chunk2)\n",
|
||
"\n",
|
||
"\n",
|
||
"\n",
|
||
"# print(pts_src.shape)\n",
|
||
"H, _ = cv2.findHomography(pts_src, pts_dst, cv2.RANSAC, 0.5, maxIters=1000)\n",
|
||
"print(\"Homography:\", H)\n",
|
||
"n, R, t, *o = cv2.decomposeHomographyMat(H, K)\n",
|
||
"\n",
|
||
"# vm = VisualizationManager()\n",
|
||
"# vm.update_homography_grid(np.array(chunk2.image), H, 20)\n",
|
||
"\n",
|
||
"# print(H1)\n",
|
||
"# print(H2)\n",
|
||
"# print(\"Translation\", t)\n",
|
||
"# print(Rotation.from_matrix(R).as_euler('ZYX', degrees=True))\n",
|
||
"\n",
|
||
"\n",
|
||
"pts_src = pts_src.reshape((-1, 2))\n",
|
||
"pts_dst = pts_dst.reshape((-1, 2))\n",
|
||
"pts_src_3 = np.c_[np.array(pts_src), np.ones(np.array(pts_src).shape[0])]\n",
|
||
"pts_dst_3 = np.c_[np.array(pts_dst), np.ones(np.array(pts_dst).shape[0])]\n",
|
||
"Z = H2.copy()\n",
|
||
"Z[:, 2] = [0, 0, 1]\n",
|
||
"Z = np.linalg.inv(Z)\n",
|
||
"Z[:, 2] = [0, 22, 1]\n",
|
||
"err = (pts_dst - (Z @ pts_src_3.T).T[:, :2]) ** 2\n",
|
||
"err.mean()"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 223,
|
||
"id": "b3a594de",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"name": "stdout",
|
||
"output_type": "stream",
|
||
"text": [
|
||
"[[-2.041077998578922e-17, 0.3333333333333333], [6.123233995736766e-17, 1.0], [6.123233995736766e-17, -1.0], [-2.041077998578922e-17, -0.3333333333333333]]\n"
|
||
]
|
||
}
|
||
],
|
||
"source": [
|
||
"geo = Geolocation(0, 0, 1, *np.radians([0, 90, 0]))\n",
|
||
"R = geo.get_rotation_matrix()\n",
|
||
"\n",
|
||
"def norm(x: np.ndarray) -> np.ndarray:\n",
|
||
" return x / (x[2] + 2)\n",
|
||
"\n",
|
||
"def invr(p: np.ndarray, R: np.ndarray) -> np.ndarray:\n",
|
||
" return np.linalg.solve(np.array([\n",
|
||
" [R[0][0] - R[2][0] * p[0], R[0][1] - R[2][1] * p[0]],\n",
|
||
" [R[1][0] - R[2][0] * p[1], R[1][1] - R[2][1] * p[1]],\n",
|
||
" ]), np.array([\n",
|
||
" R[2][2] * p[0] - R[0][2],\n",
|
||
" R[2][2] * p[1] - R[1][2],\n",
|
||
" ]))\n",
|
||
"\n",
|
||
"# pt_src = np.array([-10000.99, 0, 1])\n",
|
||
"# pt_dst = norm(R @ pt_src)\n",
|
||
"pts_dst = np.array([\n",
|
||
" [-1, +1, 0],\n",
|
||
" [+1, +1, 0],\n",
|
||
" [+1, -1, 0],\n",
|
||
" [-1, -1, 0],\n",
|
||
"])\n",
|
||
"\n",
|
||
"# pts_src = [invr(p, R).tolist() for p in pts_dst]\n",
|
||
"pts_src = np.array([norm(p) for p in (R @ pts_dst.T).T])[:, :2].tolist()\n",
|
||
"print(pts_src)\n",
|
||
"\n",
|
||
"pts_draw = pts_src + pts_src[:1]\n",
|
||
"fig, ax = plt.subplots(1, 1)\n",
|
||
"# print(pts_src)\n",
|
||
"ax.plot(*zip(*pts_draw))\n",
|
||
"ax.set_xlim(-5, 5)\n",
|
||
"ax.set_ylim(-5, 5)\n",
|
||
"ax.set_aspect('equal')\n",
|
||
"fig.show()"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 2,
|
||
"id": "db21cca4",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"array([[[ 4, 6]],\n",
|
||
"\n",
|
||
" [[ 6, 8]],\n",
|
||
"\n",
|
||
" [[ 8, 10]]])"
|
||
]
|
||
},
|
||
"execution_count": 2,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"import numpy as np\n",
|
||
"np.array([\n",
|
||
" [[1, 2]],\n",
|
||
" [[3, 4]],\n",
|
||
" [[5, 6]],\n",
|
||
"]) + np.array([3, 4])"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "ea8f82bb",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"name": "stdout",
|
||
"output_type": "stream",
|
||
"text": [
|
||
"[[ 0.75189338 -0.27456284 352.22526201]\n",
|
||
" [ 0.06748123 0.78125365 48.06952898]\n",
|
||
" [ -0.00022715 -0.00024594 1. ]] [[350. 0. 350.]\n",
|
||
" [ 0. 350. 350.]\n",
|
||
" [ 0. 0. 1.]]\n",
|
||
"Ts: [[[ 0.82623933 -0.02899366 -0.05676676]\n",
|
||
" [-0.02881035 0.87503077 -0.02628644]\n",
|
||
" [ 0.32663485 0.15221405 1.18647679]]\n",
|
||
"\n",
|
||
" [[ 0.82623933 -0.02899366 -0.05676676]\n",
|
||
" [-0.02881035 0.87503077 -0.02628644]\n",
|
||
" [ 0.32663485 0.15221405 1.18647679]]\n",
|
||
"\n",
|
||
" [[ 0.88892726 0.00040257 0.38367359]\n",
|
||
" [ 0.00021926 0.88864413 0.17873312]\n",
|
||
" [ 0.00027199 0.00023264 1.11017551]]\n",
|
||
"\n",
|
||
" [[ 0.88892726 0.00040257 0.38367359]\n",
|
||
" [ 0.00021926 0.88864413 0.17873312]\n",
|
||
" [ 0.00027199 0.00023264 1.11017551]]]\n",
|
||
"T: [[0.88892726 0.00040257 0.38367359]\n",
|
||
" [0.00021926 0.88864413 0.17873312]\n",
|
||
" [0.00027199 0.00023264 1.11017551]]\n",
|
||
"ts: [[[ 0.10494197]\n",
|
||
" [ 0.08302777]\n",
|
||
" [ 0.52089416]]\n",
|
||
"\n",
|
||
" [[-0.10494197]\n",
|
||
" [-0.08302777]\n",
|
||
" [-0.52089416]]\n",
|
||
"\n",
|
||
" [[ 0.41630374]\n",
|
||
" [ 0.25035158]\n",
|
||
" [ 0.2307649 ]]\n",
|
||
"\n",
|
||
" [[-0.41630374]\n",
|
||
" [-0.25035158]\n",
|
||
" [-0.2307649 ]]]\n",
|
||
"t: [0.41630374 0.25035158 0.2307649 ]\n",
|
||
"Position(x=0.42, y=0.25, z=1.23, yaw=5.0°, pitch=0.9°, roll=-2.9°)\n"
|
||
]
|
||
},
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"(np.float64(145.70630950200106),\n",
|
||
" np.float64(87.62305149881264),\n",
|
||
" np.float64(1.2307649007528827))"
|
||
]
|
||
},
|
||
"execution_count": 2,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"from pathlib import Path\n",
|
||
"import math\n",
|
||
"import cv2\n",
|
||
"\n",
|
||
"from scipy.spatial.transform import Rotation\n",
|
||
"import numpy as np\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"\n",
|
||
"from PIL import Image\n",
|
||
"\n",
|
||
"from autopilot import AutoPilot\n",
|
||
"from vision_chunk import VisionChunk\n",
|
||
"from simulator import Simulator\n",
|
||
"from position import Position\n",
|
||
"from utility import cv2_to_pil\n",
|
||
"from visualization import VisualizationManager\n",
|
||
"\n",
|
||
"chunk = VisionChunk.load_image(Path('images') / 'photo_1.png')\n",
|
||
"image_width = chunk.image.size[0]\n",
|
||
"center = image_width / 2\n",
|
||
"\n",
|
||
"focus_length = image_width / 2\n",
|
||
"K = np.array([\n",
|
||
" [focus_length, 0, center],\n",
|
||
" [0, focus_length, center],\n",
|
||
" [0, 0, 1],\n",
|
||
"])\n",
|
||
"np.set_printoptions(suppress=True)\n",
|
||
"\n",
|
||
"def get_image(chunk: VisionChunk, geo: Position) -> tuple[Image.Image, np.ndarray]:\n",
|
||
" H = geo.get_homography_matrix(K)\n",
|
||
" warped = cv2.warpPerspective(np.array(chunk.image), H, (image_width, image_width), flags=cv2.INTER_LINEAR)\n",
|
||
" plt.imshow(warped)\n",
|
||
" plt.show()\n",
|
||
" return cv2_to_pil(warped), H\n",
|
||
"\n",
|
||
"\n",
|
||
"x = 150 / focus_length\n",
|
||
"y = 70 / focus_length\n",
|
||
"prev_geo = Position(y, -x, 1.30, math.radians(-5), math.radians(-4), math.radians(3))\n",
|
||
"cur_geo = Position(x, y, 1.25, math.radians(5), math.radians(1), math.radians(-3))\n",
|
||
"image1, H1 = get_image(chunk, prev_geo)\n",
|
||
"image2, H2 = get_image(chunk, cur_geo)\n",
|
||
"\n",
|
||
"chunk1 = VisionChunk(image1, 'brisk')\n",
|
||
"chunk2 = VisionChunk(image2, 'brisk')\n",
|
||
"\n",
|
||
"autopilot = AutoPilot()\n",
|
||
"# pts_src, pts_dst = autopilot.calculate_optical_flow(chunk1, chunk2)\n",
|
||
"pts_src, pts_dst, *_ = chunk1.detect_and_match_keypoints(chunk2)\n",
|
||
"\n",
|
||
"# print(pts_src.shape)\n",
|
||
"H, _ = cv2.findHomography(pts_src, pts_dst, cv2.RANSAC, 0.5, maxIters=1000)\n",
|
||
"# H = H2 @ np.linalg.inv(H1)\n",
|
||
"\n",
|
||
"prev_geo.iapply(H, K)\n",
|
||
"print(prev_geo)\n",
|
||
"prev_geo.x * focus_length, prev_geo.y * focus_length, prev_geo.z"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 5,
|
||
"id": "f9e16a1b",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"np.int64(0)"
|
||
]
|
||
},
|
||
"execution_count": 5,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"np.array([\n",
|
||
" [[0], [1], [2]],\n",
|
||
" [[4], [2], [6]],\n",
|
||
" [[2], [3], [5]],\n",
|
||
" [[7], [1], [5]],\n",
|
||
"]).sum((1, 2)).argmin()"
|
||
]
|
||
}
|
||
],
|
||
"metadata": {
|
||
"kernelspec": {
|
||
"display_name": ".venv",
|
||
"language": "python",
|
||
"name": "python3"
|
||
},
|
||
"language_info": {
|
||
"codemirror_mode": {
|
||
"name": "ipython",
|
||
"version": 3
|
||
},
|
||
"file_extension": ".py",
|
||
"mimetype": "text/x-python",
|
||
"name": "python",
|
||
"nbconvert_exporter": "python",
|
||
"pygments_lexer": "ipython3",
|
||
"version": "3.11.0"
|
||
}
|
||
},
|
||
"nbformat": 4,
|
||
"nbformat_minor": 5
|
||
}
|