1
0
forked from HQU-gxy/CVTH3PE
This commit is contained in:
2025-06-03 16:42:51 +08:00
parent 072bf1c46f
commit eb9738cb02
8 changed files with 3042 additions and 7 deletions

2
.gitignore vendored
View File

@ -10,3 +10,5 @@ wheels/
.venv
.hypothesis
samples
*.jpg
*.parquet

View File

@ -1,6 +1,7 @@
from collections import OrderedDict, defaultdict
from dataclasses import dataclass
from datetime import datetime
import string
from typing import Any, TypeAlias, TypedDict, Optional, Sequence
from beartype import beartype
@ -522,10 +523,14 @@ def to_homogeneous(points: Num[Array, "N 2"] | Num[Array, "N 3"]) -> Num[Array,
raise ValueError(f"Invalid shape for points: {points.shape}")
import awkward as ak
@jaxtyped(typechecker=beartype)
def point_line_distance(
points: Num[Array, "N 3"] | Num[Array, "N 2"],
line: Num[Array, "N 3"],
description: str,
eps: float = 1e-9,
):
"""
@ -544,6 +549,12 @@ def point_line_distance(
"""
numerator = abs(line[:, 0] * points[:, 0] + line[:, 1] * points[:, 1] + line[:, 2])
denominator = jnp.sqrt(line[:, 0] * line[:, 0] + line[:, 1] * line[:, 1])
# line_data = {"a": line[:, 0], "b": line[:, 1], "c": line[:, 2]}
# line_x_y = {"x": points[:, 0], "y": points[:, 1]}
# ak.to_parquet(
# line_data, f"/home/admin/Code/CVTH3PE/line_a_b_c_{description}.parquet"
# )
# ak.to_parquet(line_x_y, f"/home/admin/Code/CVTH3PE/line_x_y_{description}.parquet")
return numerator / (denominator + eps)
@ -571,7 +582,7 @@ def left_to_right_epipolar_distance(
"""
F_t = fundamental_matrix.transpose()
line1_in_2 = jnp.matmul(left, F_t)
return point_line_distance(right, line1_in_2)
return point_line_distance(right, line1_in_2, "left_to_right")
@jaxtyped(typechecker=beartype)
@ -597,7 +608,7 @@ def right_to_left_epipolar_distance(
$$x^{\\prime T}Fx = 0$$
"""
line2_in_1 = jnp.matmul(right, fundamental_matrix)
return point_line_distance(left, line2_in_1)
return point_line_distance(left, line2_in_1, "right_to_left")
def distance_between_epipolar_lines(

223
filter_object_by_box.py Normal file
View File

@ -0,0 +1,223 @@
import awkward as ak
import numpy as np
from pathlib import Path
from matplotlib import pyplot as plt
import cv2
from typing import Optional, cast, Final, TypedDict
from typing import (
Any,
Generator,
Optional,
Sequence,
TypeAlias,
TypedDict,
cast,
overload,
)
from jaxtyping import Array, Float, Num, jaxtyped
from shapely import box
from app.visualize.whole_body import visualize_whole_body
import pyproj
from shapely.geometry import Polygon
from sympy import false, true
NDArray: TypeAlias = np.ndarray
# 盒子各个面的三维三角形集合
box_triangles_list = [
["4", "6", "7"],
["4", "5", "6"],
["2", "5", "6"],
["1", "2", "5"],
["1", "2", "3"],
["0", "1", "3"],
["0", "3", "7"],
["0", "4", "7"],
["2", "6", "7"],
["2", "3", "7"],
["0", "4", "5"],
["0", "1", "5"],
]
class Camera_Params(TypedDict):
rvec: Num[NDArray, "3"]
tvec: Num[NDArray, "3"]
camera_matrix: Num[Array, "3 3"]
dist: Num[Array, "N"]
width: int
height: int
class KeypointDataset(TypedDict):
frame_index: int
boxes: Num[NDArray, "N 4"]
kps: Num[NDArray, "N J 2"]
kps_scores: Num[NDArray, "N J"]
# 三维坐标系根据相机内外参计算该镜头下的二维重投影坐标
def reprojet_3d_to_2d(point_3d, camera_param):
point_2d, _ = cv2.projectPoints(
objectPoints=point_3d,
rvec=np.array(camera_param.params.Rt[:3, :3]),
tvec=np.array(camera_param.params.Rt[:3, 3]),
cameraMatrix=np.array(camera_param.params.K),
distCoeffs=np.array(camera_param.params.dist_coeffs),
)
point_2d = point_2d.reshape(-1).astype(int)
return point_2d
# 计算盒子三维坐标系
def calculaterCubeVersices(position, dimensions):
[cx, cy, cz] = position
[width, height, depth] = dimensions
halfWidth = width / 2
halfHeight = height / 2
halfDepth = depth / 2
return [
[cx - halfWidth, cy - halfHeight, cz - halfDepth],
[cx + halfWidth, cy - halfHeight, cz - halfDepth],
[cx + halfWidth, cy + halfHeight, cz - halfDepth],
[cx - halfWidth, cy + halfHeight, cz - halfDepth],
[cx - halfWidth, cy - halfHeight, cz + halfDepth],
[cx + halfWidth, cy - halfHeight, cz + halfDepth],
[cx + halfWidth, cy + halfHeight, cz + halfDepth],
[cx - halfWidth, cy + halfHeight, cz + halfDepth],
]
# 获得盒子三维坐标系
def calculater_box_3d_points():
# 盒子原点位置,相对于六面体中心偏移
box_ori_potision = [0.205 + 0.2, 0.205 + 0.50, -0.205 - 0.2]
# 盒子边长1.5米1.5米深度1.8米
box_geometry = [0.65, 1.8, 1.5]
filter_box_points_3d = calculaterCubeVersices(box_ori_potision, box_geometry)
filter_box_points_3d = {
str(index): element for index, element in enumerate(filter_box_points_3d)
}
return filter_box_points_3d
# 计算盒子坐标系的二维重投影数据
def calculater_box_2d_points(filter_box_points_3d, camera_param):
box_points_2d = dict()
for element_index, elment_point_3d in enumerate(filter_box_points_3d.values()):
box_points_2d[str(element_index)] = reprojet_3d_to_2d(
np.array(elment_point_3d), camera_param
).tolist()
return box_points_2d
# 盒子总的二维平面各三角形坐标点
def calculater_box_common_scope(box_points_2d):
box_triangles_all_points = []
# 遍历三角形个数
for i in range(len(box_triangles_list)):
# 获取单个三角形二维平面坐标点
single_triangles = []
for element_key in box_triangles_list[i]:
single_triangles.append(box_points_2d[element_key])
box_triangles_all_points.append(single_triangles)
return box_triangles_all_points
def calculate_triangle_union(triangles):
"""
计算多个三角形的并集区域
参数:
triangles: 包含多个三角形的列表,每个三角形由三个点的坐标组成
返回:
union_area: 并集区域的面积
union_polygon: 表示并集区域的多边形对象
"""
# 创建多边形对象列表
polygons = [Polygon(tri) for tri in triangles]
# 计算并集
union_polygon = polygons[0]
for polygon in polygons[1:]:
union_polygon = union_polygon.union(polygon)
# 计算并集面积
union_area = union_polygon.area
return union_area, union_polygon
# 射线法判断坐标点是否在box二维重投影的区域内
def point_in_polygon(p, polygon):
x, y = p
n = len(polygon)
intersections = 0
on_boundary = False
for i in range(n):
xi, yi = polygon[i]
xj, yj = polygon[(i + 1) % n] # 闭合多边形
# 检查点是否在顶点上
if (x == xi and y == yi) or (x == xj and y == yj):
on_boundary = True
break
# 检查点是否在线段上(非顶点情况)
if (min(xi, xj) <= x <= max(xi, xj)) and (min(yi, yj) <= y <= max(yi, yj)):
cross = (x - xi) * (yj - yi) - (y - yi) * (xj - xi)
if cross == 0:
on_boundary = True
break
# 计算射线与边的交点(非水平边)
if (yi > y) != (yj > y):
slope = (xj - xi) / (yj - yi) if (yj - yi) != 0 else float("inf")
x_intersect = xi + (y - yi) * slope
if x <= x_intersect:
intersections += 1
if on_boundary:
return false
return intersections % 2 == 1 # 奇数为内部返回True
# 获取并集区域坐标点
def get_contours(union_polygon):
if union_polygon.geom_type == "Polygon":
# 单一多边形
x, y = union_polygon.exterior.xy
contours = [(list(x)[i], list(y)[i]) for i in range(len(x))]
contours = np.array(contours, np.int32)
return contours
# 筛选落在盒子二维重投影区域内的关键点信息
def filter_kps_box(kps, contours):
# 存放筛选后的目标框
# new_boxes_data = []
# 存放筛选后的2d姿态点数据
# new_kps_data = []
# 存放筛选后的2d姿态置信度
# 遍历未筛选的目标框
x1, y1 = kps[0]
x2, y2 = kps[16]
# 保留目标框中心在范围内的坐标点
x_center = (x1 + x2) / 2
y_centet = (y1 + y2) / 2
if point_in_polygon([x1, y1], contours) and point_in_polygon([x2, y2], contours):
# if point_in_polygon([x_center, y_centet], contours) :
return true
else:
return false
# return new_kps_data

1277
play.ipynb Normal file

File diff suppressed because it is too large Load Diff

View File

@ -65,8 +65,10 @@ from app.visualize.whole_body import visualize_whole_body
NDArray: TypeAlias = np.ndarray
# %%
DATASET_PATH = Path("samples") / "04_02"
AK_CAMERA_DATASET: ak.Array = ak.from_parquet(DATASET_PATH / "camera_params.parquet")
CAMERA_PATH = Path(
"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/camera_params"
)
AK_CAMERA_DATASET: ak.Array = ak.from_parquet(CAMERA_PATH / "camera_params.parquet")
DELTA_T_MIN = timedelta(milliseconds=10)
display(AK_CAMERA_DATASET)
@ -102,6 +104,13 @@ class ExternalCameraParams(TypedDict):
# %%
# %%
DATASET_PATH = Path(
"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/detect_result/segement_1"
)
def read_dataset_by_port(port: int) -> ak.Array:
P = DATASET_PATH / f"{port}.parquet"
return ak.from_parquet(P)
@ -110,6 +119,7 @@ def read_dataset_by_port(port: int) -> ak.Array:
KEYPOINT_DATASET = {
int(p): read_dataset_by_port(p) for p in ak.to_numpy(AK_CAMERA_DATASET["port"])
}
display(KEYPOINT_DATASET)
# %%
@ -184,6 +194,8 @@ def preprocess_keypoint_dataset(
)
# %%
# %%
DetectionGenerator: TypeAlias = Generator[Detection, None, None]
@ -326,13 +338,31 @@ def homogeneous_to_euclidean(
# %%
FPS = 24
image_gen_5600 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5600], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5600][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5601 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5601], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5601][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5602 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5602], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5602][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5603 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5603], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5603][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5604 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5604], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5604][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5605 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5605], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5605][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5606 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5606], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5606][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5607 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5607], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5607][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5608 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5608], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5608][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
image_gen_5609 = preprocess_keypoint_dataset(KEYPOINT_DATASET[5609], from_camera_params(AK_CAMERA_DATASET[AK_CAMERA_DATASET["port"] == 5609][0]), FPS, datetime(2024, 4, 2, 12, 0, 0)) # type: ignore
display(1 / FPS)
sync_gen = sync_batch_gen(
[image_gen_5600, image_gen_5601, image_gen_5602], timedelta(seconds=1 / FPS)
[
image_gen_5601,
# image_gen_5602,
# image_gen_5603,
image_gen_5604,
image_gen_5605,
image_gen_5606,
# image_gen_5607,
image_gen_5608,
image_gen_5609,
],
timedelta(seconds=1 / FPS),
)
# %%
@ -345,7 +375,7 @@ display(sorted_detections)
display(
list(
map(
lambda x: {"timestamp": str(x.timestamp), "camera": x.camera.id},
lambda x: {"timestamp": str(x.timestamp), "camera": x.camera.id, "keypoint":x.keypoints.shape},
sorted_detections,
)
)
@ -413,6 +443,7 @@ for el in clusters_detections[0]:
p = plt.imshow(im)
display(p)
# %%
im_prime = np.zeros((HEIGHT, WIDTH, 3), dtype=np.uint8)
for el in clusters_detections[1]:

406
plot_epipolar_lines.ipynb Normal file

File diff suppressed because one or more lines are too long

View File

@ -0,0 +1,193 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 8,
"id": "11cc2345",
"metadata": {},
"outputs": [],
"source": [
"import awkward as ak\n",
"import numpy as np\n",
"from pathlib import Path"
]
},
{
"cell_type": "code",
"execution_count": 9,
"id": "84348d97",
"metadata": {},
"outputs": [],
"source": [
"CAMERA_INDEX ={\n",
" 2:\"5602\",\n",
" 4:\"5604\",\n",
"}\n",
"index = 4\n",
"CAMERA_PATH = Path(\"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/camera_params\")\n",
"camera_data = ak.from_parquet(CAMERA_PATH / CAMERA_INDEX[index]/ \"extrinsic.parquet\")"
]
},
{
"cell_type": "code",
"execution_count": 10,
"id": "1d771740",
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"<pre>[{rvec: [[-2.26], [0.0669], [-2.15]], tvec: [[0.166], ...]},\n",
" {rvec: [[2.07], [0.144], [2.21]], tvec: [[0.143], ...]},\n",
" {rvec: [[2.09], [0.0872], [2.25]], tvec: [[0.141], ...]},\n",
" {rvec: [[2.16], [0.172], [2.09]], tvec: [[0.162], ...]},\n",
" {rvec: [[2.15], [0.18], [2.09]], tvec: [[0.162], ...]},\n",
" {rvec: [[-2.22], [0.117], [-2.14]], tvec: [[0.162], ...]},\n",
" {rvec: [[2.18], [0.176], [2.08]], tvec: [[0.166], ...]},\n",
" {rvec: [[2.18], [0.176], [2.08]], tvec: [[0.166], ...]},\n",
" {rvec: [[-2.26], [0.116], [-2.1]], tvec: [[0.17], ...]},\n",
" {rvec: [[-2.26], [0.124], [-2.09]], tvec: [[0.171], ...]},\n",
" ...,\n",
" {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
" {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
" {rvec: [[2.12], [0.151], [2.16]], tvec: [[0.152], ...]},\n",
" {rvec: [[-2.3], [0.0733], [-2.1]], tvec: [[0.175], ...]},\n",
" {rvec: [[2.1], [0.16], [2.17]], tvec: [[0.149], ...]},\n",
" {rvec: [[2.1], [0.191], [2.13]], tvec: [[0.153], ...]},\n",
" {rvec: [[2.11], [0.196], [2.12]], tvec: [[0.154], ...]},\n",
" {rvec: [[2.19], [0.171], [2.08]], tvec: [[0.166], ...]},\n",
" {rvec: [[2.24], [0.0604], [2.12]], tvec: [[0.166], ...]}]\n",
"---------------------------------------------------------------------------\n",
"backend: cpu\n",
"nbytes: 10.1 kB\n",
"type: 90 * {\n",
" rvec: var * var * float64,\n",
" tvec: var * var * float64\n",
"}</pre>"
],
"text/plain": [
"<Array [{rvec: [...], tvec: [...]}, ..., {...}] type='90 * {rvec: var * var...'>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"display(camera_data)"
]
},
{
"cell_type": "code",
"execution_count": 11,
"id": "59fde11b",
"metadata": {},
"outputs": [],
"source": [
"data = []\n",
"for element in camera_data:\n",
" rvec = element[\"rvec\"]\n",
" if rvec[0]<0:\n",
" data.append({\"rvec\": rvec, \"tvec\": element[\"tvec\"]})"
]
},
{
"cell_type": "code",
"execution_count": 12,
"id": "4792cbc4",
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"<pyarrow._parquet.FileMetaData object at 0x7799cbf62d40>\n",
" created_by: parquet-cpp-arrow version 19.0.1\n",
" num_columns: 2\n",
" num_rows: 30\n",
" num_row_groups: 1\n",
" format_version: 2.6\n",
" serialized_size: 0"
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"ak.to_parquet(ak.from_iter(data),\"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/camera_params/5604/re_extrinsic.parquet\")"
]
},
{
"cell_type": "code",
"execution_count": 13,
"id": "8225ee33",
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"<pre>[{rvec: [[-2.26], [0.0669], [-2.15]], tvec: [[0.166], ...]},\n",
" {rvec: [[-2.22], [0.117], [-2.14]], tvec: [[0.162], ...]},\n",
" {rvec: [[-2.26], [0.116], [-2.1]], tvec: [[0.17], ...]},\n",
" {rvec: [[-2.26], [0.124], [-2.09]], tvec: [[0.171], ...]},\n",
" {rvec: [[-2.24], [0.133], [-2.11]], tvec: [[0.167], ...]},\n",
" {rvec: [[-2.22], [0.0556], [-2.2]], tvec: [[0.158], ...]},\n",
" {rvec: [[-2.27], [0.119], [-2.09]], tvec: [[0.172], ...]},\n",
" {rvec: [[-2.34], [0.0663], [-2.06]], tvec: [[0.181], ...]},\n",
" {rvec: [[-2.21], [0.117], [-2.15]], tvec: [[0.161], ...]},\n",
" {rvec: [[-2.33], [0.0731], [-2.08]], tvec: [[0.179], ...]},\n",
" ...,\n",
" {rvec: [[-2.23], [0.106], [-2.13]], tvec: [[0.166], ...]},\n",
" {rvec: [[-2.21], [0.054], [-2.2]], tvec: [[0.157], ...]},\n",
" {rvec: [[-2.19], [0.0169], [-2.25]], tvec: [[0.151], ...]},\n",
" {rvec: [[-2.2], [0.0719], [-2.19]], tvec: [[0.157], ...]},\n",
" {rvec: [[-2.22], [0.0726], [-2.18]], tvec: [[0.161], ...]},\n",
" {rvec: [[-2.2], [0.0742], [-2.19]], tvec: [[0.158], ...]},\n",
" {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
" {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
" {rvec: [[-2.3], [0.0733], [-2.1]], tvec: [[0.175], ...]}]\n",
"---------------------------------------------------------------------------\n",
"backend: cpu\n",
"nbytes: 3.4 kB\n",
"type: 30 * {\n",
" rvec: var * var * float64,\n",
" tvec: var * var * float64\n",
"}</pre>"
],
"text/plain": [
"<Array [{rvec: [...], tvec: [...]}, ..., {...}] type='30 * {rvec: var * var...'>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"temp_data = ak.from_parquet(\"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/camera_params/5604/re_extrinsic.parquet\")\n",
"display(temp_data)"
]
}
],
"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.12.9"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

File diff suppressed because one or more lines are too long