initial commit
This commit is contained in:
+198
@@ -0,0 +1,198 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
calib.py — Odin1 的相机/雷达标定参数与 FishPoly 鱼眼相机模型。
|
||||
|
||||
公式来源：odin_ros_driver/src/pointcloud_depth_converter.cpp
|
||||
createDistortionMaps()（厂商逐像素实现），这里改写成向量化的连续函数。
|
||||
|
||||
坐标约定（已由驱动源码确认）：
|
||||
Tcl_0 把【雷达系】的点变到【相机系】：p_cam = R · p_lidar + t
|
||||
（证据：驱动里 Kcl_ = K * Tcl，然后 transformPointCloud(cloud, cloud_in_cam, Kcl_)）
|
||||
|
||||
相机模型（FishPoly）：
|
||||
归一化针孔坐标 (x, y) -> r = sqrt(x²+y²) -> θ = atan(r)
|
||||
θ_d = θ + k2θ² + k3θ³ + k4θ⁴ + k5θ⁵ + k6θ⁶ + k7θ⁷
|
||||
畸变后的归一化坐标 = (x/r, y/r) * θ_d
|
||||
像素 = (A11·x_d + A12·y_d + u0, A22·y_d + v0)
|
||||
|
||||
注意：这里所有"像素"都指**原始（未去畸变）图像**的像素，也就是 /odin1/image。
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
# ---------------- calib.yaml（设备 #O1-N090100083） ----------------
|
||||
A11 = 730.51652555414909
|
||||
A12 = -0.68917984297115186
|
||||
A22 = 730.30384140813805
|
||||
U0 = 785.10214573595919
|
||||
V0 = 670.33088344368650
|
||||
|
||||
K2 = -8.2510665959742261e-03
|
||||
K3 = 1.6324575756549702e-02
|
||||
K4 = -5.6837005957061821e-02
|
||||
K5 = 4.9991959834060670e-02
|
||||
K6 = -2.4463726125325536e-02
|
||||
K7 = 3.8528646040351232e-03
|
||||
|
||||
IMAGE_WIDTH = 1600
|
||||
IMAGE_HEIGHT = 1296
|
||||
MAX_INCIDENT_DEG = 120.0
|
||||
|
||||
# Tcl_0：雷达 -> 相机
|
||||
TCL = np.array([
|
||||
[-0.00177, -0.99998, 0.00586, 0.03809],
|
||||
[ 0.00744, -0.00588, -0.99996, -0.00189],
|
||||
[ 0.99997, -0.00173, 0.00745, -0.01966],
|
||||
[ 0.0, 0.0, 0.0, 1.0 ],
|
||||
], dtype=np.float64)
|
||||
R_CAM_LIDAR = TCL[:3, :3]
|
||||
T_CAM_LIDAR = TCL[:3, 3]
|
||||
|
||||
# 安全上限：θ 接近 90° 时 tan(θ) 发散。
|
||||
# 这颗镜头水平半视角约 63°，图像四角可到 ~87°，所以上限放到 89°。
|
||||
_THETA_MAX = np.deg2rad(89.0)
|
||||
|
||||
|
||||
def theta_d(theta):
|
||||
"""把入射角 θ 映射成畸变后的归一化半径。"""
|
||||
th = np.asarray(theta, dtype=np.float64)
|
||||
return (th + K2 * th**2 + K3 * th**3 + K4 * th**4
|
||||
+ K5 * th**5 + K6 * th**6 + K7 * th**7)
|
||||
|
||||
|
||||
def dtheta_d(theta):
|
||||
"""θ_d 对 θ 的导数（牛顿迭代用）。"""
|
||||
th = np.asarray(theta, dtype=np.float64)
|
||||
return (1.0 + 2 * K2 * th + 3 * K3 * th**2 + 4 * K4 * th**3
|
||||
+ 5 * K5 * th**4 + 6 * K6 * th**5 + 7 * K7 * th**6)
|
||||
|
||||
|
||||
def solve_theta(rho_d, iters=30):
|
||||
"""反解：给定畸变归一化半径 rho_d，求入射角 θ。"""
|
||||
rho_d = np.asarray(rho_d, dtype=np.float64)
|
||||
th = np.minimum(rho_d, _THETA_MAX).astype(np.float64)
|
||||
for _ in range(iters):
|
||||
f = theta_d(th) - rho_d
|
||||
fp = dtheta_d(th)
|
||||
fp = np.where(np.abs(fp) < 1e-12, 1e-12, fp)
|
||||
step = f / fp
|
||||
th = np.clip(th - step, 0.0, _THETA_MAX)
|
||||
if np.all(np.abs(step) < 1e-12):
|
||||
break
|
||||
return th
|
||||
|
||||
|
||||
def pixel_to_ray(u, v):
|
||||
"""原始(畸变)图像像素 -> 相机系下的单位方向向量。
|
||||
|
||||
标量输入返回 (3,)，数组输入返回 (N,3)。
|
||||
"""
|
||||
u = np.asarray(u, dtype=np.float64)
|
||||
v = np.asarray(v, dtype=np.float64)
|
||||
yd = (v - V0) / A22
|
||||
xd = (u - U0 - A12 * yd) / A11
|
||||
|
||||
rho = np.hypot(xd, yd)
|
||||
small = rho < 1e-9
|
||||
rho_safe = np.where(small, 1.0, rho)
|
||||
|
||||
theta = solve_theta(rho)
|
||||
r = np.tan(theta)
|
||||
|
||||
x = xd * (r / rho_safe)
|
||||
y = yd * (r / rho_safe)
|
||||
d = np.stack([x, y, np.ones_like(x)], axis=-1)
|
||||
d = d / np.linalg.norm(d, axis=-1, keepdims=True)
|
||||
return d
|
||||
|
||||
|
||||
def project_cam_to_pixel(p_cam, margin=0.0):
|
||||
"""相机系点 (N,3) -> 原始图像像素 (u, v, valid)。"""
|
||||
p = np.atleast_2d(np.asarray(p_cam, dtype=np.float64))
|
||||
Z = p[:, 2]
|
||||
valid = np.isfinite(p).all(axis=1) & (Z > 1e-6)
|
||||
Zs = np.where(valid, Z, 1.0)
|
||||
|
||||
x = p[:, 0] / Zs
|
||||
y = p[:, 1] / Zs
|
||||
r = np.hypot(x, y)
|
||||
theta = np.arctan(r)
|
||||
thd = theta_d(theta)
|
||||
|
||||
s = np.where(r > 1e-9, thd / np.where(r > 1e-9, r, 1.0), 1.0)
|
||||
xd = x * s
|
||||
yd = y * s
|
||||
|
||||
u = A11 * xd + A12 * yd + U0
|
||||
v = A22 * yd + V0
|
||||
valid &= np.isfinite(u) & np.isfinite(v)
|
||||
valid &= (u >= -margin) & (u <= IMAGE_WIDTH - 1 + margin)
|
||||
valid &= (v >= -margin) & (v <= IMAGE_HEIGHT - 1 + margin)
|
||||
return u, v, valid
|
||||
|
||||
|
||||
def lidar_to_cam(pts):
|
||||
"""雷达系点 (N,3) -> 相机系点 (N,3)。"""
|
||||
pts = np.atleast_2d(np.asarray(pts, dtype=np.float64))
|
||||
return pts @ R_CAM_LIDAR.T + T_CAM_LIDAR
|
||||
|
||||
|
||||
def cam_to_lidar(pts):
|
||||
"""相机系点 (N,3) -> 雷达系点 (N,3)。"""
|
||||
pts = np.atleast_2d(np.asarray(pts, dtype=np.float64))
|
||||
return (pts - T_CAM_LIDAR) @ R_CAM_LIDAR
|
||||
|
||||
|
||||
def cam_to_base_link(pts, cam_xyz=(0.0, 0.0, 0.0), rpy_deg=(0.0, 0.0, 0.0)):
|
||||
"""相机系 -> 车体系。
|
||||
|
||||
坐标约定：
|
||||
相机系：x 右, y 下, z 前
|
||||
车体系：x 前, y 左, z 上（ROS REP-103, base_link）
|
||||
|
||||
参数：
|
||||
cam_xyz : 相机光心在车体系下的位置 (m)，例如 (0.1, 0.0, 0.3)
|
||||
rpy_deg : 相机相对车体的姿态 (roll, pitch, yaw)，单位度
|
||||
pitch > 0 表示镜头向下俯（常见安装）
|
||||
"""
|
||||
pts = np.atleast_2d(np.asarray(pts, dtype=np.float64))
|
||||
R = _base_from_cam_rotation(rpy_deg)
|
||||
return pts @ R.T + np.asarray(cam_xyz, dtype=np.float64)
|
||||
|
||||
|
||||
def base_link_to_cam(pts, cam_xyz=(0.0, 0.0, 0.0), rpy_deg=(0.0, 0.0, 0.0)):
|
||||
"""车体系 -> 相机系（cam_to_base_link 的逆）。"""
|
||||
pts = np.atleast_2d(np.asarray(pts, dtype=np.float64))
|
||||
R = _base_from_cam_rotation(rpy_deg)
|
||||
return (pts - np.asarray(cam_xyz, dtype=np.float64)) @ R
|
||||
|
||||
|
||||
def _base_from_cam_rotation(rpy_deg=(0.0, 0.0, 0.0)):
|
||||
"""构造 R_base_cam。零姿态时：相机 z(前)->车体 x(前)，x(右)->车体 -y，y(下)->车体 -z。"""
|
||||
roll, pitch, yaw = np.deg2rad(rpy_deg)
|
||||
Rx = np.array([[1, 0, 0],
|
||||
[0, np.cos(roll), -np.sin(roll)],
|
||||
[0, np.sin(roll), np.cos(roll)]])
|
||||
Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)],
|
||||
[0, 1, 0],
|
||||
[-np.sin(pitch), 0, np.cos(pitch)]])
|
||||
Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0],
|
||||
[np.sin(yaw), np.cos(yaw), 0],
|
||||
[0, 0, 1]])
|
||||
R0 = np.array([[0.0, 0.0, 1.0],
|
||||
[-1.0, 0.0, 0.0],
|
||||
[0.0, -1.0, 0.0]])
|
||||
return Rz @ Ry @ Rx @ R0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# 往返一致性自测：像素 -> 射线 -> 投回像素
|
||||
print('Tcl determinant (should be +1):', np.linalg.det(R_CAM_LIDAR))
|
||||
print('camera position in lidar frame (m):', cam_to_lidar(np.zeros((1, 3)))[0])
|
||||
print()
|
||||
for u, v in [(800, 648), (100, 100), (1500, 1200), (400, 900), (1599, 0)]:
|
||||
d = pixel_to_ray(u, v)
|
||||
p = d * 3.0
|
||||
u2, v2, ok = project_cam_to_pixel(p)
|
||||
print('pixel (%4d,%4d) -> ray %s -> back (%8.2f,%8.2f) valid=%s err=%.2e'
|
||||
% (u, v, np.round(d, 4), u2[0], v2[0], ok[0],
|
||||
np.hypot(u2[0] - u, v2[0] - v)))
|
||||
@@ -0,0 +1,12 @@
|
||||
# 板子端（Ubuntu 24.04 + ROS2 Jazzy）
|
||||
#
|
||||
# ROS2 本体用 apt 安装，不要写进 pip requirements：
|
||||
# sudo apt install ros-jazzy-rclpy ros-jazzy-sensor-msgs \
|
||||
# ros-jazzy-nav-msgs ros-jazzy-message-filters
|
||||
#
|
||||
pynng>=0.9.0
|
||||
numpy>=1.26
|
||||
|
||||
# 仅当使用 --no-compressed（原始图需要编码）时，二选一：
|
||||
# opencv-python>=4.8
|
||||
# Pillow>=10.0
|
||||
@@ -0,0 +1,611 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
session_server_node.py — 板子端：会话式「抓帧 / 回传图像 / 回传点云 / 误差跟踪」
|
||||
|
||||
一次运行 = 一个会话。电脑端每一条请求、板子每一条应答都带 session
|
||||
（id / client_ip / board_ip / started_ns），会话之间状态隔离。
|
||||
|
||||
pynng REQ/REP 协议（默认 tcp://0.0.0.0:5555）：
|
||||
|
||||
hello 握手；返回标定分辨率、是否支持板子端计算
|
||||
capture 等一帧同步的（图像 + 点云 + 位姿），返回图像字节；
|
||||
本会话记住这一帧（点云、位姿），并清掉旧目标
|
||||
click 默认：电脑只发 (u, v)，板子用本地点云算
|
||||
p_cam / p_lidar / p_map，并把它记成目标（点云不出板）
|
||||
cloud 可选（--compute pc）：返回 capture 那一帧的点云
|
||||
set_target 可选（--compute pc）：电脑算好的 p_map 回传
|
||||
status 返回当前 pos / yaw / target / Δ（只给坐标，不给距离）
|
||||
reset 清除本会话的目标
|
||||
|
||||
投影优化：
|
||||
click 第一次会建一个「像素格子 -> 点索引」倒排表（每帧一次），
|
||||
之后同一帧的点击只查周围几个格子，不再全量投影，平面拟合也只在
|
||||
局部候选里做。格子大小 --index-cell（默认 32 px）。
|
||||
|
||||
用法（板子）：
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
python3 session_server_node.py --port 5555
|
||||
|
||||
常用参数：
|
||||
--image-topic /odin1/image/compressed （默认走压缩图，直接转发，不用编码）
|
||||
--no-compressed （强制用原始 /odin1/image，需要 cv2 或 Pillow）
|
||||
--cloud-topic /odin1/cloud_raw
|
||||
--odom-topic /odin1/odometry
|
||||
"""
|
||||
import argparse
|
||||
import threading
|
||||
import time
|
||||
import zlib
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import (QoSProfile, ReliabilityPolicy, HistoryPolicy,
|
||||
qos_profile_sensor_data)
|
||||
from sensor_msgs.msg import Image, CompressedImage, PointCloud2
|
||||
from nav_msgs.msg import Odometry
|
||||
from message_filters import Subscriber, ApproximateTimeSynchronizer
|
||||
|
||||
import pynng
|
||||
|
||||
import wire
|
||||
|
||||
try:
|
||||
import calib
|
||||
CALIB_W = int(calib.IMAGE_WIDTH)
|
||||
CALIB_H = int(calib.IMAGE_HEIGHT)
|
||||
except Exception: # 没放 calib.py 也能跑，只是分辨率用默认值
|
||||
CALIB_W, CALIB_H = 1600, 1296
|
||||
|
||||
# 板子端本地算 p_cam/p_lidar/p_map（默认模式）。没有 target_math.py 时
|
||||
# 自动降级为"电脑端算 + 点云回传"。
|
||||
try:
|
||||
import target_math
|
||||
_HAS_TARGET_MATH = True
|
||||
except Exception:
|
||||
target_math = None
|
||||
_HAS_TARGET_MATH = False
|
||||
|
||||
# sensor_msgs/PointField.datatype -> numpy dtype
|
||||
_PF = {1: np.int8, 2: np.uint8, 3: np.int16, 4: np.uint16,
|
||||
5: np.int32, 6: np.uint32, 7: np.float32, 8: np.float64}
|
||||
_CH = {'bgr8': 3, 'rgb8': 3, 'bgra8': 4, 'rgba8': 4, 'mono8': 1}
|
||||
|
||||
|
||||
def stamp_ns(h):
|
||||
return int(h.stamp.sec) * 1_000_000_000 + int(h.stamp.nanosec)
|
||||
|
||||
|
||||
def unpack_cloud(msg):
|
||||
"""按字段名/偏移解析 PointCloud2（兼容 padding 与非对齐字段）。"""
|
||||
names, formats, offsets = [], [], []
|
||||
for f in msg.fields:
|
||||
if f.datatype not in _PF:
|
||||
continue
|
||||
names.append(f.name)
|
||||
formats.append(_PF[f.datatype])
|
||||
offsets.append(f.offset)
|
||||
dt = np.dtype({'names': names, 'formats': formats,
|
||||
'offsets': offsets, 'itemsize': msg.point_step})
|
||||
arr = np.frombuffer(msg.data, dtype=dt, count=msg.width * msg.height)
|
||||
return arr, names
|
||||
|
||||
|
||||
def image_to_array(msg):
|
||||
ch = _CH.get(msg.encoding)
|
||||
if ch is None:
|
||||
return None
|
||||
row = msg.step if msg.step else msg.width * ch
|
||||
buf = np.frombuffer(msg.data, np.uint8).reshape(msg.height, row)
|
||||
img = buf[:, :msg.width * ch].reshape(msg.height, msg.width, ch)
|
||||
return img.copy()
|
||||
|
||||
|
||||
def quat_to_yaw(x, y, z, w):
|
||||
return float(np.arctan2(2.0 * (w * z + x * y),
|
||||
1.0 - 2.0 * (y * y + z * z)))
|
||||
|
||||
|
||||
def quat_to_rot(q):
|
||||
x, y, z, w = [float(v) for v in q]
|
||||
return np.array([
|
||||
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
||||
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
||||
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
||||
], dtype=np.float64)
|
||||
|
||||
|
||||
class SessionServer(Node):
|
||||
def __init__(self, args):
|
||||
super().__init__('session_server')
|
||||
self.args = args
|
||||
self._stop = False
|
||||
self._rep = None
|
||||
|
||||
self._lock = threading.Lock()
|
||||
self._cv = threading.Condition(self._lock)
|
||||
self._latest = None
|
||||
self._odom_hist = deque(maxlen=400)
|
||||
self._sessions = {}
|
||||
|
||||
self._setup_subs()
|
||||
|
||||
self.get_logger().info(
|
||||
'会话服务: image=%s cloud=%s odom=%s'
|
||||
% (args.image_topic, args.cloud_topic, args.odom_topic))
|
||||
self.get_logger().info(
|
||||
'标定分辨率 %dx%d，等待 %s 的第一帧…' % (CALIB_W, CALIB_H, args.image_topic))
|
||||
|
||||
# ------------------------------------------------------------------ 订阅
|
||||
def _setup_subs(self):
|
||||
img_type = CompressedImage if self.args.image_topic.rstrip('/').endswith('/compressed') else Image
|
||||
a = Subscriber(self, img_type, self.args.image_topic,
|
||||
qos_profile=qos_profile_sensor_data)
|
||||
b = Subscriber(self, PointCloud2, self.args.cloud_topic,
|
||||
qos_profile=qos_profile_sensor_data)
|
||||
self.sync = ApproximateTimeSynchronizer([a, b],
|
||||
queue_size=self.args.queue,
|
||||
slop=self.args.slop)
|
||||
self.sync.registerCallback(self.on_pair)
|
||||
|
||||
# 里程计：best-effort 订阅既能接 reliable 也能接 best-effort 的发布者
|
||||
odom_qos = QoSProfile(depth=100,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST)
|
||||
self.create_subscription(Odometry, self.args.odom_topic,
|
||||
self.on_odom, odom_qos)
|
||||
|
||||
def on_odom(self, msg):
|
||||
p = msg.pose.pose.position
|
||||
q = msg.pose.pose.orientation
|
||||
rec = {
|
||||
'stamp_ns': stamp_ns(msg.header),
|
||||
'recv_mono': time.monotonic(),
|
||||
'pos': (float(p.x), float(p.y), float(p.z)),
|
||||
'quat': (float(q.x), float(q.y), float(q.z), float(q.w)),
|
||||
'frame_id': msg.header.frame_id,
|
||||
'child_frame_id': msg.child_frame_id,
|
||||
}
|
||||
with self._lock:
|
||||
self._odom_hist.append(rec)
|
||||
|
||||
def on_pair(self, img_msg, pc_msg):
|
||||
"""图像 + 点云时间对齐后，把这一帧放进 latest。"""
|
||||
arr, names = unpack_cloud(pc_msg)
|
||||
if not all(k in names for k in ('x', 'y', 'z')):
|
||||
self.get_logger().warn('点云没有 x/y/z 字段，实际：%s' % names)
|
||||
return
|
||||
|
||||
xyz = np.stack([arr['x'], arr['y'], arr['z']], axis=-1).astype(np.float32)
|
||||
conf = np.ascontiguousarray(arr['confidence']) if 'confidence' in names else None
|
||||
cloud_stamp = stamp_ns(pc_msg.header)
|
||||
img_stamp = stamp_ns(img_msg.header)
|
||||
|
||||
if isinstance(img_msg, CompressedImage):
|
||||
frame = {
|
||||
'kind': 'compressed',
|
||||
'blob': bytes(img_msg.data),
|
||||
'format': str(img_msg.format or 'jpeg'),
|
||||
'width': None,
|
||||
'height': None,
|
||||
}
|
||||
else:
|
||||
img = image_to_array(img_msg)
|
||||
if img is None:
|
||||
self.get_logger().warn('不支持的图像编码：%s' % img_msg.encoding)
|
||||
return
|
||||
frame = {
|
||||
'kind': 'raw',
|
||||
'img': img,
|
||||
'format': None,
|
||||
'width': int(img_msg.width),
|
||||
'height': int(img_msg.height),
|
||||
'encoding': img_msg.encoding,
|
||||
}
|
||||
|
||||
frame.update({
|
||||
'xyz': xyz,
|
||||
'conf': conf,
|
||||
'cloud_stamp_ns': cloud_stamp,
|
||||
'img_stamp_ns': img_stamp,
|
||||
'cloud_frame_id': pc_msg.header.frame_id,
|
||||
'recv_mono': time.monotonic(),
|
||||
})
|
||||
|
||||
with self._cv:
|
||||
self._latest = frame
|
||||
self._cv.notify_all()
|
||||
|
||||
self.get_logger().debug(
|
||||
'同步帧: 图像 %s, 点云 %d 点, 时间差 %.2f ms'
|
||||
% (frame['kind'], xyz.shape[0], (cloud_stamp - img_stamp) / 1e6))
|
||||
|
||||
# ------------------------------------------------------------------ 帧工具
|
||||
def wait_for_frame(self, after_mono, timeout):
|
||||
"""等一帧 recv 时间 >= after_mono 的帧；超时返回当前最新帧。"""
|
||||
deadline = time.monotonic() + timeout
|
||||
with self._cv:
|
||||
while True:
|
||||
frame = self._latest
|
||||
if frame is not None and frame['recv_mono'] >= after_mono:
|
||||
return dict(frame)
|
||||
left = deadline - time.monotonic()
|
||||
if left <= 0:
|
||||
return dict(frame) if frame is not None else None
|
||||
self._cv.wait(min(left, 0.2))
|
||||
|
||||
def _nearest_odom_locked(self, stamp_ns_value):
|
||||
if not self._odom_hist:
|
||||
return None
|
||||
return min(self._odom_hist, key=lambda r: abs(r['stamp_ns'] - stamp_ns_value))
|
||||
|
||||
def frame_odom(self, frame):
|
||||
with self._lock:
|
||||
od = self._nearest_odom_locked(frame['cloud_stamp_ns'])
|
||||
if od is None:
|
||||
return None
|
||||
return {
|
||||
'stamp_ns': int(od['stamp_ns']),
|
||||
'position': [float(v) for v in od['pos']],
|
||||
'orientation_xyzw': [float(v) for v in od['quat']],
|
||||
'frame_id': od['frame_id'],
|
||||
'child_frame_id': od['child_frame_id'],
|
||||
'delta_ms': (od['stamp_ns'] - frame['cloud_stamp_ns']) / 1e6,
|
||||
}
|
||||
|
||||
def encode_image(self, frame):
|
||||
"""返回 (bytes, format)。压缩图直接转发；原图才编码。"""
|
||||
if frame.get('kind') == 'compressed':
|
||||
return frame['blob'], frame.get('format') or 'jpeg'
|
||||
|
||||
img = frame['img']
|
||||
try:
|
||||
import cv2
|
||||
ok, buf = cv2.imencode(
|
||||
'.jpg', img,
|
||||
[int(cv2.IMWRITE_JPEG_QUALITY), int(self.args.jpeg_quality)])
|
||||
if ok:
|
||||
return buf.tobytes(), 'jpeg'
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
try:
|
||||
import io
|
||||
from PIL import Image as PILImage
|
||||
enc = frame.get('encoding', 'bgr8')
|
||||
rgb = img[..., ::-1] if enc.startswith('bgr') else img[..., :3]
|
||||
b = io.BytesIO()
|
||||
PILImage.fromarray(np.ascontiguousarray(rgb)).save(
|
||||
b, format='JPEG', quality=int(self.args.jpeg_quality))
|
||||
return b.getvalue(), 'jpeg'
|
||||
except Exception as exc:
|
||||
raise RuntimeError('原图编码失败，需要 OpenCV 或 Pillow：%s' % exc)
|
||||
|
||||
# ------------------------------------------------------------------ 会话
|
||||
def _prune(self):
|
||||
now = time.monotonic()
|
||||
for sid in list(self._sessions):
|
||||
if now - self._sessions[sid]['last'] > self.args.session_ttl:
|
||||
del self._sessions[sid]
|
||||
|
||||
def handle_hello(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
self._sessions.setdefault(sid, {
|
||||
'session': hdr['session'], 'frame': None,
|
||||
'target': None, 'target_info': None,
|
||||
'last': time.monotonic(),
|
||||
})
|
||||
self._sessions[sid]['last'] = time.monotonic()
|
||||
return {
|
||||
'ok': True, 'type': 'hello',
|
||||
'node': self.get_name(),
|
||||
'calib_width': CALIB_W, 'calib_height': CALIB_H,
|
||||
'board_can_compute': bool(_HAS_TARGET_MATH),
|
||||
'compute_modes': (['board', 'pc'] if _HAS_TARGET_MATH else ['pc']),
|
||||
'topics': {'image': self.args.image_topic,
|
||||
'cloud': self.args.cloud_topic,
|
||||
'odom': self.args.odom_topic},
|
||||
}
|
||||
|
||||
def handle_capture(self, hdr):
|
||||
self._prune()
|
||||
t_req = time.monotonic()
|
||||
frame = self.wait_for_frame(t_req - self.args.max_frame_age,
|
||||
self.args.frame_timeout)
|
||||
if frame is None:
|
||||
return {'ok': False, 'type': 'frame',
|
||||
'error': '还没有收到同步的图像+点云'}, b''
|
||||
|
||||
blob, fmt = self.encode_image(frame)
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
self._sessions[sid] = {
|
||||
'session': hdr['session'], 'frame': frame,
|
||||
'target': None, 'target_info': None,
|
||||
'last': time.monotonic(),
|
||||
}
|
||||
|
||||
rep = {
|
||||
'ok': True, 'type': 'frame',
|
||||
'image_format': fmt,
|
||||
'image_bytes': len(blob),
|
||||
'image_width': frame.get('width'),
|
||||
'image_height': frame.get('height'),
|
||||
'calib_width': CALIB_W, 'calib_height': CALIB_H,
|
||||
'image_stamp_ns': int(frame['img_stamp_ns']),
|
||||
'cloud_stamp_ns': int(frame['cloud_stamp_ns']),
|
||||
'stamp_delta_ms': (frame['cloud_stamp_ns'] - frame['img_stamp_ns']) / 1e6,
|
||||
'n_points': int(frame['xyz'].shape[0]),
|
||||
'board_can_compute': bool(_HAS_TARGET_MATH),
|
||||
'odom': self.frame_odom(frame),
|
||||
}
|
||||
return rep, blob
|
||||
|
||||
def handle_click(self, hdr):
|
||||
"""板子端算：像素 -> p_cam / p_lidar / p_map，并把它记成目标。
|
||||
|
||||
这是默认模式：点云不出板，电脑只发 (u, v)。
|
||||
"""
|
||||
if not _HAS_TARGET_MATH:
|
||||
return {'ok': False, 'type': 'target',
|
||||
'error': '板子没装 target_math.py，请电脑端用 --compute pc'}, b''
|
||||
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
frame = None if s is None else s.get('frame')
|
||||
if frame is None:
|
||||
return {'ok': False, 'type': 'target', 'error': '先 capture'}, b''
|
||||
|
||||
u = float(hdr['u'])
|
||||
v = float(hdr['v'])
|
||||
radius = float(hdr.get('radius', self.args.radius))
|
||||
fit = bool(hdr.get('fit_plane', True))
|
||||
|
||||
# 每帧只建一次索引；同一帧后续点击只查局部格子
|
||||
with self._lock:
|
||||
index = None if s is None else s.get('index')
|
||||
if index is None:
|
||||
index = target_math.TargetIndex(frame['xyz'], conf=frame.get('conf'),
|
||||
cell=self.args.index_cell)
|
||||
with self._lock:
|
||||
if s is not None:
|
||||
s['index'] = index
|
||||
|
||||
res = index.locate(u, v, radius=radius, fit_plane=fit)
|
||||
out = {
|
||||
'ok': True, 'type': 'target',
|
||||
'found': bool(res['found']),
|
||||
'u': u, 'v': v,
|
||||
'n_valid': int(res['n_valid']),
|
||||
'n_cand': int(res['n_cand']),
|
||||
'plane_used': bool(res['plane_used']),
|
||||
'multi_layer': bool(res['multi_layer']),
|
||||
}
|
||||
if not res['found']:
|
||||
return out, b''
|
||||
|
||||
out.update({
|
||||
'p_cam': [float(x) for x in res['p_cam']],
|
||||
'p_lidar': [float(x) for x in res['p_lidar']],
|
||||
'hit_u': float(res['hit_u']),
|
||||
'hit_v': float(res['hit_v']),
|
||||
'px_err': float(res['px_err']),
|
||||
'confidence': (None if res.get('confidence') is None
|
||||
else float(res['confidence'])),
|
||||
})
|
||||
|
||||
od = self.frame_odom(frame)
|
||||
if od is None:
|
||||
out['p_map'] = None
|
||||
out['error'] = '本帧没有里程计位姿'
|
||||
return out, b''
|
||||
|
||||
R = quat_to_rot(od['orientation_xyzw'])
|
||||
t = np.asarray(od['position'], dtype=np.float64)
|
||||
pm = R @ np.asarray(res['p_lidar'], dtype=np.float64) + t
|
||||
|
||||
out['p_map'] = [float(x) for x in pm]
|
||||
out['odom'] = od
|
||||
with self._lock:
|
||||
s['target'] = pm
|
||||
s['target_info'] = out
|
||||
s['last'] = time.monotonic()
|
||||
return out, b''
|
||||
|
||||
def handle_cloud(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
frame = None if s is None else s.get('frame')
|
||||
if frame is None:
|
||||
return {'ok': False, 'type': 'cloud', 'error': '先 capture'}, b''
|
||||
|
||||
xyz = np.asarray(frame['xyz'], dtype='<f4')
|
||||
body = zlib.compress(xyz.tobytes(), 1)
|
||||
rep = {
|
||||
'ok': True, 'type': 'cloud',
|
||||
'encoding': 'zlib_f32',
|
||||
'n_points': int(xyz.shape[0]),
|
||||
'shape': [int(xyz.shape[0]), 3],
|
||||
'raw_bytes': int(xyz.nbytes),
|
||||
'zlib_bytes': int(len(body)),
|
||||
'cloud_stamp_ns': int(frame['cloud_stamp_ns']),
|
||||
}
|
||||
return rep, body
|
||||
|
||||
def handle_set_target(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
if s is None or s.get('frame') is None:
|
||||
return {'ok': False, 'type': 'target', 'error': '先 capture'}, b''
|
||||
pm = hdr.get('p_map')
|
||||
if not pm or len(pm) != 3:
|
||||
return {'ok': False, 'type': 'target',
|
||||
'error': 'p_map 需要 3 个数'}, b''
|
||||
s['target'] = np.asarray([float(pm[0]), float(pm[1]), float(pm[2])],
|
||||
dtype=np.float64)
|
||||
s['target_info'] = {k: hdr.get(k) for k in ('u', 'v', 'p_cam', 'p_lidar', 'p_map')}
|
||||
s['last'] = time.monotonic()
|
||||
target = [float(v) for v in s['target']]
|
||||
return {'ok': True, 'type': 'target', 'target': target}, b''
|
||||
|
||||
def handle_status(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
od = self._odom_hist[-1] if self._odom_hist else None
|
||||
target = None if s is None else s.get('target')
|
||||
|
||||
out = {'ok': True, 'type': 'status', 'has_target': target is not None}
|
||||
if od is None:
|
||||
out.update({'valid': False, 'error': '还没有里程计'})
|
||||
return out, b''
|
||||
|
||||
age = time.monotonic() - od['recv_mono']
|
||||
pos = np.asarray(od['pos'], dtype=np.float64)
|
||||
out.update({
|
||||
'valid': bool(age <= self.args.odom_timeout),
|
||||
'pos': [float(v) for v in pos],
|
||||
'yaw_deg': float(np.rad2deg(quat_to_yaw(*od['quat']))),
|
||||
'odom_age_s': float(age),
|
||||
'odom_frame': od['frame_id'],
|
||||
'odom_child': od['child_frame_id'],
|
||||
})
|
||||
if target is not None:
|
||||
d = pos - np.asarray(target, dtype=np.float64)
|
||||
out['target'] = [float(v) for v in target]
|
||||
out['delta'] = [float(v) for v in d] # 只给坐标差，不给距离
|
||||
return out, b''
|
||||
|
||||
def handle_reset(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
if s is not None:
|
||||
s['target'] = None
|
||||
s['target_info'] = None
|
||||
s['last'] = time.monotonic()
|
||||
return {'ok': True, 'type': 'reset'}, b''
|
||||
|
||||
def dispatch(self, typ, hdr):
|
||||
if typ == 'hello':
|
||||
return self.handle_hello(hdr), b''
|
||||
if typ == 'capture':
|
||||
return self.handle_capture(hdr)
|
||||
if typ == 'click':
|
||||
return self.handle_click(hdr)
|
||||
if typ == 'cloud':
|
||||
return self.handle_cloud(hdr)
|
||||
if typ == 'set_target':
|
||||
return self.handle_set_target(hdr)
|
||||
if typ == 'status':
|
||||
return self.handle_status(hdr)
|
||||
if typ == 'reset':
|
||||
return self.handle_reset(hdr)
|
||||
return {'ok': False, 'type': typ, 'error': 'unknown type'}, b''
|
||||
|
||||
# ------------------------------------------------------------------ 服务循环
|
||||
def serve(self):
|
||||
addr = 'tcp://0.0.0.0:%d' % self.args.port
|
||||
self._rep = pynng.Rep0(listen=addr)
|
||||
self._rep.recv_max_size = 32 * 1024 * 1024
|
||||
self._rep.recv_timeout = 500
|
||||
self.get_logger().info('pynng REP 监听 %s' % addr)
|
||||
|
||||
while not self._stop:
|
||||
try:
|
||||
data = self._rep.recv()
|
||||
except pynng.Timeout:
|
||||
continue
|
||||
except pynng.Closed:
|
||||
break
|
||||
except Exception as exc:
|
||||
self.get_logger().error('recv 失败: %s' % exc)
|
||||
continue
|
||||
|
||||
hdr = {}
|
||||
try:
|
||||
hdr, _ = wire.unpack(data)
|
||||
typ = hdr.get('type', '?')
|
||||
sess = hdr.get('session') or {}
|
||||
sid = sess.get('id', '?')
|
||||
reply, out_blob = self.dispatch(typ, hdr)
|
||||
reply['session'] = sess # 原样带回，含 IP
|
||||
reply['server_ns'] = time.time_ns()
|
||||
self._rep.send(wire.pack(reply, out_blob))
|
||||
self.get_logger().info(
|
||||
'[%s] %-10s from %s -> ok=%s'
|
||||
% (sid[:8], typ, sess.get('client_ip', '?'), reply.get('ok')))
|
||||
except Exception as exc:
|
||||
try:
|
||||
self._rep.send(wire.pack({
|
||||
'ok': False, 'type': hdr.get('type', 'error'),
|
||||
'error': str(exc),
|
||||
'session': hdr.get('session') or {},
|
||||
'server_ns': time.time_ns(),
|
||||
}))
|
||||
except Exception:
|
||||
pass
|
||||
self.get_logger().error('处理请求失败: %s' % exc)
|
||||
|
||||
def stop(self):
|
||||
self._stop = True
|
||||
if self._rep is not None:
|
||||
try:
|
||||
self._rep.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def parse_args():
|
||||
ap = argparse.ArgumentParser(description='会话式抓帧/定位/误差服务')
|
||||
ap.add_argument('--port', type=int, default=5555)
|
||||
ap.add_argument('--image-topic', default='/odin1/image/compressed')
|
||||
ap.add_argument('--cloud-topic', default='/odin1/cloud_raw')
|
||||
ap.add_argument('--odom-topic', default='/odin1/odometry')
|
||||
ap.add_argument('--no-compressed', action='store_true',
|
||||
help='强制用原始 /odin1/image（需要 cv2 或 Pillow）')
|
||||
ap.add_argument('--slop', type=float, default=0.08, help='图像/点云允许时间差(秒)')
|
||||
ap.add_argument('--queue', type=int, default=20)
|
||||
ap.add_argument('--max-frame-age', type=float, default=0.35,
|
||||
help='抓帧时允许复用多旧的帧(秒)')
|
||||
ap.add_argument('--frame-timeout', type=float, default=2.0,
|
||||
help='等新帧的最长时间(秒)')
|
||||
ap.add_argument('--jpeg-quality', type=int, default=85)
|
||||
ap.add_argument('--radius', type=float, default=10.0,
|
||||
help='click 请求没带半径时的默认候选像素半径')
|
||||
ap.add_argument('--index-cell', type=int, default=32,
|
||||
help='像素格子大小(px)，用于每帧的投影索引')
|
||||
ap.add_argument('--odom-timeout', type=float, default=1.0,
|
||||
help='里程计超过这个时间没更新就标记 invalid(秒)')
|
||||
ap.add_argument('--session-ttl', type=float, default=600.0)
|
||||
args = ap.parse_args()
|
||||
if args.no_compressed:
|
||||
args.image_topic = '/odin1/image'
|
||||
return args
|
||||
|
||||
|
||||
def main():
|
||||
args = parse_args()
|
||||
rclpy.init()
|
||||
node = SessionServer(args)
|
||||
th = threading.Thread(target=node.serve, daemon=True)
|
||||
th.start()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.stop()
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,234 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
target_math.py — 纯 numpy 的"像素 -> 三维目标"定位核心（不依赖 ROS，方便离线测试）。
|
||||
|
||||
输入：一帧点云（雷达系）+ 查询像素
|
||||
输出：目标在相机系/雷达系下的坐标、距离、置信度、是否有多个深度层等
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
import calib
|
||||
|
||||
|
||||
def _fit_plane_ray(P, ray, max_residual=0.05):
|
||||
"""PCA 拟合平面，再让射线与平面求交。返回 (点, 残差)。"""
|
||||
if P.shape[0] < 6:
|
||||
return None, None
|
||||
c = P.mean(axis=0)
|
||||
_, _, vt = np.linalg.svd(P - c)
|
||||
n = vt[2]
|
||||
denom = float(n @ ray)
|
||||
if abs(denom) < 1e-6:
|
||||
return None, None
|
||||
lam = float(n @ c) / denom
|
||||
if lam <= 0:
|
||||
return None, None
|
||||
p = lam * ray
|
||||
residual = float(np.sqrt(np.mean(((P - c) @ n) ** 2)))
|
||||
if residual > max_residual:
|
||||
return None, residual
|
||||
return p, residual
|
||||
|
||||
|
||||
def locate(xyz, query_u, query_v, conf=None, radius=10.0, depth_band=0.6,
|
||||
fit_plane=True, max_residual=0.05, min_conf=None, min_z=0.05):
|
||||
"""在点云里找查询像素方向上最近的可见点。
|
||||
|
||||
参数
|
||||
xyz : (N,3) 雷达系点云
|
||||
query_u/v : 查询像素
|
||||
conf : (N,) 置信度，可选
|
||||
radius : 候选像素半径
|
||||
返回 dict（未找到时 found=False）
|
||||
"""
|
||||
xyz = np.asarray(xyz, dtype=np.float64)
|
||||
good = np.isfinite(xyz).all(axis=1) & (np.abs(xyz).sum(axis=1) > 1e-6)
|
||||
if conf is not None and min_conf is not None:
|
||||
good &= np.asarray(conf) >= min_conf
|
||||
idx = np.where(good)[0]
|
||||
pts = xyz[good]
|
||||
|
||||
p_cam = calib.lidar_to_cam(pts)
|
||||
u, v, ok = calib.project_cam_to_pixel(p_cam)
|
||||
|
||||
dpx = np.hypot(u - query_u, v - query_v)
|
||||
cand = ok & (dpx <= radius) & (p_cam[:, 2] > min_z)
|
||||
|
||||
rng = np.linalg.norm(p_cam, axis=1)
|
||||
res = {
|
||||
'found': False, 'n_valid': int(pts.shape[0]), 'n_cand': int(cand.sum()),
|
||||
'radius': float(radius), 'query_u': float(query_u), 'query_v': float(query_v),
|
||||
}
|
||||
if not cand.any():
|
||||
return res
|
||||
|
||||
cand_idx = np.where(cand)[0]
|
||||
order = cand_idx[np.argsort(rng[cand_idx])]
|
||||
sel = int(order[0])
|
||||
|
||||
ray = calib.pixel_to_ray(query_u, query_v)
|
||||
p_near = p_cam[sel]
|
||||
|
||||
# 深度分层：候选距离跨度大 => 可能擦过边缘
|
||||
r_sorted = np.sort(rng[cand_idx])
|
||||
multi = bool(len(r_sorted) > 3 and (r_sorted[-1] - r_sorted[0]) > 2.0)
|
||||
|
||||
p_final = p_near
|
||||
plane_used = False
|
||||
residual = None
|
||||
if fit_plane:
|
||||
band = p_cam[cand_idx][rng[cand_idx] <= rng[sel] + depth_band]
|
||||
p_fit, residual = _fit_plane_ray(band, ray, max_residual)
|
||||
if p_fit is not None:
|
||||
p_final = p_fit
|
||||
plane_used = True
|
||||
|
||||
res.update({
|
||||
'found': True,
|
||||
'p_cam': p_final,
|
||||
'range': float(np.linalg.norm(p_final)),
|
||||
'z': float(p_final[2]),
|
||||
'p_lidar': calib.cam_to_lidar(p_final)[0],
|
||||
'hit_u': float(u[sel]),
|
||||
'hit_v': float(v[sel]),
|
||||
'px_err': float(dpx[sel]),
|
||||
'lidar_index': int(idx[sel]),
|
||||
'plane_used': plane_used,
|
||||
'plane_residual': residual,
|
||||
'multi_layer': multi,
|
||||
'cand_ranges': rng[cand_idx],
|
||||
'confidence': float(conf[idx[sel]]) if conf is not None else None,
|
||||
})
|
||||
return res
|
||||
|
||||
|
||||
class TargetIndex:
|
||||
"""一帧点云的「像素格子 -> 点索引」倒排表。
|
||||
|
||||
为什么需要：
|
||||
locate() 每次点击都会把 N 个点全量投影一遍。N≈5 万时一次几毫秒到
|
||||
几十毫秒，点一次无所谓；但同一帧反复点、或在板子上跑时，这部分是
|
||||
重复劳动。这里在【每帧】只做一次全量投影 + 分桶，之后每次点击只
|
||||
检查查询像素周围几个格子里的点，平面拟合也只在局部候选里做。
|
||||
|
||||
用法：
|
||||
idx = TargetIndex(xyz, conf=conf) # 每帧建一次
|
||||
res = idx.locate(u, v, radius=10) # 每次点击，返回结构和 locate 一样
|
||||
|
||||
注意：
|
||||
- 桶按点的像素坐标分，格子越大，查询时扫到的点越多；默认 32 px。
|
||||
- 只索引落在图像内的点；图像外的点本来也不会被点到。
|
||||
"""
|
||||
|
||||
def __init__(self, xyz, conf=None, cell=32, min_z=0.05):
|
||||
xyz = np.asarray(xyz, dtype=np.float64)
|
||||
good = np.isfinite(xyz).all(axis=1) & (np.abs(xyz).sum(axis=1) > 1e-6)
|
||||
self.idx = np.where(good)[0]
|
||||
self.xyz = xyz[good]
|
||||
self.conf = None if conf is None else np.asarray(conf)[good]
|
||||
|
||||
self.p_cam = calib.lidar_to_cam(self.xyz)
|
||||
self.u, self.v, ok = calib.project_cam_to_pixel(self.p_cam)
|
||||
self.rng = np.linalg.norm(self.p_cam, axis=1)
|
||||
self.ok = ok & (self.p_cam[:, 2] > float(min_z))
|
||||
|
||||
self.cell = max(4, int(cell))
|
||||
self.nx = int(np.ceil(calib.IMAGE_WIDTH / float(self.cell)))
|
||||
self.ny = int(np.ceil(calib.IMAGE_HEIGHT / float(self.cell)))
|
||||
|
||||
sel = np.where(self.ok)[0]
|
||||
if sel.size:
|
||||
cx = np.clip((self.u[sel] // self.cell).astype(np.int64),
|
||||
0, self.nx - 1)
|
||||
cy = np.clip((self.v[sel] // self.cell).astype(np.int64),
|
||||
0, self.ny - 1)
|
||||
keys = cy * self.nx + cx
|
||||
order = np.argsort(keys, kind='stable')
|
||||
keys = keys[order]
|
||||
sel = sel[order]
|
||||
uniq, starts = np.unique(keys, return_index=True)
|
||||
ends = np.append(starts[1:], keys.size)
|
||||
self.buckets = {int(k): sel[s:e]
|
||||
for k, s, e in zip(uniq, starts, ends)}
|
||||
else:
|
||||
self.buckets = {}
|
||||
|
||||
def _gather(self, query_u, query_v, radius):
|
||||
c = self.cell
|
||||
x0 = max(0, int(np.floor((query_u - radius) / c)))
|
||||
x1 = min(self.nx - 1, int(np.floor((query_u + radius) / c)))
|
||||
y0 = max(0, int(np.floor((query_v - radius) / c)))
|
||||
y1 = min(self.ny - 1, int(np.floor((query_v + radius) / c)))
|
||||
|
||||
parts = []
|
||||
for cy in range(y0, y1 + 1):
|
||||
base = cy * self.nx
|
||||
for cx in range(x0, x1 + 1):
|
||||
arr = self.buckets.get(base + cx)
|
||||
if arr is not None and arr.size:
|
||||
parts.append(arr)
|
||||
if not parts:
|
||||
return np.empty(0, dtype=np.int64)
|
||||
return np.concatenate(parts)
|
||||
|
||||
def locate(self, query_u, query_v, radius=10.0, depth_band=0.6,
|
||||
fit_plane=True, max_residual=0.05, min_conf=None):
|
||||
"""局部查找，返回与 locate() 相同的字段。"""
|
||||
query_u = float(query_u)
|
||||
query_v = float(query_v)
|
||||
|
||||
res = {
|
||||
'found': False, 'n_valid': int(self.idx.size),
|
||||
'n_cand': 0, 'radius': float(radius),
|
||||
'query_u': query_u, 'query_v': query_v,
|
||||
'indexed': True,
|
||||
}
|
||||
|
||||
cand = self._gather(query_u, query_v, radius)
|
||||
if cand.size == 0:
|
||||
return res
|
||||
|
||||
dpx = np.hypot(self.u[cand] - query_u, self.v[cand] - query_v)
|
||||
keep = dpx <= radius
|
||||
if min_conf is not None and self.conf is not None:
|
||||
keep &= self.conf[cand] >= min_conf
|
||||
cand = cand[keep]
|
||||
dpx = dpx[keep]
|
||||
res['n_cand'] = int(cand.size)
|
||||
if cand.size == 0:
|
||||
return res
|
||||
|
||||
# 距离最近的点
|
||||
order = np.argsort(self.rng[cand])
|
||||
cand = cand[order]
|
||||
dpx = dpx[order]
|
||||
sel = int(cand[0])
|
||||
|
||||
ray = calib.pixel_to_ray(query_u, query_v)
|
||||
r_sorted = np.sort(self.rng[cand])
|
||||
multi = bool(r_sorted.size > 3 and (r_sorted[-1] - r_sorted[0]) > 2.0)
|
||||
|
||||
p_final = self.p_cam[sel]
|
||||
plane_used = False
|
||||
residual = None
|
||||
if fit_plane:
|
||||
band = self.p_cam[cand[self.rng[cand] <= self.rng[sel] + depth_band]]
|
||||
p_fit, residual = _fit_plane_ray(band, ray, max_residual)
|
||||
if p_fit is not None:
|
||||
p_final = p_fit
|
||||
plane_used = True
|
||||
|
||||
res.update({
|
||||
'found': True,
|
||||
'p_cam': p_final,
|
||||
'p_lidar': calib.cam_to_lidar(p_final)[0],
|
||||
'hit_u': float(self.u[sel]),
|
||||
'hit_v': float(self.v[sel]),
|
||||
'px_err': float(dpx[0]),
|
||||
'lidar_index': int(self.idx[sel]),
|
||||
'plane_used': plane_used,
|
||||
'plane_residual': residual,
|
||||
'multi_layer': multi,
|
||||
'confidence': None if self.conf is None else float(self.conf[sel]),
|
||||
})
|
||||
return res
|
||||
@@ -0,0 +1,65 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
wire.py — 会话消息的打包 / 解包。
|
||||
|
||||
pynng 一条消息只能是单块二进制，所以这里做一层很简单的外壳：
|
||||
b"LS01" | u32 头长度 | JSON 头（UTF-8） | 二进制体
|
||||
|
||||
每个头里都带 session：
|
||||
{
|
||||
"id": "<一次运行一个 uuid>",
|
||||
"client_ip": "192.168.43.23", # 电脑自己的地址
|
||||
"board_ip": "192.168.43.50", # 板子的地址
|
||||
"started_ns": 1712345678901234567,
|
||||
"note": "..."
|
||||
}
|
||||
|
||||
这样每一条请求和应答都能看出属于哪个会话、从哪台机器来。
|
||||
"""
|
||||
import json
|
||||
import struct
|
||||
import time
|
||||
import uuid
|
||||
|
||||
MAGIC = b"LS01"
|
||||
_LEN = struct.Struct("<I")
|
||||
|
||||
|
||||
def new_session(board_ip, client_ip, note=""):
|
||||
"""新建一个会话（电脑端每次启动调一次）。"""
|
||||
return {
|
||||
"id": uuid.uuid4().hex,
|
||||
"client_ip": str(client_ip),
|
||||
"board_ip": str(board_ip),
|
||||
"started_ns": time.time_ns(),
|
||||
"note": str(note),
|
||||
}
|
||||
|
||||
|
||||
def pack(header, blob=b""):
|
||||
"""dict + 二进制体 -> bytes。"""
|
||||
h = json.dumps(header, separators=(",", ":"), ensure_ascii=False).encode("utf-8")
|
||||
return MAGIC + _LEN.pack(len(h)) + h + bytes(blob)
|
||||
|
||||
|
||||
def unpack(data):
|
||||
"""bytes -> (dict, 二进制体)。"""
|
||||
if len(data) < 8 or data[:4] != MAGIC:
|
||||
raise ValueError("bad frame magic")
|
||||
n = _LEN.unpack_from(data, 4)[0]
|
||||
if len(data) < 8 + n:
|
||||
raise ValueError("truncated header")
|
||||
header = json.loads(data[8:8 + n].decode("utf-8"))
|
||||
return header, data[8 + n:]
|
||||
|
||||
|
||||
def session_id(header):
|
||||
return (header.get("session") or {}).get("id")
|
||||
|
||||
|
||||
def check_session(header, session):
|
||||
"""应答必须属于当前会话，防止串台。"""
|
||||
got = session_id(header)
|
||||
if got != session["id"]:
|
||||
raise RuntimeError("session mismatch: got %r, want %r" % (got, session["id"]))
|
||||
Reference in New Issue
Block a user