clc; clear; close('all'); addpath('../../ref'); addpath('../../ref/check_sum'); FRAME_HEADER = 'RadarEye'; FRAME_END = 'REND'; MAX_LENGTH = 1000000; isGui = 1; if isGui hGui = struct(); hGui.Figure = figure('WindowState', 'maximized'); hGui.Axes_image = subplot(3, 4, [1,2,5,6]); hGui.hDataImage = imagesc(nan(320, 680, 3)); axis image set(gca, 'XTick', [], 'YTick', []); imageT=title('CAMERA', 'FontSize', 16, 'FontWeight', 'bold'); hGui.Axes_vr = subplot(3,4,[9,10]); hGui.fftv = imagesc(128, 128, nan(128, 128)); grid minor; axis xy; box on; set(gca, 'FontSize', 12); xlabel('VelIdx', 'FontSize', 14); ylabel('RngIdx', 'FontSize', 14); title('fftv', 'FontSize', 16, 'FontWeight', 'bold'); hGui.Axes_xy = subplot(3, 4, [3,7,11]); hGui.hDataPointZY = scatter(nan, nan, 'oG'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('Z (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Point', 'FontSize', 16); hGui.Axes_xy = subplot(3, 4, [4,8,12]); hGui.hDataPointXYPZ = scatter(nan, nan, 'blue','oG'); hold on; hGui.hDataPointXYNZ = scatter(nan, nan, 'red','oG'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('X (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Point', 'FontSize', 16); end if isGui hGui_ref = struct(); hGui_ref.Figure = figure('WindowState', 'maximized'); hGui_ref.Axes_xy = subplot(3, 4, [2,6,10]); hGui_ref.hDataPointZY = scatter(nan, nan, 'oG'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('Z (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Rotation Point', 'FontSize', 16); hGui_ref.Axes_xy = subplot(3, 4, [1,5,9]); hGui_ref.hDataPointZY_before = scatter(nan, nan, 'oG'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('Z (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Raw Point', 'FontSize', 16); hGui_ref.Axes_xy = subplot(3, 4, [4,8,12]); hGui_ref.hDataPointXYPZ = scatter(nan, nan, 'blue','oG'); hold on; hGui_ref.hDataPointXYNZ = scatter(nan, nan, 'red','oG'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('X (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Rotation Point', 'FontSize', 16); hGui_ref.Axes_xy = subplot(3, 4, [3,7,11]); hGui_ref.hDataPointXYPZ_before = scatter(nan, nan, 'blue','oG'); hold on; hGui_ref.hDataPointXYNZ_before = scatter(nan, nan, 'red','oG'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('X (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Raw Point', 'FontSize', 16); end if isGui hGui_filter = struct(); hGui_filter.Figure = figure('WindowState', 'maximized'); hGui_filter.Axes_raw = subplot(1, 2, 1); hGui_filter.raw_hDataPointXYPZ = scatter(nan, nan, 'blue','oG'); hold on; hGui_filter.raw_hDataPointXYNZ = scatter(nan, nan, 'red','oG'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('X (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Raw Point', 'FontSize', 16); hGui_filter.Axes_filter = subplot(1, 2, 2); hGui_filter.filter_hDataPointXYPZ = scatter(nan, nan, 'blue','oG'); hold on; hGui_filter.filter_hDataPointXYNZ = scatter(nan, nan, 'red','oG'); hold on; hGui_filter.cluster_hDataPointXYNZ = scatter(nan, nan, 'green','filled'); axis xy; grid minor; xlim(10*[ -1, +1, ]); ylim(60*[ 0, 1, ]); set(gca, 'FontSize', 12); xlabel('X (m)', 'FontSize', 14); ylabel('Y (m)', 'FontSize', 14); title('Filter Point', 'FontSize', 16); end % ====== 初始化图像 ====== global t0 time_hist dist_hist dist_hist2 hPlot hPlot2 t0 = tic; time_hist = []; dist_hist = []; dist_hist2 = []; figure; hold on; grid on; hPlot = plot(nan, nan, 'r.-', 'LineWidth', 1.5); hPlot2 = plot(nan, nan, 'b.-', 'LineWidth', 1.5); xlabel('Time (s)'); ylabel('Nearest Cluster Distance (m)'); title('Obstacle Distance vs Time'); targetFrame = struct(); ori_point = struct(); cfar_point = struct(); tempdata=[]; % axis image % set(gca, 'XTick', [], 'YTick', []); % title('CAMERA', 'FontSize', 16, 'FontWeight', 'bold'); path = 'C:\Users\VK\Desktop\nationstech\采样分析专用\采样数据\250929\20250929_163854_340'; listBin = dir(fullfile(path, '*.bin')); numBin = length(listBin); fprintf(" numBin %d \r\n",numBin); pc_hist = cell(10,1); % pc_hist{1}: 上一帧(t-1)，pc_hist{2}: 上上帧(t-2) q_hist = cell(10,1); pos_hist = cell(10,1); dataBuffer = []; idxBin = 1; %%%第一版聚类参数 cluster_params_1 = [ ... struct('y_min',0,'y_max',15,'dist_thresh',0.5,'min_points',4); ... struct('y_min',16,'y_max',25,'dist_thresh',0.8,'min_points',3); ... struct('y_min',25,'y_max',50,'dist_thresh',1.5,'min_points',3); ... ]; %%%第二版聚类参数 cluster_params = [ ... struct('y_min',0,'y_max',10,'dist_thresh',0.5,'min_points',4); ... struct('y_min',10.1,'y_max',20,'dist_thresh',1,'min_points',3); ... struct('y_min',20.1,'y_max',35,'dist_thresh',1.8,'min_points',3); ... ]; % 原始定义追踪参数 params_1.match_thresh = 2; % 匹配阈值 (米) params_1.inc_val = 1; % 命中 +1 params_1.dec_val = 1; % 丢失 -1 params_1.min_conf = 3; % 置信度 >=3 才显示 % 第二版定义追踪参数 params_2.match_thresh = 1.3; % 匹配阈值 (米) params_2.inc_val = 1; % 命中 +1 params_2.dec_val = 1; % 丢失 -1 params_2.min_conf = 2; % 置信度 >=2 才显示 all_points = []; points_body_cur =[]; while(idxBin <= numBin) % 读取被测雷达数据文件 pathBin = fullfile(path, listBin(idxBin).name); [ ~, currentTimestr, ~, ] = fileparts(pathBin); currentTime = datetime(currentTimestr, ... 'InputFormat', 'uuuuMMdd_HHmmss_SSSSSS'); % fileBin = fopen(pathBin, 'rb'); % dataBin = uint8(fread(fileBin, inf, 'uint8')).'; % fclose(fileBin); [pose, radarData] = read_bin_with_pose(pathBin); dataBuffer = cat(2, dataBuffer, radarData); quat = pose(1:4); pos = pose(5:7); euler = quat2euler_enu(quat); % 转换为欧拉角 [ listFrame, dataBuffer] = extract_frame(dataBuffer); numFrame = length(listFrame); % disp(currentTime); idxFrame = 1; while (idxFrame <= numFrame) frame = listFrame(idxFrame); % fprintf("Type %d\r\n",frame.Type); paramAlg.ResRng = single(0.6958); paramAlg.ResVel = single(0.3519); switch frame.Type case 7 %点云数据 framePayload = struct(); framePayload.Idx = int32(typecast(frame.Raw(4 + (1:4)), 'uint32')); framePayload.Tick = int32(typecast(frame.Raw(8 + (1:4)), 'uint32')); framePayload.NumPoint = uint32(typecast(frame.Raw(12 + (1:4)), 'uint32')); % ori_point.num = framePayload.NumPoint; ori_point.r=[]; ori_point.v=[]; ori_point.Azi=[]; ori_point.Ele=[]; ori_point.pow=[]; ori_point.x=[]; ori_point.y=[]; ori_point.z=[]; numBytePerPoint = int32(100); idx = 0; % fprintf("Type %d, idx %d , tick %d , num %d\r\n" ... % ,frame.Type,framePayload.Idx,framePayload.Tick,framePayload.NumPoint); for idxPoint = 1:framePayload.NumPoint rIdx = typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(1:4)), 'int32'); vIdx = (typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(5:8)), 'int32') - 65); pow = typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(97:100)), 'int32'); azi = typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(89:92)), 'single'); ele = typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(93:96)), 'single'); ele = -(ele + 0.18); if ele > -15/180*pi && ele < 15/180*pi && powerFilter(rIdx,pow) == 1 %1、去除俯仰角大于20度的点，2、能量过滤 idx = idx + 1; ori_point.r(idx) = single(rIdx)*paramAlg.ResRng; ori_point.v(idx) = single(vIdx)*paramAlg.ResVel; ori_point.Azi(idx) = (-azi); %X值左右反了，水平角取反 ori_point.Ele(idx) = ele+12/180*pi; %补偿雷达安装角度值 % ori_point.Ele(idx) = ele; ori_point.pow(idx) = pow; ori_point.x(idx) = ori_point.r((idx))*cos(ori_point.Ele((idx)))*sin(ori_point.Azi((idx))); ori_point.y(idx) = ori_point.r((idx))*cos(ori_point.Ele((idx)))*cos(ori_point.Azi((idx))); ori_point.z(idx) = ori_point.r((idx))*sin(ori_point.Ele((idx))); tempdata = cat(1,tempdata,[single(idxBin),single(idxFrame),single(rIdx),ori_point.r(idx),ori_point.v(idx),single(ori_point.pow(idx)),ori_point.x(idx),ori_point.y(idx),ori_point.z(idx),ori_point.Azi(idx),ori_point.Ele(idx),single(1)]); % else %记录被过滤掉的点云数据 % r = single(rIdx)*paramAlg.ResRng; % v = single(vIdx)*paramAlg.ResVel; % azi = -azi; % ele = ele+12/180*pi; % x = r*cos(ele)*sin(azi); % y = r*cos(ele)*cos(azi); % z = r*sin(ele); % tempdata = cat(1,tempdata,[single(idxBin),single(idxFrame),single(rIdx),r,v,single(pow),x,y,z,azi,ele,single(0)]); end end ori_point.num = idx; %%%%%%%%%%%%%%%%%%%%%%%%% 点云旋转 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if ori_point.num > 0 % 构造点云矩阵 points_body_cur = [ori_point.x(:), ori_point.y(:), ori_point.z(:)]; % N×3 quat_cur = quat(:)'; % pose(1:4) = [w x y z] pos_cur = pos(:)'; %%%%%% 最新帧点云进行姿态转换 R_body2enu = quat2rotm(quat_cur); % 机体系 -> ENU系 points_enu_raw = (R_body2enu * points_body_cur')'; q_yaw = Quaternion_Enu_ByEuler(-euler(3), 0, 0); % 只保留 yaw R_yaw = quat2rotm(q_yaw); % Yaw 补偿旋转 % 应用 yaw 补偿 points_enu = (R_yaw * points_enu_raw')'; cur_point_enu.x = points_enu(:,1); cur_point_enu.y = points_enu(:,2); cur_point_enu.z = points_enu(:,3); cur_point_enu.num = size(points_enu,1); % filter_condition_cur = (points_enu(:,3) > -1.8) & (points_enu(:,1) < 10.0) ... % & (points_enu(:,1) > -10.0) & (points_enu(:,2) < 30.0); % filtered_points = points_enu(filter_condition_cur, :); % all_points = points_enu; if euler(2) < -0.16 ratio_posz = sum(points_enu(:,3) < 0) / cur_point_enu.num; if ratio_posz >= 0.6 % 过滤掉当前点云 % disp('当前帧点云被丢弃: z>0 占比过高'); % cur_point_enu.num = 0; % cur_point_enu.x = []; % cur_point_enu.y = []; % cur_point_enu.z = []; points_body_cur = 0; all_points = []; else all_points = points_enu; end else all_points = points_enu; end %%%%%% 历史帧点云进行旋转映射 %%% 第一帧历史点云 if ~isempty(pc_hist{1}) quat_hist1 = q_hist{1}; R_body2enu_hist1 = quat2rotm(quat_hist1); % 机体系 -> ENU系 points_hist1 = (R_body2enu_hist1 * pc_hist{1}')'; points_enu_hist1 = points_hist1 + pos_hist{1} - pos_cur; points_rfu_hist1 = (R_yaw * points_enu_hist1')'; all_points = [all_points; points_rfu_hist1]; end %%% 第二帧历史点云 if ~isempty(pc_hist{2}) quat_hist2 = q_hist{2}; R_body2enu_hist2 = quat2rotm(quat_hist2); % 机体系 -> ENU系 points_hist2 = (R_body2enu_hist2 * pc_hist{2}')'; points_enu_hist2 = points_hist2 + pos_hist{2} - pos_cur; points_rfu_hist2 = (R_yaw * points_enu_hist2')'; all_points = [all_points; points_rfu_hist2]; end %%% 第三帧历史点云 if ~isempty(pc_hist{3}) quat_hist3 = q_hist{3}; R_body2enu_hist3 = quat2rotm(quat_hist3); % 机体系 -> ENU系 points_hist3 = (R_body2enu_hist3 * pc_hist{3}')'; points_enu_hist3 = points_hist3 + pos_hist{3} - pos_cur; points_rfu_hist3 = (R_yaw * points_enu_hist3')'; hist3_point.num = size(points_rfu_hist3,1); if(hist3_point.num > 0) all_points = [all_points; points_rfu_hist3]; end end %%% 第四帧历史点云 if ~isempty(pc_hist{4}) quat_hist4 = q_hist{4}; R_body2enu_hist4 = quat2rotm(quat_hist4); % 机体系 -> ENU系 points_hist4 = (R_body2enu_hist4 * pc_hist{4}')'; points_enu_hist4 = points_hist4 + pos_hist{4} - pos_cur; points_rfu_hist4 = (R_yaw * points_enu_hist4')'; hist4_point.num = size(points_rfu_hist4,1); if(hist4_point.num > 0) all_points = [all_points; points_rfu_hist4]; end end %%% 第五帧历史点云 if ~isempty(pc_hist{5}) quat_hist5 = q_hist{5}; R_body2enu_hist5 = quat2rotm(quat_hist5); % 机体系 -> ENU系 points_hist5 = (R_body2enu_hist5 * pc_hist{5}')'; points_enu_hist5 = points_hist5 + pos_hist{5} - pos_cur; points_rfu_hist5 = (R_yaw * points_enu_hist5')'; hist5_point.num = size(points_rfu_hist5,1); if(hist5_point.num > 0) all_points = [all_points; points_rfu_hist5]; end end %%% 第六帧历史点云 if ~isempty(pc_hist{6}) quat_hist6 = q_hist{6}; R_body2enu_hist6 = quat2rotm(quat_hist6); % 机体系 -> ENU系 points_hist6 = (R_body2enu_hist6 * pc_hist{6}')'; points_enu_hist6 = points_hist6 + pos_hist{6} - pos_cur; points_rfu_hist6 = (R_yaw * points_enu_hist6')'; hist6_point.num = size(points_rfu_hist6,1); if(hist6_point.num > 0) all_points = [all_points; points_rfu_hist6]; end end %%% 第七帧历史点云 if ~isempty(pc_hist{7}) quat_hist7 = q_hist{7}; R_body2enu_hist7 = quat2rotm(quat_hist7); % 机体系 -> ENU系 points_hist7 = (R_body2enu_hist7 * pc_hist{7}')'; points_enu_hist7 = points_hist7 + pos_hist{7} - pos_cur; points_rfu_hist7 = (R_yaw * points_enu_hist7')'; hist7_point.num = size(points_rfu_hist7,1); if(hist7_point.num > 0) all_points = [all_points; points_rfu_hist7]; end end %%% 第八帧历史点云 if ~isempty(pc_hist{8}) quat_hist8 = q_hist{8}; R_body2enu_hist8 = quat2rotm(quat_hist8); % 机体系 -> ENU系 points_hist8 = (R_body2enu_hist8 * pc_hist{8}')'; points_enu_hist8 = points_hist8 + pos_hist{8} - pos_cur; points_rfu_hist8 = (R_yaw * points_enu_hist8')'; hist8_point.num = size(points_rfu_hist8,1); if(hist8_point.num > 0) all_points = [all_points; points_rfu_hist8]; end end %%% 第九帧历史点云 if ~isempty(pc_hist{9}) quat_hist9 = q_hist{9}; R_body2enu_hist9 = quat2rotm(quat_hist9); % 机体系 -> ENU系 points_hist9 = (R_body2enu_hist9 * pc_hist{9}')'; points_enu_hist9 = points_hist9 + pos_hist{9} - pos_cur; points_rfu_hist9 = (R_yaw * points_enu_hist9')'; hist9_point.num = size(points_rfu_hist9,1); if(hist9_point.num > 0) all_points = [all_points; points_rfu_hist9]; end end % %%% 第十帧历史点云 % if ~isempty(pc_hist{10}) % quat_hist10 = q_hist{10}; % R_body2enu_hist10 = quat2rotm(quat_hist10); % 机体系 -> ENU系 % points_hist10 = (R_body2enu_hist10 * pc_hist{10}')'; % points_enu_hist10 = points_hist10 + pos_hist{10} - pos_cur; % points_rfu_hist10 = (R_yaw * points_enu_hist10')'; % hist10_point.num = size(points_rfu_hist10,1); % if(hist10_point.num > 0) % all_points = [all_points; points_rfu_hist10]; % end % end if ~isempty(all_points) filter_condition_cur = true(size(all_points,1),1); for i = 1:size(all_points, 1) x_value = all_points(i, 1); % 获取当前点的 x 值 y_value = all_points(i, 2); % 获取当前点的 y 值 z_value = all_points(i, 3); % 获取当前点的 z 值 if y_value >= 35 || y_value <= 0 filter_condition_cur(i) = false; elseif x_value >=10 || x_value <= -10 filter_condition_cur(i) = false; elseif y_value < 5 % 如果 y < 5，则 z > -2 if x_value >= 4 || x_value <= -4 filter_condition_cur(i) = false; end if z_value <= -1.0 filter_condition_cur(i) = false; end elseif y_value >= 5 && y_value < 10 % 如果 5 <= y < 10，则 z > -1.5 if x_value >= 7 || x_value <= -7 filter_condition_cur(i) = false; end if z_value <= -1.2 filter_condition_cur(i) = false; end elseif y_value >= 10 % 如果 y >= 20，则 z > -1 if z_value <= -1.0 filter_condition_cur(i) = false; end end end filtered_points = all_points(filter_condition_cur, :); filtered_point_rfu.x = filtered_points(:, 1); filtered_point_rfu.y = filtered_points(:, 2); filtered_point_rfu.z = filtered_points(:, 3); filtered_point_rfu.num = size(filtered_points, 1); %%% 聚类 clusters_all_1 = multiRangeClustering(filtered_point_rfu, cluster_params_1); clusters_all = multiRangeClustering(filtered_point_rfu, cluster_params); [cx, cy] = updateObstacleTracking(clusters_all, params_2); [cx_1, cy_1] = updateObstacleTracking(clusters_all_1, params_1); % 更新 scatter 数据 set(hGui_filter.cluster_hDataPointXYNZ, 'XData', cx, 'YData', cy); % 找到 y 最小的聚类点 if ~isempty(cy) nearestDist = min(cy); else nearestDist = 0; end if ~isempty(cy_1) nearestDist2 = min(cy_1); else nearestDist2 = 0; end % 当前时间 t = toc(t0); % 追加数据 global time_hist dist_hist dist_hist2 hPlot hPlot2 time_hist(end+1) = t; dist_hist(end+1) = nearestDist; dist_hist2(end+1) = nearestDist2; % 更新曲线 set(hPlot, 'XData', time_hist, 'YData', dist_hist); set(hPlot2, 'XData', time_hist, 'YData', dist_hist2); drawnow limitrate; if(filtered_point_rfu.num > 0) posIdx_filter = filtered_point_rfu.z>=0; negIdx_filter = filtered_point_rfu.z<0; if sum(posIdx_filter) > 0 posZX_filter = filtered_point_rfu.x(posIdx_filter); posZY_filter = filtered_point_rfu.y(posIdx_filter); posSize = 10; else posZX_filter = nan; posZY_filter = nan; posSize = nan; end if sum(negIdx_filter) negZX_filter = filtered_point_rfu.x(negIdx_filter); negZY_filter = filtered_point_rfu.y(negIdx_filter); negSize = 10; else negZX_filter = nan; negZY_filter = nan; negSize = nan; end %显示Z值大于等于0的点 hGui_filter.filter_hDataPointXYPZ.XData = posZX_filter; hGui_filter.filter_hDataPointXYPZ.YData = posZY_filter; hGui_filter.filter_hDataPointXYPZ.SizeData = posSize; %显示Z值小于0的点 hGui_filter.filter_hDataPointXYNZ.XData = negZX_filter; hGui_filter.filter_hDataPointXYNZ.YData = negZY_filter; hGui_filter.filter_hDataPointXYNZ.SizeData = negSize; else hGui_filter.filter_hDataPointXYPZ.XData = nan; hGui_filter.filter_hDataPointXYPZ.YData = nan; hGui_filter.filter_hDataPointXYPZ.SizeData = nan; hGui_filter.filter_hDataPointXYNZ.XData = nan; hGui_filter.filter_hDataPointXYNZ.YData = nan; hGui_filter.filter_hDataPointXYNZ.SizeData = nan; end end %%%%%% 更新历史信息 % pc_hist{10} = pc_hist{9}; pc_hist{9} = pc_hist{8}; pc_hist{8} = pc_hist{7}; pc_hist{7} = pc_hist{6}; pc_hist{6} = pc_hist{5}; pc_hist{5} = pc_hist{4}; pc_hist{4} = pc_hist{3}; pc_hist{3} = pc_hist{2}; pc_hist{2} = pc_hist{1}; % t-2 <- t-1 pc_hist{1} = points_body_cur; % t-1 <- 当前帧body坐标系点云 % q_hist{10} = q_hist{9}; q_hist{9} = q_hist{8}; q_hist{8} = q_hist{7}; q_hist{7} = q_hist{6}; q_hist{6} = q_hist{5}; q_hist{5} = q_hist{4}; q_hist{4} = q_hist{3}; q_hist{3} = q_hist{2}; q_hist{2} = q_hist{1}; % t-2 <- t-1 q_hist{1} = quat_cur; % t-1 <- 当前帧 Q % pos_hist{10} = pos_hist{9}; pos_hist{9} = pos_hist{8}; pos_hist{8} = pos_hist{7}; pos_hist{7} = pos_hist{6}; pos_hist{6} = pos_hist{5}; pos_hist{5} = pos_hist{4}; pos_hist{4} = pos_hist{3}; pos_hist{3} = pos_hist{2}; pos_hist{2} = pos_hist{1}; % t-2 <- t-1 pos_hist{1} = pos_cur; % t-1 <- 当前帧 pos %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 更新旋转后点云信息 if isGui if(cur_point_enu.num > 0) posIdx = cur_point_enu.z>=0; negIdx = cur_point_enu.z<0; if sum(posIdx) > 0 posZX = cur_point_enu.x(posIdx); posZY = cur_point_enu.y(posIdx); posSize = 10; else posZX = nan; posZY = nan; posSize = nan; end if sum(negIdx) negZX = cur_point_enu.x(negIdx); negZY = cur_point_enu.y(negIdx); negSize = 10; else negZX = nan; negZY = nan; negSize = nan; end %显示Z值大于等于0的点 hGui_ref.hDataPointXYPZ.XData = posZX; hGui_ref.hDataPointXYPZ.YData = posZY; hGui_ref.hDataPointXYPZ.SizeData = posSize; %显示Z值小于0的点 hGui_ref.hDataPointXYNZ.XData = negZX; hGui_ref.hDataPointXYNZ.YData = negZY; hGui_ref.hDataPointXYNZ.SizeData = negSize; hGui_ref.hDataPointZY.XData = cur_point_enu.z; hGui_ref.hDataPointZY.YData = cur_point_enu.y; hGui_ref.hDataPointZY.SizeData = 10;%1e-4*double([ ori_point.pow, ] + 1000); else hGui_ref.hDataPointXYPZ.XData = nan; hGui_ref.hDataPointXYPZ.YData = nan; hGui_ref.hDataPointXYPZ.SizeData = nan; hGui_ref.hDataPointXYNZ.XData = nan; hGui_ref.hDataPointXYNZ.YData = nan; hGui_ref.hDataPointXYNZ.SizeData = nan; hGui_ref.hDataPointZY.XData = nan; hGui_ref.hDataPointZY.YData = nan; hGui_ref.hDataPointZY.SizeData = nan; end end end if isGui if(ori_point.num > 0) posIdx = ori_point.z>=0; negIdx = ori_point.z<0; if sum(posIdx) > 0 posZX = ori_point.x(posIdx); posZY = ori_point.y(posIdx); posSize = 10; else posZX = nan; posZY = nan; posSize = nan; end if sum(negIdx) negZX = ori_point.x(negIdx); negZY = ori_point.y(negIdx); negSize = 10; else negZX = nan; negZY = nan; negSize = nan; end %显示Z值大于等于0的点 hGui.hDataPointXYPZ.XData = posZX; hGui.hDataPointXYPZ.YData = posZY; hGui.hDataPointXYPZ.SizeData = posSize; hGui_ref.hDataPointXYPZ_before.XData = posZX; hGui_ref.hDataPointXYPZ_before.YData = posZY; hGui_ref.hDataPointXYPZ_before.SizeData = posSize; %显示Z值小于0的点 hGui.hDataPointXYNZ.XData = negZX; hGui.hDataPointXYNZ.YData = negZY; hGui.hDataPointXYNZ.SizeData = negSize; hGui_ref.hDataPointXYNZ_before.XData = negZX; hGui_ref.hDataPointXYNZ_before.YData = negZY; hGui_ref.hDataPointXYNZ_before.SizeData = negSize; hGui.hDataPointZY.XData = ori_point.z; hGui.hDataPointZY.YData = ori_point.y; hGui.hDataPointZY.SizeData = 10;%1e-4*double([ ori_point.pow, ] + 1000); hGui_ref.hDataPointZY_before.XData = ori_point.z; hGui_ref.hDataPointZY_before.YData = ori_point.y; hGui_ref.hDataPointZY_before.SizeData = 10;%1e-4*double([ ori_point.pow, ] + 1000); else hGui.hDataPointXYPZ.XData = nan; hGui.hDataPointXYPZ.YData = nan; hGui.hDataPointXYPZ.SizeData = nan; hGui_ref.hDataPointXYPZ_before.XData = nan; hGui_ref.hDataPointXYPZ_before.YData = nan; hGui_ref.hDataPointXYPZ_before.SizeData = nan; hGui.hDataPointXYNZ.XData = nan; hGui.hDataPointXYNZ.YData = nan; hGui.hDataPointXYNZ.SizeData = nan; hGui_ref.hDataPointXYNZ_before.XData = nan; hGui_ref.hDataPointXYNZ_before.YData = nan; hGui_ref.hDataPointXYNZ_before.SizeData = nan; hGui.hDataPointZY.XData = nan; hGui.hDataPointZY.YData = nan; hGui.hDataPointZY.SizeData = nan; hGui_ref.hDataPointZY_before.XData = nan; hGui_ref.hDataPointZY_before.YData = nan; hGui_ref.hDataPointZY_before.SizeData = nan; end end case 6 framePayload = struct(); framePayload.Idx = int32(typecast(frame.Raw(4 + (1:4)), 'uint32')); framePayload.Tick = int32(typecast(frame.Raw(8 + (1:4)), 'uint32')); framePayload.NumPoint = uint32(typecast(frame.Raw(12 + (1:4)), 'uint32')); cfar_point.num = framePayload.NumPoint; cfar_point.r = []; cfar_point.v = []; cfar_point.pow = []; numBytePerPoint = int32(100); for idxPoint = 1:framePayload.NumPoint cfar_point.r(idxPoint) = typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(1:4)), 'int32'); cfar_point.v(idxPoint) = typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(5:8)), 'int32') - int32(64); cfar_point.pow(idxPoint) = single(typecast(frame.Raw(int32(16) + (int32(idxPoint) - 1)*numBytePerPoint + int32(97:100)), 'int32')); end case 5 DataBuffer = frame.Raw(13:frame.Length - 8); total_len = 128 * 8 * 128; nt2 = uint8(reshape(DataBuffer,[4,total_len/4])); for i=1:total_len/4 nt3(i) = typecast(fliplr(squeeze(nt2(:,i))),'single'); end float_single = complex(nt3(1:2:end),nt3(2:2:end)); ntt4 = reshape(float_single,[128, 128]); for k=1:128 fft2d_abs(:,k)= abs(ntt4(:,k)); end if isGui hGui.fftv.CData = fft2d_abs; end end idxFrame = idxFrame + 1; %显示图片 oriImage = imread(fullfile(path, ... sprintf("%s.jpg", currentTimestr))); % rotatedImage = imrotate(oriImage, 180, 'bilinear', 'crop'); % hGui.hDataImage.CData = rotatedImage; hGui.hDataImage.CData = oriImage; drawnow; end idxBin = idxBin + 1; pause(0.05); end function [pose, radarData] = read_bin_with_pose(filename) % 读取整个bin文件 fid = fopen(filename, 'rb'); rawData = uint8(fread(fid, inf,'*uint8')).'; fclose(fid); % 文件至少要有 32 字节才能包含位姿 if numel(rawData) < 32 error('文件太短，没有位姿数据'); end % 末尾 32 字节是 float32 位姿 poseBytes = rawData(end-31:end); pose = typecast(poseBytes, 'single'); % [q0,q1,q2,q3,x,y,z,valid] % 剩余部分是雷达数据 radarData = rawData(1:end-32); end function [listFrame, dataBuffer] = extract_frame(dataBuffer) FRAME_HEADER = 'RadarEye'; FRAME_END = 'REND'; listFrame = []; while true lenBuffer = length(dataBuffer); % 必须至少有 16 字节（8B帧头 + 4B长度 + 4B类型） if lenBuffer < 16 return; % 等待更多数据 end % 找帧头 if ~strcmp(char(dataBuffer(1:8)), FRAME_HEADER) % 没找到帧头 → 丢掉一个字节继续找 dataBuffer = dataBuffer(2:end); continue; end % 读取 Length frame = struct(); frame.Header = FRAME_HEADER; frame.Length = double(typecast(dataBuffer(9:12), 'uint32')); % 合理性检查（防止 Length 乱值） if frame.Length <= 0 || frame.Length > 200000 % 丢掉帧头继续 dataBuffer = dataBuffer(9:end); continue; end % 数据不足整个帧，等下一轮再处理 if lenBuffer < 12 + frame.Length return; end % 读取 Type frame.Type = typecast(dataBuffer(13:16), 'uint32'); % 根据 Type 决定校验数据范围 if frame.Type == 6 || frame.Type == 7 frame.Check = uint32(0); dataToCheck = typecast(dataBuffer(13:(12 + frame.Length - 4)), 'uint32'); elseif frame.Type == 5 frame.Length = frame.Length + 12; % 再次检查长度 if frame.Length > 200000 || lenBuffer < 12 + frame.Length dataBuffer = dataBuffer(9:end); continue; end frame.Check = uint32(0); dataToCheck = typecast(dataBuffer(13:(12 + frame.Length - 4)), 'uint32'); else % 未知类型，丢掉帧头继续 dataBuffer = dataBuffer(9:end); continue; end % 计算校验和 for i = 1:length(dataToCheck) frame.Check = bitxor(frame.Check, dataToCheck(i)); end % 检查帧尾 frame.End = char(dataBuffer((12 + frame.Length - 3):(12 + frame.Length))); if frame.Check ~= 0 || ~strcmp(frame.End, FRAME_END) % 校验失败 → 丢掉一个字节继续 dataBuffer = dataBuffer(2:end); continue; end % 保存原始数据 frame.Raw = dataBuffer(13:(12 + frame.Length - 8)); frame.bin = dataBuffer(1:(12 + frame.Length)); % 存到结果 listFrame = [listFrame; frame]; % 移除已解析的帧 dataBuffer = dataBuffer((12 + frame.Length + 1):end); end end %根据不同距离不同能量过滤数据 function isFlag=powerFilter(range,pow) isFlag = 0; if range < 10 && pow > 1200000 isFlag = 1; elseif range >= 10 && range < 20 && pow > 800000 isFlag = 1; elseif range >= 20 && range < 30 && pow > 500000 isFlag = 1; elseif range >= 30 && range < 40 && pow > 400000 isFlag = 1; elseif range >= 40 && range < 50 && pow > 300000 isFlag = 1; elseif range >= 50 && range < 60 && pow > 200000 isFlag = 1; elseif range >= 60 && range < 70 && pow > 200000 isFlag = 1; elseif range >= 70 && range < 80 && pow > 200000 isFlag = 1; elseif range >= 80 && pow > 200000 isFlag = 1; end end function euler = quat2euler_enu(Q) % Q = [Q0 Q1 Q2 Q3] (标量在前, 与C代码一致) q0 = Q(1); q1 = Q(2); q2 = Q(3); q3 = Q(4); % 方向余弦矩阵 DCM dcm = zeros(3,3); q0s = q0*q0; q1s = q1*q1; q2s = q2*q2; q3s = q3*q3; dcm(1,1) = q0s + q1s - q2s - q3s; dcm(1,2) = 2*(q1*q2 - q0*q3); dcm(1,3) = 2*(q0*q2 + q1*q3); dcm(2,1) = 2*(q1*q2 + q0*q3); dcm(2,2) = q0s - q1s + q2s - q3s; dcm(2,3) = 2*(q2*q3 - q0*q1); dcm(3,1) = 2*(q1*q3 - q0*q2); dcm(3,2) = 2*(q0*q1 + q2*q3); dcm(3,3) = q0s - q1s - q2s + q3s; % 欧拉角 (ENU 系统，弧度制) phi = atan2(-dcm(3,1), dcm(3,3)); % roll theta = asin(dcm(3,2)); % pitch psi = atan2(-dcm(1,2), dcm(2,2)); % yaw % 处理 pitch = ±90° 奇异点 eps = 1e-4; if abs(theta - pi/2) < eps phi = 0; psi = atan2(-dcm(1,3), -dcm(2,3)); elseif abs(theta + pi/2) < eps phi = 0; psi = atan2(dcm(1,3), dcm(2,3)); end %弧度制 euler = [phi, theta, psi]; % [roll pitch yaw] % 转换为角度制 % euler = rad2deg([phi, theta, psi]); % [roll pitch yaw] in degree end function Q = Quaternion_Enu_ByEuler(yaw, pitch, roll) % Quaternion_Enu_ByEuler 将欧拉角(yaw, pitch, roll)转换为四元数 % 输入角度单位为弧度 (rad) % Q = [q0 q1 q2 q3] (标量在前，向量在后) % 计算半角的正弦和余弦 cy = cos(yaw/2); cp = cos(pitch/2); cr = cos(roll/2); sy = sin(yaw/2); sp = sin(pitch/2); sr = sin(roll/2); q0 = cy*cp*cr - sy*sp*sr; q1 = cy*sp*cr - sy*cp*sr; q2 = cy*cp*sr + sy*sp*cr; q3 = cy*sp*sr + sy*cp*cr; % 组合成向量 Q = [q0 q1 q2 q3]; end function clusters = euclideanClusterExtraction(filtered_point_rfu, dist_thresh, min_points) % filtered_point_rfu: 结构体，包含 x, y, z, num % dist_thresh: 聚类的阈值 (比如 0.5 m) % min_points: 最小点数阈值 (比如 3) % 转换为矩阵 Nx3 points = [filtered_point_rfu.x, filtered_point_rfu.y, filtered_point_rfu.z]; N = size(points, 1); visited = false(N, 1); % 标记访问 cluster_list = {}; % 保存聚类结果 for i = 1:N if ~visited(i) % 新的聚类 cluster_points = points(i, :); visited(i) = true; % 广度优先搜索（BFS） queue = i; while ~isempty(queue) idx = queue(1); queue(1) = []; % 出队 % 找邻居 diffs = points - points(idx, :); dists = sum(diffs.^2, 2); neighbors = find(dists < dist_thresh^2 & ~visited); if ~isempty(neighbors) visited(neighbors) = true; cluster_points = [cluster_points; points(neighbors, :)]; queue = [queue; neighbors]; %#ok end end % 只保留点数超过 min_points 的聚类 if size(cluster_points, 1) >= min_points cluster_list{end+1} = cluster_points; %#ok end end end % 输出格式化结果 clusters = struct('points', {}, 'center', {}, 'num', {}); for k = 1:numel(cluster_list) pts = cluster_list{k}; clusters(k).points = pts; clusters(k).center = mean(pts, 1); clusters(k).num = size(pts, 1); end end function clusters_all = multiRangeClustering(points, params) % points: struct，包含 x,y,z,num % params: 每个距离区间的聚类参数设置 clusters_all = []; for i = 1:length(params) % 提取该区间的点 rangeIdx = points.y >= params(i).y_min & points.y < params(i).y_max; pts_range = [points.x(rangeIdx), points.y(rangeIdx), points.z(rangeIdx)]; if isempty(pts_range) continue; end % 在该区间用对应参数做聚类 clusters = euclideanClusterExtraction(... struct('x',pts_range(:,1),'y',pts_range(:,2),'z',pts_range(:,3),'num',size(pts_range,1)), ... params(i).dist_thresh, ... params(i).min_points); % 合并结果 clusters_all = [clusters_all, clusters]; end end function [cx, cy] = updateObstacleTracking(clusters, params) persistent trackedTargets if isempty(trackedTargets) trackedTargets = struct('x', {}, 'y', {}, 'confidence', {}, 'updated', {}); end % --- 提取每个簇的代表点 --- numClusters = numel(clusters); cx_new = nan(1,numClusters); cy_new = nan(1,numClusters); for k = 1:numClusters pts = clusters(k).points; if isempty(pts), continue; end [min_y, idx] = min(pts(:,2)); cx_new(k) = pts(idx,1); cy_new(k) = min_y; end cur_points = [cx_new(:), cy_new(:)]; % --- 标记所有目标未更新 --- for i = 1:numel(trackedTargets) trackedTargets(i).updated = false; end % --- 遍历检测点，更新或新建目标 --- for i = 1:size(cur_points,1) px = cur_points(i,1); py = cur_points(i,2); if isnan(px) || isnan(py), continue; end % 动态参数选择 if py <= 10 match_thresh = 1; min_conf = 4; max_conf = 6; elseif py <= 20 match_thresh = 1.3; min_conf = 2; max_conf = 5; elseif py <= 35 match_thresh = 1.8; min_conf = 2; max_conf = 5; else continue; end % --- 匹配已有目标 --- dists = arrayfun(@(t) hypot(t.x - px, t.y - py), trackedTargets); if isempty(dists) minDist = inf; else [minDist, minIdx] = min(dists); end if ~isempty(dists) && minDist < match_thresh % 命中已有目标 trackedTargets(minIdx).x = px; trackedTargets(minIdx).y = py; trackedTargets(minIdx).confidence = ... min(trackedTargets(minIdx).confidence + params.inc_val, max_conf); trackedTargets(minIdx).updated = true; else % 新建目标 newTarget.x = px; newTarget.y = py; newTarget.confidence = 1; % 初始值 newTarget.updated = true; trackedTargets = [trackedTargets, newTarget]; end end % --- 更新置信度 --- for i = 1:numel(trackedTargets) if ~trackedTargets(i).updated trackedTargets(i).confidence = trackedTargets(i).confidence - params.dec_val; end end % 仅保留置信度 > 0 的目标 if ~isempty(trackedTargets) trackedTargets = trackedTargets([trackedTargets.confidence] > 0); end % --- 生成输出点 --- cx = []; cy = []; for i = 1:numel(trackedTargets) py = trackedTargets(i).y; if py <= 10 min_conf = 4; elseif py <= 20 min_conf = 2; elseif py <= 35 min_conf = 2; else continue; end if trackedTargets(i).confidence >= min_conf cx(end+1) = trackedTargets(i).x; %#ok cy(end+1) = py; %#ok end end if isempty(cx) cx = 0; cy = 0; end end