（matlab程序）激光雷达数据的跟踪级融合.rar

classdef helperPanelDisplay < matlab.System properties ColorOrder = standardColorOrder; end properties % Figure or panel to mount this display on Parent end properties GroundTruthColorIndex = 4; RadarTrackColorIndex = 1; LidarTrackColorIndex = 2; FusedTrackColorIndex = 9; RadarCoverageColorIndex = 2; LidarCoverageColorIndex = 3; GroundPointCloudColorIndex = 5; ObstaclePointCloudColorIndex = 4; CroppedPointCloudColorIndex = 3; EgoVehicleIndex = 1; EgoVehicleColorIndex = 7; LidarDetectionColorIndex = 7; RadarDetectionColorIndex = 6; end properties (Nontunable) PlotRadarTracks = true; PlotLidarTracks = true; PlotFusedTracks = true; PlotPointClouds = true; PlotSegmentedPointClouds = true; PlotRadarDetections = true; PlotLidarDetections = true; PlotCoverage = true; PlotRadarResolution = false; PlotGroundTruth = true; PlotRoads = true; PlotTrackVelocity = true; PlotRadarDetectionVelocity = false; PlotTrackCovariance = true; ShowLegend = true; FollowActorID end properties ViewLocation = [-100 0]; ViewHeight = 100; ViewYaw = 0; ViewPitch = 60; ViewRoll = 0; end properties (Dependent, SetAccess = protected) GroundTruthColor RadarTrackColor LidarTrackColor FusedTrackColor RadarCoverageColor LidarCoverageColor GroundPointCloudColor ObstaclePointCloudColor CroppedPointCloudColor EgoVehicleColor LidarDetectionColor RadarDetectionColor end properties pGroundTruthPlotter pRadarTrackPlotter pRadarDetectionPlotter pLidarTrackPlotter pLidarDetectionPlotter pFusedTrackPlotter pCoveragePlotter pGroundPointCloudPlotter pObstaclePointCloudPlotter pCroppedPointCloudPlotter pTheaterPlot pBirdsEyePlot pFigure pDimStruct = struct('Length',0,'Width',0,'Height',0,'OriginOffset',[0 0 0]); pCoverageConfigStruct = coverageConfig(radarSensor(1)); pScenarioPlot end properties pResolutionPlotters end methods function val = get.GroundTruthColor(obj) val = obj.ColorOrder(obj.GroundTruthColorIndex,:); end function val = get.RadarTrackColor(obj) val = obj.ColorOrder(obj.RadarTrackColorIndex,:); end function val = get.LidarTrackColor(obj) val = obj.ColorOrder(obj.LidarTrackColorIndex,:); end function val = get.FusedTrackColor(obj) val = obj.ColorOrder(obj.FusedTrackColorIndex,:); end function val = get.RadarCoverageColor(obj) val = obj.ColorOrder(obj.RadarCoverageColorIndex,:); end function val = get.LidarCoverageColor(obj) val = obj.ColorOrder(obj.LidarCoverageColorIndex,:); end function val = get.GroundPointCloudColor(obj) val = obj.ColorOrder(obj.GroundPointCloudColorIndex,:); end function val = get.EgoVehicleColor(obj) val = obj.ColorOrder(obj.EgoVehicleColorIndex,:); end function val = get.LidarDetectionColor(obj) val = obj.ColorOrder(obj.LidarDetectionColorIndex,:); end function val = get.RadarDetectionColor(obj) val = obj.ColorOrder(obj.RadarDetectionColorIndex,:); end function val = get.ObstaclePointCloudColor(obj) val = obj.ColorOrder(obj.ObstaclePointCloudColorIndex,:); end function val = get.CroppedPointCloudColor(obj) val = obj.ColorOrder(obj.CroppedPointCloudColorIndex,:); end end methods function obj = helperPanelDisplay(varargin) setProperties(obj,nargin,varargin{:}); end end methods (Access = {?helperLidarRadarTrackFusionDisplay}) function showLegend(obj) % Show legend of existing figure on a new axes assert(obj.ShowLegend,'Legend must exist'); ax = obj.pTheaterPlot.Parent; ax.Position = [0 0 0 0]; l = legend(ax); l.Position = [0 0 1 1]; end end methods (Access = protected) function setupImpl(obj, varargin) setupPlotters(obj, varargin{:}); end function stepImpl(obj, scenario, ... radarSensors, radarDetections, radarTracks, ... lidarSensors, ptClouds, lidarDetections, segmentationInfo, lidarTracks, ... fusedTracks) % Plot truth plotTruth(obj, scenario, radarSensors, lidarSensors); % Plot detection data plotSensorData(obj, scenario, radarDetections, ptClouds, lidarDetections, segmentationInfo); % Plot track data plotTracks(obj, radarTracks, lidarTracks, fusedTracks); end end %% Plot updates methods function plotTruth(obj, scenario, radarSensors, lidarSensors) if obj.PlotGroundTruth obj.pScenarioPlot.plotActors(scenario.Actors); else obj.pScenarioPlot.plotActors(driving.scenario.Actor.empty(0,1)); end if obj.PlotCoverage [pos, range, orientation, fov] = parseCoverageAreaConfigs(obj, scenario, radarSensors, lidarSensors); for i = 1:numel(obj.pCoveragePlotter) obj.pCoveragePlotter{i}.plotCoverageArea(pos(i,:),range(i),orientation(i),fov(i)); end % Plot radar resolutions if obj.PlotRadarResolution plotResolutions(obj, scenario, radarSensors); end end end function plotResolutions(obj, scenario, radarSensors) ego = scenario.Actors(obj.EgoVehicleIndex); egoPos = ego.Position; egoOrient = quaternion([ego.Yaw ego.Pitch ego.Roll],'eulerd','ZYX','frame'); numPlotters = numel(radarSensors); if isempty(obj.pResolutionPlotters) obj.pResolutionPlotters = cell(numPlotters,1); for i = 1:numPlotters obj.pResolutionPlotters{i} = plot3(nan,nan,nan,'Color',obj.RadarCoverageColor); end end for i = 1:numel(radarSensors) sensorPos = [radarSensors{i}.SensorLocation radarSensors{i}.Height]; sensorOrient = quaternion([radarSensors{i}.Yaw radarSensors{i}.Pitch radarSensors{i}.Roll],'eulerd','ZYX','frame'); fov = radarSensors{1}.FieldOfView(1); azRes = radarSensors{i}.AzimuthResolution; maxRange = radarSensors{i}.MaxRange; rRes = radarSensors{i}.RangeResolution; az = deg2rad((-fov(1)/2:azRes:fov(1)/2)); r = (0:rRes:maxRange); resLines = zeros(0,2); for m = 1:numel(az) resLines = [resLines;0 az(m);maxRange az(m);nan az(m)]; %#ok<AGROW> end for m = 1:numel(r) for k = 1:numel(az) resLines = [resLines;r(m) az(k)]; %#ok<AGROW> end resLines = [resLines;nan nan]; %#ok<AGROW> end [resLinesX,resLinesY,resLinesZ] = sph2cart(resLines(:,2),0*resLines(:,2),resLines(:,1)); resLinesXYZ = [resLinesX resLinesY resLinesZ]; resLinesXYZ = rotatepoint(sensorOrient,resLinesXYZ) + sensorPos; resLine