Track-Level雷达和激光雷达数据的融合

这个示例使用:

打开脚本

这个例子向您展示了如何生成一个对象级跟踪列表从测量雷达和激光雷达传感器和进一步融合他们使用track-level融合方案。你处理的雷达测量使用一个扩展的对象跟踪和激光雷达测量使用一个联合概率数据关联(JPDA)追踪。您进一步融合这些痕迹使用track-level融合方案。工作流程的示意图如下所示。

看到使用ROS雷达和激光雷达数据的融合(ROS工具箱)该算法的一个例子使用rosbag记录数据。

设置场景合成数据生成

在这个例子中使用的场景创建的使用drivingScenario(自动驾驶工具箱)。雷达和激光雷达传感器的数据是模拟使用drivingRadarDataGenerator(自动驾驶工具箱)和lidarPointCloudGenerator(自动驾驶工具箱),分别。场景的创建和传感器模型包裹在helper函数helperCreateRadarLidarScenario。在场景和合成数据生成的更多信息,请参考以编程方式创建驾驶场景(自动驾驶工具箱)。

%的可重复的结果rng (2021);%创建场景,自我,得到雷达和激光雷达传感器(场景、egoVehicle雷达,激光雷达)= helperCreateRadarLidarScenario;

自我车辆安装有四个二维雷达传感器。前后雷达传感器的视场45度。左边和右边雷达传感器有一个150度的视野。每个雷达方位分辨率为6度,2.5米的范围内。自我也安装一个3 d激光雷达传感器和360度的视野方位和仰角40度。激光雷达方位分辨率0.2度和1.25度仰角海拔(32通道)。可视化的配置传感器和模拟传感器数据在下面的动画。注意,雷达分辨率高于对象,因此每个对象返回多个测量。还要注意,激光雷达与演员的低多边形网格以及路面从这些对象返回多个点。

雷达跟踪算法

正如前面提到的,雷达分辨率高于对象并返回多个检测每个对象。传统的追踪器,如全球最近邻(GNN)和联合概率数据关联(JPDA)假定传感器返回最多每扫描一个检测每个对象。因此,高分辨率传感器的检测必须集群与传统追踪器在处理之前或必须使用扩展对象追踪处理。扩展对象追踪器不需要pre-clustering检测和通常估计的运动状态(例如,位置和速度)和对象的程度。为更详细的比较传统的追踪器和扩展对象追踪器,参考扩展对象的高速公路车辆跟踪雷达和摄像头的例子。

一般来说,扩展对象追踪者提供更好的评估对象的处理集群和数据协会同时使用时间的历史痕迹。在这个例子中,雷达检测处理使用高斯混合概率假设密度(GM-PHD)跟踪器(trackerPHD和gmphd)和一个矩形目标模型。配置跟踪的更多细节,请参阅“GM-PHD矩形对象跟踪器”部分扩展对象的高速公路车辆跟踪雷达和摄像头的例子。

使用雷达测量跟踪对象的算法是包裹在助手类,helperRadarTrackingAlgorithm™,实现为一个系统对象。这个类输出数组objectTrack对象和定义他们的状态根据以下约定:

$美元[x y \ s \{\θ}\ \{ω\}\ L \ W]美元$

radarTrackingAlgorithm = helperRadarTrackingAlgorithm(雷达);

激光雷达跟踪算法

也类似于雷达,激光雷达传感器返回多个测量每个对象。此外,传感器返回大量的点从路上,必须移除之前作为一种目标跟踪算法的输入。而激光雷达数据从障碍可以通过扩展直接加工对象跟踪算法,传统的跟踪算法仍更普遍使用激光雷达数据进行跟踪。第一这一趋势的原因主要是由于观察到更高的计算复杂度为大型数据集扩展对象的追踪器。第二个原因是投资等先进的深度上优于探测器PointPillars [1], VoxelNet[2]和PIXOR[3],它可以部分的点云并返回边界框检测车辆。这些探测器可以帮助克服传统跟踪器的性能下降由于不当集群。

在这个例子中,激光雷达数据处理使用传统的联合概率数据关联(JPDA)跟踪器,配置了一个交互多模型(IMM)过滤器。激光雷达数据的预处理,去除点云是由使用RANSAC-based plane-fitting算法和边界框是由执行Euclidian-based距离聚类算法。关于该算法的更多信息,请参考使用激光雷达跟踪车辆:从点云来跟踪列表的例子。有关的例子相比,跟踪执行场景框架和追踪调优不同大小不同的跟踪对象。进一步定义不同的状态变量约束的运动轨道的方向估计的航向角。

跟踪对象的算法使用激光雷达数据包装在助手类,helperLidarTrackingAlgorithm作为系统对象实现。这个类输出数组objectTrack对象和定义它们的状态根据以下约定:

$美元[x \ \ s \{\θ}\{ω\}\ z \{\点{z}} \ H L \ W \]美元$

美国通用雷达算法类似的定义。三维传感器,激光雷达跟踪输出三个额外的州, z美元 , ${\点{z}}$ 和 H美元指z坐标(m), z-velocity(米/秒),和高度分别跟踪对象的(m)。

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(激光雷达);

建立了熔化炉、度量和可视化

熔化炉

接下来,您将建立一个融合算法融合跟踪雷达和激光雷达追踪器的列表。类似于其他跟踪算法,建立track-level融合算法的第一步是定义状态向量的选择(或状态)的融合或中央的踪迹。在这种情况下,融合跟踪的状态选择激光雷达一样。后选择一个中心跟踪状态,您定义的变换中央跟踪当地跟踪状态。在这种情况下,当地的跟踪状态是指雷达和激光雷达跟踪。要做到这一点,你用fuserSourceConfiguration对象。

定义雷达的配置源。的helperRadarTrackingAlgorithm输出跟踪与SourceIndex设置为1。的SourceIndex提供一个属性在每个跟踪器来唯一地标识并允许融合算法区分来自不同来源的追踪。因此,您设置SourceIndex雷达配置的属性一样的雷达跟踪。你设置IsInitializingCentralTracks来真正的让未赋值的雷达跟踪发起新的中央的踪迹。接下来,定义的变换在中央状态跟踪雷达状态,反之亦然。辅助功能central2radar和radar2central执行两个变换和包含在这个例子。

radarConfig = fuserSourceConfiguration (“SourceIndex”,1…“IsInitializingCentralTracks”,真的,…“CentralToLocalTransformFcn”@central2radar,…“LocalToCentralTransformFcn”,@radar2central);

定义激光雷达的配置源。由于激光雷达跟踪的状态是一样的中央,你不定义任何转换。

lidarConfig = fuserSourceConfiguration (“SourceIndex”2,…“IsInitializingCentralTracks”,真正的);

下一步是定义state-fusion算法。state-fusion算法需要多个州和州协方差在中央整数作为输入,并返回一个状态融合估计和协方差。在本例中,您使用协方差交叉算法提供的辅助功能,helperRadarLidarFusionFcn。一个通用的协方差交叉两个高斯分布估计算法的意思 x_i美元和协方差 P_i美元可以定义根据以下方程:

$美元P_ {F} ^ {1} = w_ {1} {P_ {1}} ^ {1} + w_ {2} {P_ {2}} ^ {1} $$

$F F $间{}= P_ {} (w_ {1} {P_{1}} ^{1}间的{1}+ w_ {2} {P_{2}} ^{1}间的{2})美元$

在哪里美元间{F} $ 和 $美元P_ {F} $$ 熔融状态协方差和吗 $美元w_ {1} $$ 和 $美元w_ {2} $$ 从每个估计混合系数。通常,这些混合系数估计通过最小化行列式或协方差融合的跟踪。在这个例子中,混合权重估计通过最小化的行列式位置每个估计的协方差。此外,随着雷达不估计三维,三维状态只有融合激光雷达。有关详细信息,请参阅helperRadarLidarFusionFcn函数的末尾显示这个脚本。

接下来,您组装使用的所有信息trackFuser对象。

%中央跟踪的状态是一样的激光雷达的跟踪,%因此你使用相同的状态转换函数。函数是%在helperLidarTrackingAlgorithm类中定义的。f = lidarTrackingAlgorithm.StateTransitionFcn;%创建一个trackFuser对象熔化炉= trackFuser (“SourceConfigurations”{radarConfig; lidarConfig},…“StateTransitionFcn”f…“ProcessNoise”诊断接头([1 3]),…“HasAdditiveProcessNoise”假的,…“AssignmentThreshold”,250年正无穷,…“ConfirmationThreshold”[3 - 5],…“DeletionThreshold”,5 [5],…“StateFusion”,“自定义”,…“CustomStateFusionFcn”,@helperRadarLidarFusionFcn);

指标

在本例中,您使用广义评估每个算法的性能最优的子模式任务指标(GOSPA)指标。设置三个独立指标使用trackGOSPAMetric为每一个追踪器。GOSPA度量目标跟踪系统的性能评估提供了一个标量成本。较低的价值度量表明更好的跟踪算法的性能。

使用GOSPA指标自定义运动模式,在本例中,使用的设置距离属性之间的“自定义”和定义距离函数跟踪及其相关地面真理。这些距离函数,在这个例子中helperRadarDistance,helperLidarDistance。

%雷达GOSPAgospaRadar = trackGOSPAMetric (“距离”,“自定义”,…“DistanceFcn”@helperRadarDistance,…“CutoffDistance”25);%激光雷达GOSPAgospaLidar = trackGOSPAMetric (“距离”,“自定义”,…“DistanceFcn”@helperLidarDistance,…“CutoffDistance”25);中央/融合GOSPA %gospaCentral = trackGOSPAMetric (“距离”,“自定义”,…“DistanceFcn”@helperLidarDistance,…%状态是激光雷达一样“CutoffDistance”25);

可视化

这个示例的可视化实现使用一个助手类helperLidarRadarTrackFusionDisplay。显示分为4板。显示块的测量和跟踪每个传感器的融合跟踪估计。的传说显示如下所示。此外,轨道被他们独特的身份(注释TrackID)以及一个前缀。前缀“R”、“L”和“F”代表雷达,激光雷达,分别和融合估计。

%创建一个显示。% FollowActorID控制行为人所示特写%显示显示= helperLidarRadarTrackFusionDisplay (“FollowActorID”3);%显示持续的传说showLegend(显示、场景);

运行场景和追踪器

接下来,您推进的情况下,生成合成数据从传感器和处理来生成跟踪每个系统。你也计算每个使用地面实况跟踪的度量可以从场景。

% Initialzie GOSPA指标及其组件跟踪算法。gospa = 0 (0);missTarget = 0 (0);falseTracks = 0 (0);%初始化fusedTracksfusedTracks = objectTrack.empty (0,1);%一个计数器时间步骤运行存储gospa指标。idx = 1;%地面真理标准。这个变量更新每一个时间步%自动成为一个演员的句柄。groundTruth = scenario.Actors(2:结束);而推进(场景)%当前时间时间= scenario.SimulationTime;%收集和自我对跟踪雷达和激光雷达测量%的场景。请参阅下面的helperCollectSensorData。[radarDetections, ptCloud egoPose] = helperCollectSensorData (egoVehicle、雷达、激光雷达、时间);%生成雷达跟踪radarTracks = radarTrackingAlgorithm (egoPose、radarDetections、时间);%产生激光雷达跟踪和分析信息,如边界框%检测和点云分割信息[lidarTracks, lidarDetections segmentationInfo] =…lidarTrackingAlgorithm (egoPose、ptCloud、时间);%连接雷达和激光雷达跟踪localTracks = [radarTracks; lidarTracks];%更新熔化炉。第一个电话必须包含一个当地的跟踪如果~ (isempty (localTracks) & & ~ isLocked(熔化炉))fusedTracks =熔化炉(localTracks、时间);结束%所有追踪器捕获GOSPA及其组件[gospa idx), ~, ~, ~, missTarget (1, idx) falseTracks (idx)] = gospaRadar (radarTracks groundTruth);[gospa (2, idx), ~, ~, ~, missTarget (2, idx) falseTracks (idx)] = gospaLidar (lidarTracks groundTruth);[gospa (3, idx), ~, ~, ~, missTarget (3 idx) falseTracks (idx)] = gospaCentral (fusedTracks groundTruth);%更新显示显示器(场景中,雷达,radarDetections radarTracks,…激光雷达、ptCloud lidarDetections、segmentationInfo lidarTracks,…fusedTracks);%更新索引存储GOSPA指标idx = idx + 1;结束%更新示例动画updateExampleAnimations(显示);

评估性能

评估每一个跟踪器的性能使用可视化以及定量指标。分析不同的场景和事件的理解track-level融合方案如何帮助实现一个更好的估计车辆的状态。

跟踪维护

下面的动画显示了整个运行每三个时间步。注意,每个三个跟踪系统,雷达,激光雷达,track-level融合,能够跟踪所有四个车辆在该方案中,没有错误的跟踪确认。

还可以定量测量这方面的性能使用“错过目标”和“错误跟踪”GOSPA度量组件。注意到下面的数字低于目标组件从一个更高的价值由于建立延迟和下降到零在大约5 - 10步骤为每个跟踪系统。也注意到错误跟踪所有系统组件是零,这表明没有错误的跟踪确认。

%的阴谋错过目标组件图;情节(missTarget ',“线宽”2);传奇(“雷达”,激光雷达的,“融合”);标题(“错过了目标指标”);包含(“时间步”);ylabel (“指标”);网格在;%情节虚假跟踪组件图;情节(falseTracks ',“线宽”2);传奇(“雷达”,激光雷达的,“融合”);标题(“错误跟踪指标”);包含(“时间步”);ylabel (“指标”);网格在;

Track-level准确性

每个追踪的track-level或定位精度也可以定量评估在每个时间步GOSPA度量。一个较低的值表示更好的跟踪精度。没有错过目标或假追踪,度量了定位错误造成每个车辆的状态估计。

注意,融合GOSPA度量估计低于单个传感器的指标,这表明跟踪精度提高后从每个传感器的融合跟踪估计。

%绘制GOSPA图;情节(gospa ',“线宽”2);传奇(“雷达”,激光雷达的,“融合”);标题(“GOSPA指标”);包含(“时间步”);ylabel (“指标”);网格在;

间隔太近的目标

正如前面提到的,这个示例使用一个基于欧式距离的聚类和边界框装配激光雷达数据反馈给传统跟踪算法。聚类算法通常遭受当对象是间隔太近。使用的检测器配置在这个例子中,当通过车辆接近车辆的自我,探测器集群点云从每辆车进入一个更大的边界框。你可以注意到下面的动画轨迹漂移远离车辆中心。因为追踪报道了更高的确定性估计几步,融合估计也是最初的影响。然而,随着不确定性的增加,其与融合估计变得较弱。这是由于协方差交叉算法选择混合重量为每个指定跟踪基于每个估计的确定性。

这种效应也在GOSPA度量。你可以注意到在上面的GOSPA度量情节激光雷达指标显示了峰值在第65次步骤。

雷达跟踪这个事件期间不受影响,因为两个主要原因。首先,每个探测雷达传感器输出数据与信息,通过不同的噪音水平之外的汽车相比慢移动车。这将导致增加统计检测从个人汽车之间的距离。其次,扩展对象追踪评估多个可能对预测轨迹聚类假设,导致拒绝不当集群和接受适当的集群。注意,正确选择最好的集群扩展对象追踪器,跟踪的过滤器必须健壮的在某种程度上,可以捕获两个集群之间的区别。例如,跟踪过程高噪音和高度不确定的尺寸可能无法正确地声明一个集群,因为它过早的年龄和高灵活性占不确定事件。

在长期目标

远离目标雷达传感器,测量的准确性降低,因为减少在探测器信噪比和分辨率有限的传感器。这导致高测量的不确定性,进而降低了跟踪精度。注意下面的特写镜头显示雷达的跟踪估计是远离地面雷达传感器和真理是报告更高的不确定性。然而,激光雷达传感器报告足够的测量数据点云来生成一个边界框“缩水”。收缩效应建模在激光雷达的测量模型跟踪算法允许跟踪保持跟踪与正确的尺寸。在这种情况下,雷达和激光雷达混合重量高于允许比雷达融合估计更准确的估计。

总结

在这个示例中,您了解了如何设置一个track-level融合算法的融合跟踪雷达和激光雷达传感器。您还了解了如何评估一个跟踪算法使用广义最优的子模式指标及其相关组件。

效用函数

collectSensorData

一个函数来生成雷达和激光雷达测量在当前时间步。

函数[radarDetections, ptCloud egoPose] = helperCollectSensorData (egoVehicle、雷达、激光雷达、时间)%对自我目标车辆的当前姿势tgtPoses = targetPoses (egoVehicle);radarDetections =细胞(0,1);为i = 1:元素个数(雷达)thisRadarDetections =步骤(雷达{我},tgtPoses、时间);radarDetections = [radarDetections; thisRadarDetections];% #好< AGROW >结束%生成从激光雷达点云rdMesh = roadMesh (egoVehicle);ptCloud =步骤(激光雷达,tgtPoses、rdMesh、时间);%计算自我车辆跟踪场景的构成框架。通常%获得使用一个INS系统。如果不可用,这可以设置%“起源”在自我车辆的跟踪框架。egoPose =姿势(egoVehicle);结束

radar2cental

函数将在雷达跟踪状态的跟踪中央状态。

函数centralTrack = radar2central (radarTrack)%初始化跟踪正确的大小centralTrack = objectTrack (“状态”1)0(10日,…“StateCovariance”、眼睛(10));%的同步特性radarTrack状态和StateCovariance除外% radarTrack看到syncTrack定义如下。centralTrack = syncTrack (centralTrack radarTrack);xRadar = radarTrack.State;PRadar = radarTrack.StateCovariance;H = 0 (10、7);%雷达中心线性变换矩阵H (1, - 1) = 1;H (2, 2) = 1;H (3,3) = 1;H (4, 4) = 1;H (5,5) = 1;6 H(8日)= 1;7 H(9日)= 1;xCentral = H * xRadar;%线性状态变换PCentral = H * PRadar * H ';%线性协方差变换PCentral([6 7 10],[6 7 10]) =眼(3);%的国家%设置状态和协方差的中央centralTrack。状态= xCentral; centralTrack.StateCovariance = PCentral;结束

central2radar

函数将在中央状态跟踪雷达的跟踪状态。

函数radarTrack = central2radar (centralTrack)%初始化跟踪正确的大小radarTrack = objectTrack (“状态”1)0(7日,…“StateCovariance”、眼睛(7));%的同步特性centralTrack状态和StateCovariance除外% radarTrack看到syncTrack定义如下。radarTrack = syncTrack (radarTrack centralTrack);xCentral = centralTrack.State;PCentral = centralTrack.StateCovariance;H = 0 (7, 10);%雷达线性变换矩阵的核心H (1, - 1) = 1;H (2, 2) = 1;H (3,3) = 1;H (4, 4) = 1;H (5,5) = 1;H (6、8) = 1;H (7、9) = 1;xRadar = H * xCentral;%线性状态变换PRadar = H * PCentral * H ';%线性协方差变换%设置状态和协方差的雷达跟踪radarTrack。状态= xRadar; radarTrack.StateCovariance = PRadar;结束

syncTrack

一个函数来同步属性与另一个除了一个跟踪状态和StateCovariance属性。

函数tr1 = syncTrack (tr1 tr2)道具=属性(tr1);notState = ~ strcmpi(道具、“状态”);notCov = ~ strcmpi(道具、“StateCovariance”);道具=道具(notState & notCov);为i = 1:元素个数(道具)tr1。(道具{我})= tr2。(道具{我});结束结束

构成

一个函数返回构成自我车辆的结构。

函数egoPose egoPose =姿势(egoVehicle)。位置= egoVehicle.Position;egoPose。速度= egoVehicle.Velocity;egoPose。偏航= egoVehicle.Yaw;egoPose。距= egoVehicle.Pitch;egoPose。滚= egoVehicle.Roll;结束

helperLidarDistance

函数计算归一化距离估计的跟踪雷达状态和分配的地面实况。

函数dist = helperLidarDistance(跟踪、真理)%计算实际值的状态估计的跟踪器%中心是不同的起源和追踪评估中心rOriginToCenter = -truth.OriginOffset (:) + (0, 0, truth.Height / 2);腐烂=四元数([真理。偏航真理。距truth.Roll),“eulerd”,“ZYX股票”,“帧”);actPos = truth.Position (:) + rotatepoint(腐烂,rOriginToCenter ') ';实际速度和z-rate %actVel =[规范(truth.Velocity (1:2)); truth.Velocity (3)];%实际偏航actYaw = truth.Yaw;%的实际尺寸。actDim = [truth.Length; truth.Width truth.Height];%实际偏航率actYawRate = truth.AngularVelocity (3);%计算每个估计误差加权的“需求”%的系统。使用指定的距离距离在各个方面%的估计,协方差的定义是“需求”。这%有助于避免倾斜距离当跟踪下/ /报告%的不确定性,因为在状态/计量模型不准确。%位置错误。estPos =。状态([1 2 6]); reqPosCov = 0.1*eye(3); e = estPos - actPos; d1 = sqrt(e'/reqPosCov*e);%速度误差estVel =。状态(7 [3]);reqVelCov = 5 *眼(2);e = estVel - actVel;d2 =√e”/ reqVelCov * e);%偏航误差estYaw = track.State (4);reqYawCov = 5;e = estYaw - actYaw;d3 =√e”/ reqYawCov * e);%偏航率误差estYawRate = track.State (5);reqYawRateCov = 1;e = estYawRate - actYawRate;d4 =√e”/ reqYawRateCov * e);%尺寸错误estDim =。状态([8 9 10]); reqDimCov = eye(3); e = estDim - actDim; d5 = sqrt(e'/reqDimCov*e);%总距离dist = d1 + d2 + d3 + d4 + d5;结束

helperRadarDistance

函数计算归一化距离估计的跟踪雷达状态和分配的地面实况。

函数dist = helperRadarDistance(跟踪、真理)%计算实际值的状态估计的跟踪器%中心是不同的起源和追踪评估中心rOriginToCenter = -truth.OriginOffset (:) + (0, 0, truth.Height / 2);腐烂=四元数([真理。偏航真理。距truth.Roll),“eulerd”,“ZYX股票”,“帧”);actPos = truth.Position (:) + rotatepoint(腐烂,rOriginToCenter ') ';actPos = actPos (1:2);%只有二维%的实际速度actVel =规范(truth.Velocity (1:2));%实际偏航actYaw = truth.Yaw;%的实际尺寸。只有二维雷达actDim = [truth.Length; truth.Width];%实际偏航率actYawRate = truth.AngularVelocity (3);%计算每个估计误差加权的“需求”%的系统。使用指定的距离距离在各个方面%的估计,协方差的定义是“需求”。这%有助于避免倾斜距离当跟踪下/ /报告%的不确定性,因为在状态/计量模型不准确。%位置错误estPos =。状态([1 - 2]);reqPosCov = 0.1 *眼(2);e = estPos - actPos;d1 =√e”/ reqPosCov * e);%的速度误差estVel = track.State (3);reqVelCov = 5;e = estVel - actVel;d2 =√e”/ reqVelCov * e);%偏航误差estYaw = track.State (4);reqYawCov = 5;e = estYaw - actYaw;d3 =√e”/ reqYawCov * e);%偏航率误差estYawRate = track.State (5);reqYawRateCov = 1;e = estYawRate - actYawRate;d4 =√e”/ reqYawRateCov * e);%尺寸错误estDim =。状态(7 [6]);reqDimCov =眼(2);e = estDim - actDim;d5 =√e”/ reqDimCov * e);%总距离dist = d1 + d2 + d3 + d4 + d5;%一个常数penality没有测量三维状态dist = dist + 3;结束

helperRadarLidarFusionFcn

中部的融合状态和状态协方差函数跟踪状态

函数(x, P) = helperRadarLidarFusionFcn (xAll笼罩)n =大小(xAll 2);依据= 0 (n, 1);%初始化x和Px = xAll (: 1);P =笼罩(:,:1);onlyLidarStates = false (10, 1);onlyLidarStates ([6 7 10]) = true;%与激光雷达融合这个信息xOnlyLidar = xAll (onlyLidarStates:);POnlyLidar =笼罩(onlyLidarStates、onlyLidarStates:);%州和协方差与雷达和激光雷达的交点xToFuse = xAll (~ onlyLidarStates:);PToFuse =笼罩(~ onlyLidarStates ~ onlyLidarStates,:);%顺序的决定因素。这有助于构建顺序%协方差与类似的决定。例如,两个大%小协方差协方差可能相交,相比较%的第三个最小的协方差。为i = 1: n检波器(i) =检波器(PToFuse(1:2, 1:2,我));结束[~,idx] =排序(侦破,“下”);xToFuse = xToFuse (:, idx);PToFuse = PToFuse (:,:, idx);%初始化融合估计thisX = xToFuse (: 1);thisP = PToFuse (:: 1);%序贯融合为我= 2:n [thisX thisP] = fusecovintUsingPos (thisX、thisP xToFuse(:,我),PToFuse(:,:我));结束%分配各个来源的融合状态x (~ onlyLidarStates) = thisX;P (~ onlyLidarStates ~ onlyLidarStates,:) = thisP;%融合一些州只与激光源有效=任何(abs (xOnlyLidar) > 1 e-6, 1);xMerge = xOnlyLidar(:,有效);PMerge = POnlyLidar(:,:有效);如果总和(有效)> 1 (xL, PL) = fusecovint (xMerge PMerge);elseif总和(有效)= = 1 xL = xMerge;PL = PMerge;其他的xL = 0 (3,1);PL =眼(3);结束x (onlyLidarStates) = xL;P (onlyLidarStates onlyLidarStates) = PL;结束函数(x, P) = fusecovintUsingPos (P1 x1, x2, P2)%协方差交叉一般采用以下%方程:% P ^ 1 = w1 * P1 ^ 1 + w2 * P2 ^ 1% x = P * (w1 * P1 x1 + w2 * P2 ^ ^ 1 * 1 * x2);%,w1 + w2 = 1%通常标量的代表像“引爆器”或协方差矩阵% P是最小化的“跟踪”计算w。这是提供的函数%”fusecovint”。然而。在这种情况下,选择w通过最小化%的决定因素协方差只“位置”。n =大小(x1, 1);idx = (1 - 2);detP1pos =侦破(P1 (idx idx));detP2pos =侦破(P2 (idx idx));w1 = detP2pos / (detP1pos + detP2pos);w2 = detP1pos / (detP1pos + detP2pos);我眼睛= (n);P1inv = I / P1;P2inv = I / P2;Pinv = w1 * P1inv + w2 * P2inv; P = I/Pinv; x = P*(w1*P1inv*x1 + w2*P2inv*x2);结束

引用

[1]朗,亚历克斯·H。,等。“PointPillars:快速编码器对象检测从云点。”Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2019.

[2]周阴,Oncel Tuzel。“Voxelnet:端到端学习基于点云三维对象检测。”Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2018.

[3],垃圾桶,蕴结罗和拉奎尔Urtasun。“Pixor:实时3 d对象检测从云点。”Proceedings of the IEEE conference on Computer Vision and Pattern Recognition. 2018.