import numpy as np
import cv2

==================== load ====================

def load_bin(bin_file):
points = np.fromfile(bin_file, dtype=np.float32)[1:].reshape(-1, 4)
mask = np.all(~np.isnan(points), axis=1)
points = points[mask]
points = points[1 : ((len(points) - 1) // 4) * 4 + 1].reshape(-1, 4)
points = points[~np.isnan(points[:, 0]), :]
return points

==================== single-axis ====================

def apply_yaw_perturb(pts, delta_yaw_rad):
c, s = np.cos(delta_yaw_rad), np.sin(delta_yaw_rad)
R = np.array([[ c, -s, 0],
[ s, c, 0],
[ 0, 0, 1]])
pts_new = pts.copy()
pts_new[:, :3] = pts[:, :3] @ R.T
return pts_new

def apply_roll_perturb(pts, delta_roll_rad):
c, s = np.cos(delta_roll_rad), np.sin(delta_roll_rad)
R = np.array([[1, 0, 0],
[0, c, -s],
[0, s, c]])
pts_new = pts.copy()
pts_new[:, :3] = pts[:, :3] @ R.T
return pts_new

def apply_pitch_perturb(pts, delta_pitch_rad):
c, s = np.cos(delta_pitch_rad), np.sin(delta_pitch_rad)
R = np.array([[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]])
pts_new = pts.copy()
pts_new[:, :3] = pts[:, :3] @ R.T
return pts_new

==================== NEW: rpy together ====================

def apply_rpy_perturb(pts, delta_roll, delta_pitch, delta_yaw):
"""
同时绕 X(roll) / Y(pitch) / Z(yaw) 旋转
顺序: roll -> pitch -> yaw
"""
cr, sr = np.cos(delta_roll), np.sin(delta_roll)
cp, sp = np.cos(delta_pitch), np.sin(delta_pitch)
cy, sy = np.cos(delta_yaw), np.sin(delta_yaw)

Rx = np.array([[1, 0, 0],
[0, cr, -sr],
[0, sr, cr]])

Ry = np.array([[ cp, 0, sp],
[ 0, 1, 0],
[-sp, 0, cp]])

Rz = np.array([[cy, -sy, 0],
[sy, cy, 0],
[ 0, 0, 1]])

R = Rz @ Ry @ Rx # yaw ∘ pitch ∘ roll

pts_new = pts.copy()
pts_new[:, :3] = pts[:, :3] @ R.T
return pts_new

==================== filter ====================

grid_conf = {
'xbound': [-80.0, 120.0, 1],
'ybound': [-40.0, 40.0, 1],
'zbound': [-2.0, 4.0, 1.0]
}

def filter_pts(pts, tdt_flag=True):
mask = (
(pts[:, 0] > grid_conf['xbound'][0]) &
(pts[:, 1] > grid_conf['ybound'][0]) &
(pts[:, 2] > grid_conf['zbound'][0]) &
(pts[:, 0] < grid_conf['xbound'][1]) &
(pts[:, 1] < grid_conf['ybound'][1]) &
(pts[:, 2] < grid_conf['zbound'][1])
)
return pts[mask]

==================== visualize ====================

def show_bin(pts, save_path="./0.jpg", scale=3.0):
x_range = [0, 120]
y_range = [-40, 40]
h = int((x_range[1] - x_range[0]) * scale)
w = int((y_range[1] - y_range[0]) * scale)
img = np.zeros((h, w), dtype=np.uint8)

for x, y, _, _ in pts:
px = (y_range[1] - y) * scale
py = (x_range[1] - x) * scale
cv2.circle(img, (int(px), int(py)), 1, 255, -1)

cv2.imwrite(save_path, img)

==================== main ====================

path_bin = "./2857062165.500.bin"

pts = load_bin(path_bin)
pts = filter_pts(pts)
pts_xyz1 = np.hstack([pts[:, :3], np.ones((pts.shape[0], 1))])

原始

show_bin(pts_xyz1, "/logs/20260127_tmp/origin.jpg")

单轴

show_bin(apply_yaw_perturb(pts_xyz1, np.deg2rad(15)),
"/logs/20260127_tmp/yaw.jpg")

三轴同时

pts_rpy = apply_rpy_perturb(
pts_xyz1,
delta_roll=np.deg2rad(5),
delta_pitch=np.deg2rad(3),
delta_yaw=np.deg2rad(15)
)
show_bin(pts_rpy, "/logs/20260127_tmp/rpy.jpg")

print("====>> success!")

pitch向下为正，向上为负
yaw向左为正，向右为负
roll向左为正，向右为负