4d_ObstacleRadarDroneCAN/matlab/eth_playback.m

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<AGROW>
                end
            end

            % 只保留点数超过 min_points 的聚类
            if size(cluster_points, 1) >= min_points
                cluster_list{end+1} = cluster_points; %#ok<AGROW>
            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<AGROW>
            cy(end+1) = py; %#ok<AGROW>
        end
    end

    if isempty(cx)
        cx = 0; cy = 0;
    end
end