레이다 데이터 및 라이다 데이터의 트랙에서의 수준 융합

다음 제품이 필요합니다.

이 예제에서는 레이다 센서와 라이다 센서의 측정값에서 객체 수준 트랙 목록을 생성하고 트랙 수준에서의 융합 방식을 사용하여 이들을 더욱더 융합하는 방법을 보여줍니다. 확장 객체 추적기를 사용하여 레이다 측정값을 처리하고 JPDA(Joint Probabilistic Data Association) 추적기를 사용하여 라이다 측정값을 처리합니다. 트랙 수준에서의 융합 방식을 사용하여 이러한 트랙을 더욱더 융합할 수 있습니다. 워크플로 구성도가 아래에 나와 있습니다.

rosbag에 기록된 데이터를 사용하는 이 알고리즘에 대한 예제는 Fusion of Radar and Lidar Data Using ROS (ROS Toolbox) 항목을 참조하십시오.

합성 데이터 생성을 위한 시나리오 설정하기

이 예제에서 사용하는 시나리오는 drivingScenario (Automated Driving Toolbox)를 사용하여 생성됩니다. 레이다 센서의 데이터와 라이다 센서의 데이터는 각각 drivingRadarDataGenerator (Automated Driving Toolbox)와 lidarPointCloudGenerator (Automated Driving Toolbox)를 사용하여 시뮬레이션됩니다. 시나리오와 센서 모델의 생성은 헬퍼 함수 helperCreateRadarLidarScenario에 래핑됩니다. 시나리오와 합성 데이터 생성에 대한 자세한 내용은 Create Driving Scenario Programmatically (Automated Driving Toolbox) 항목을 참조하십시오.

% For reproducible results
rng(2021);

% Create scenario, ego vehicle and get radars and lidar sensor
[scenario, egoVehicle, radars, lidar] = helperCreateRadarLidarScenario;

자차량에는 2차원 레이다 센서 4개가 장착되어 있습니다. 전방 레이다 센서와 후방 레이다 센서의 시야는 45도입니다. 좌측 레이다 센서와 우측 레이다 센서의 시야는 150도입니다. 각 레이다는 6도(방위각)와 2.5미터(거리)의 분해능을 가집니다. 자차량에는 360도 방위각과 40도 고도의 시야를 가진 3차원 라이다 센서도 1개 장착되어 있습니다. 라이다 센서는 0.2도 방위각과 1.25도 고도(32개 고도 채널)의 분해능을 가집니다. 아래 애니메이션에 센서의 구성과 시뮬레이션된 센서 데이터를 시각화합니다. 레이다의 분해능이 객체보다 높기 때문에 레이다는 객체당 여러 개의 측정값을 반환합니다. 또한 라이다는 도로 표면뿐만 아니라 액터의 로우 폴리 메시와 상호 작용하여 이러한 객체에서 여러 점을 반환합니다.

레이다 추적 알고리즘

앞서 언급했듯이, 레이다는 객체보다 높은 분해능을 가지며 객체당 여러 건의 검출을 반환합니다. GNN(Global Nearest Neighbor) 및 JPDA(Joint Probabilistic Data Association)와 같은 기존의 추적기는 센서가 스캔마다 객체당 최대 1건의 검출을 반환한다고 가정합니다. 따라서 고분해능 센서로부터 얻은 검출은 기존의 추적기로 처리하기 전에 군집화하거나 확장 객체 추적기를 사용하여 처리해야 합니다. 확장 객체 추적기는 검출의 사전 군집화가 필요하지 않으며 일반적으로 기구학적 상태(예: 위치 및 속도)와 객체의 범위를 모두 추정합니다. 기존의 추적기와 확장 객체 추적기의 더 자세한 비교는 Extended Object Tracking of Highway Vehicles with Radar and Camera (Sensor Fusion and Tracking Toolbox) 예제를 참조하십시오.

일반적으로 확장 객체 추적기는 트랙의 시간 내역을 사용하여 군집화와 데이터 연결을 동시에 처리하므로 객체를 더 잘 추정할 수 있습니다. 이 예제에서, 레이다 탐지는 사각형 표적 모델을 사용하는 GM-PHD(가우스 혼합-확률 가설 밀도) 추적기(trackerPHD (Sensor Fusion and Tracking Toolbox) 및 gmphd (Sensor Fusion and Tracking Toolbox))를 통해 처리됩니다. 추적기 구성에 대한 자세한 내용은 Extended Object Tracking of Highway Vehicles with Radar and Camera (Sensor Fusion and Tracking Toolbox) 예제의 "GM-PHD 사각형 객체 추적기" 섹션을 참조하십시오.

레이다 측정값을 사용하여 객체를 추적하는 알고리즘은 System object™로 구현되는 헬퍼 클래스 helperRadarTrackingAlgorithm 내에 래핑됩니다. 이 클래스는 objectTrack (Sensor Fusion and Tracking Toolbox) 객체 배열을 출력하며 다음 규칙에 따라 상태를 정의합니다.

$[x\ y\ s\ {\theta}\ {\omega}\ L\ W]$

radarTrackingAlgorithm = helperRadarTrackingAlgorithm(radars);

라이다 추적 알고리즘

레이다와 유사하게 라이다 센서도 객체당 여러 개의 측정값을 반환합니다. 또한 라이다 센서는 도로에서 상당수의 점을 반환하는데, 이러한 점은 객체 추적 알고리즘에서 입력값으로 사용되기 전에 제거되어야 합니다. 장애물의 라이다 데이터는 확장 객체 추적 알고리즘을 통해 직접 처리될 수 있지만, 기존의 추적 알고리즘이 여전히 라이다 데이터를 사용하는 추적에 더 널리 사용되고 있습니다. 이러한 추세의 첫 번째 이유는 주로 대규모 데이터 세트의 경우 확장 객체 추적기의 계산 복잡도가 더 높기 때문인 것으로 보입니다. 두 번째 이유는 포인트 클라우드를 분할하고 차량에 대한 경계 상자 검출을 반환할 수 있는 PointPillars [1], VoxelNet [2], PIXOR [3]과 같은 첨단 딥러닝 기반 검출기에 대한 투자입니다. 이러한 검출기는 비적정 군집화로 인한 기존의 추적기의 성능 저하를 극복하는 데 도움이 될 수 있습니다.

이 예제에서, 라이다 데이터는 IMM(Interacting Multiple Model) 필터로 구성된 기존의 JPDA(Joint Probabilistic Data Association) 추적기를 사용하여 처리됩니다. 포인트 클라우드를 제거하기 위한 라이다 데이터 전처리는 RANSAC 기반 평면 피팅 알고리즘을 사용하여 수행되며 경계 상자는 유클리드 기반 거리 군집화 알고리즘을 수행하여 형성됩니다. 이 알고리즘에 대한 자세한 내용은 Track Vehicles Using Lidar: From Point Cloud to Track List (Sensor Fusion and Tracking Toolbox) 예제를 참조하십시오. 연결된 예제를 비교해 보면, 추적은 시나리오 프레임에서 수행되며 추적기는 다양한 크기의 객체를 추적하기 위해 다르게 조정됩니다. 또한 변수의 상태는 추정된 방향각의 방향으로 트랙의 모션을 제약하기 위해 다르게 정의됩니다.

라이다 데이터를 사용하여 객체를 추적하는 알고리즘은 System object로 구현되는 헬퍼 클래스 helperLidarTrackingAlgorithm 내에 래핑됩니다. 이 클래스는 objectTrack (Sensor Fusion and Tracking Toolbox) 객체 배열을 출력하며 다음 규칙에 따라 상태를 정의합니다.

$[x\ y\ s\ {\theta}\ {\omega}\ z\ {\dot{z}}\ L\ W\ H]$

레이다 알고리즘에 공통적으로 사용되는 상태도 유사하게 정의됩니다. 또한 3차원 센서로서 라이다 추적기는 세 가지 추가 상태 , ${\dot{z}}$ , 를 출력합니다. 이는 각각 추적된 객체의 z 좌표(m), z 속도(m/s), 높이(m)를 나타냅니다.

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(lidar);

융합기, 메트릭 및 시각화 설정하기

융합기

다음으로, 레이다 추적기와 라이다 추적기의 트랙 목록을 융합하기 위한 융합 알고리즘을 설정합니다. 다른 추적 알고리즘과 유사하게, 트랙 수준에서의 융합 알고리즘을 설정하기 위한 첫 번째 단계는 융합된 트랙 또는 중앙 트랙에 대한 상태 벡터(또는 상태공간)의 선택을 정의하는 것입니다. 이 경우, 융합된 트랙의 상태공간은 라이다와 동일해지도록 선택됩니다. 중앙 트랙 상태공간을 선택한 후, 중앙 트랙 상태에서 로컬 트랙 상태로의 변환을 정의할 수 있습니다. 이 경우, 로컬 트랙 상태공간은 레이다 트랙의 상태와 라이다 트랙의 상태를 나타냅니다. 이 작업을 수행하기 위해 fuserSourceConfiguration (Sensor Fusion and Tracking Toolbox) 객체를 사용합니다.

레이다 소스의 구성을 정의합니다. helperRadarTrackingAlgorithm은 SourceIndex가 1로 설정된 트랙을 출력합니다. SourceIndex는 각 추적기에서 고유하게 식별할 수 있는 속성으로 제공되며, 융합 알고리즘이 여러 다른 소스의 트랙을 구별할 수 있도록 해 줍니다. 따라서 레이다 구성의 SourceIndex 속성을 레이다 트랙의 해당 속성과 동일하게 설정합니다. IsInitializingCentralTracks를 true로 설정하여 할당되지 않은 레이다 트랙이 새 중앙 트랙을 시작하게 합니다. 다음으로, 중앙 상태공간의 트랙에서 레이다 상태공간으로의 변환을 정의하고 그 반대의 변환도 정의합니다. 이 예제의 끝부분에 포함되어 있는 헬퍼 함수 central2radar와 radar2central이 이 두 가지 변환을 수행합니다.

radarConfig = fuserSourceConfiguration('SourceIndex',1,...
    'IsInitializingCentralTracks',true,...
    'CentralToLocalTransformFcn',@central2radar,...
    'LocalToCentralTransformFcn',@radar2central);

라이다 소스의 구성을 정의합니다. 라이다 트랙의 상태공간이 중앙 트랙과 동일하기 때문에 변환을 정의하지 않습니다.

lidarConfig = fuserSourceConfiguration('SourceIndex',2,...
    'IsInitializingCentralTracks',true);

다음 단계는 상태-융합 알고리즘을 정의하는 것입니다. 상태-융합 알고리즘은 중앙 상태공간의 여러 상태 및 상태 공분산을 입력값으로 사용하며 상태 및 공분산의 융합된 추정값을 반환합니다. 이 예제에서는 헬퍼 함수 helperRadarLidarFusionFcn에서 제공되는 공분산 교차 알고리즘을 사용합니다. 평균 와 공분산 를 갖는 두 개의 가우스 추정값에 대한 일반 공분산 교차 알고리즘은 다음 방정식에 따라 정의될 수 있습니다.

$P_{F}^{-1}= w_{1}{P_{1}}^{-1} + w_{2}{P_{2}}^{-1}$

$x_{F}= P_{F}(w_{1}{P_{1}}^{-1}x_{1} + w_{2}{P_{2}}^{-1}x_{2})$

여기서 $x_{F}$ 와 $P_{F}$ 는 융합된 상태와 공분산이고 $w_{1}$ 과 $w_{2}$ 는 각 추정값에서의 혼합 계수입니다. 일반적으로 이러한 혼합 계수는 융합된 공분산의 행렬식 또는 대각합을 최소화하여 추정됩니다. 이 예제에서, 혼합 가중치는 각 추정값의 위치 공분산의 행렬식을 최소화하여 추정됩니다. 또한 레이다는 3차원 상태를 추정하지 않으므로 3차원 상태는 라이다와만 융합됩니다. 자세한 내용은 이 스크립트의 끝부분에 나오는 helperRadarLidarFusionFcn 함수를 참조하십시오.

다음으로, trackFuser 객체를 사용하여 모든 정보를 조합합니다.

% The state-space of central tracks is same as the tracks from the lidar,
% therefore you use the same state transition function. The function is
% defined inside the helperLidarTrackingAlgorithm class.
f = lidarTrackingAlgorithm.StateTransitionFcn;

% Create a trackFuser object
fuser = trackFuser('SourceConfigurations',{radarConfig;lidarConfig},...
    'StateTransitionFcn',f,...
    'ProcessNoise',diag([1 3 1]),...
    'HasAdditiveProcessNoise',false,...
    'AssignmentThreshold',[250 inf],...
    'ConfirmationThreshold',[3 5],...
    'DeletionThreshold',[5 5],...
    'StateFusion','Custom',...
    'CustomStateFusionFcn',@helperRadarLidarFusionFcn);

메트릭

이 예제에서는 GOSPA(Generalized Optimal SubPattern Assignment) 메트릭을 사용하여 각 알고리즘의 성능을 평가합니다. 각 추적기에 trackGOSPAMetric (Sensor Fusion and Tracking Toolbox)을 사용하여 3개의 개별 메트릭을 설정합니다. GOSPA 메트릭은 스칼라 비용을 제공하여 추적 시스템의 성능을 평가하는 것을 목표로 합니다. 메트릭의 값이 낮을수록 추적 알고리즘의 성능이 더 양호합니다.

이 예제에서 사용한 것과 유사한 사용자 지정 모션 모델에 GOSPA 메트릭을 사용하기 위해 Distance 속성을 'custom'으로 설정하고 트랙 및 트랙과 연결된 ground truth 간의 거리 함수를 정의합니다. 이 예제의 끝부분에 나오는 거리 함수는 helperRadarDistance와 helperLidarDistance입니다.

% Radar GOSPA
gospaRadar = trackGOSPAMetric('Distance','custom',...
    'DistanceFcn',@helperRadarDistance,...
    'CutoffDistance',25);

% Lidar GOSPA
gospaLidar = trackGOSPAMetric('Distance','custom',...
    'DistanceFcn',@helperLidarDistance,...
    'CutoffDistance',25);

% Central/Fused GOSPA
gospaCentral = trackGOSPAMetric('Distance','custom',...
    'DistanceFcn',@helperLidarDistance,...% State-space is same as lidar
    'CutoffDistance',25);

시각화

이 예제의 시각화는 헬퍼 클래스 helperLidarRadarTrackFusionDisplay를 사용하여 구현됩니다. 디스플레이는 4개 패널로 나뉩니다. 디스플레이는 융합된 트랙 추정값뿐만 아니라 각 센서의 측정값과 트랙도 플로팅합니다. 디스플레이에 대한 범례가 아래에 나와 있습니다. 또한 트랙에는 각각의 고유 ID(TrackID)와 접두사로 주석이 지정됩니다. 접두사 "R", "L", "F"는 각각 레이다, 라이다, 융합된 추정값을 의미합니다.

% Create a display.
% FollowActorID controls the actor shown in the close-up
% display
display = helperLidarRadarTrackFusionDisplay('FollowActorID',3);

% Show persistent legend
showLegend(display,scenario);

시나리오와 추적기 실행하기

다음으로, 시나리오를 진행하고 모든 센서에서 합성 데이터를 생성하고 이를 처리하여 각 시스템에서 트랙을 생성합니다. 또한 시나리오에서 사용 가능한 ground truth를 사용하여 각 추적기에 대한 메트릭을 계산합니다.

% Initialize GOSPA metric and its components for all tracking algorithms.
gospa = zeros(3,0);
missTarget = zeros(3,0);
falseTracks = zeros(3,0);

% Initialize fusedTracks
fusedTracks = objectTrack.empty(0,1);

% A counter for time steps elapsed for storing gospa metrics.
idx = 1;

% Ground truth for metrics. This variable updates every time-step
% automatically being a handle to the actors.
groundTruth = scenario.Actors(2:end);

while advance(scenario)
    % Current time
    time = scenario.SimulationTime;

    % Collect radar and lidar measurements and ego pose to track in
    % scenario frame. See helperCollectSensorData below.
    [radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle, radars, lidar, time);

    % Generate radar tracks
    radarTracks = radarTrackingAlgorithm(egoPose, radarDetections, time);

    % Generate lidar tracks and analysis information like bounding box
    % detections and point cloud segmentation information
    [lidarTracks, lidarDetections, segmentationInfo] = ...
        lidarTrackingAlgorithm(egoPose, ptCloud, time);

    % Concatenate radar and lidar tracks
    localTracks = [radarTracks;lidarTracks];

    % Update the fuser. First call must contain one local track
    if ~(isempty(localTracks) && ~isLocked(fuser))
        fusedTracks = fuser(localTracks,time);
    end

    % Capture GOSPA and its components for all trackers
    [gospa(1,idx),~,~,~,missTarget(1,idx),falseTracks(1,idx)] = gospaRadar(radarTracks, groundTruth);
    [gospa(2,idx),~,~,~,missTarget(2,idx),falseTracks(2,idx)] = gospaLidar(lidarTracks, groundTruth);
    [gospa(3,idx),~,~,~,missTarget(3,idx),falseTracks(3,idx)] = gospaCentral(fusedTracks, groundTruth);

    % Update the display
    display(scenario, radars, radarDetections, radarTracks, ...
        lidar, ptCloud, lidarDetections, segmentationInfo, lidarTracks,...
        fusedTracks);

    % Update the index for storing GOSPA metrics
    idx = idx + 1;
end

% Update example animations
updateExampleAnimations(display);

성능 평가하기

정량적 메트릭뿐만 아니라 시각화를 사용하여 각 추적기의 성능을 평가합니다. 시나리오의 다양한 이벤트를 분석하고 트랙 수준에서의 융합 방식이 어떻게 차량 상태를 더 잘 추정하는 데 도움이 되는지 파악합니다.

트랙 관리

아래 애니메이션은 3개의 시간 스텝마다 전체 실행을 보여줍니다. 세 가지 추적 시스템(레이다, 라이다 및 트랙 수준에서의 융합) 각각은 시나리오에서 4대의 차량 모두를 추적할 수 있었으며 거짓 트랙으로 확인된 것은 없습니다.

또한 GOSPA 메트릭의 "missed target" 성분과 "false track" 성분을 사용하여 성능의 이러한 측면을 정량적으로 측정할 수 있습니다. 아래 Figure에서 missed target 성분은 설정 지연으로 인해 더 높은 값에서 시작하고 각 추적 시스템에 대해 약 5~10스텝 내에 0으로 내려가는 것을 볼 수 있습니다. 또한 false track 성분은 모든 시스템에 대해 0임을 알 수 있습니다. 이는 거짓 트랙으로 확인된 것은 없음을 뜻합니다.

% Plot missed target component
figure; plot(missTarget','LineWidth',2); legend('Radar','Lidar','Fused');
title("Missed Target Metric"); xlabel('Time step'); ylabel('Metric'); grid on;

% Plot false track component
figure; plot(falseTracks','LineWidth',2); legend('Radar','Lidar','Fused');
title("False Track Metric"); xlabel('Time step'); ylabel('Metric'); grid on;

트랙 수준 정확도

각 추적기의 트랙 수준 정확도나 위치추정 정확도 또한 각 시간 스텝에서 GOSPA 메트릭을 통해 정량적으로 평가할 수 있습니다. 값이 낮을수록 추적 정확도가 더 높습니다. 표적 누락 또는 거짓 트랙이 없으므로 이 메트릭은 각 차량의 상태 추정으로 인해 발생한 위치추정 오류를 포착합니다.

융합된 추정값에 대한 GOSPA 메트릭은 개별 센서에 대한 메트릭보다 낮습니다. 이는 각 센서에서의 트랙 추정값을 융합한 후에 트랙 정확도가 높아졌음을 뜻합니다.

% Plot GOSPA
figure; plot(gospa','LineWidth',2); legend('Radar','Lidar','Fused');
title("GOSPA Metric"); xlabel('Time step'); ylabel('Metric'); grid on;

조밀한 간격의 표적

앞서 언급했듯이 이 예제에서는 유클리드 거리 기반 군집화와 경계 상자 피팅을 사용하여 라이다 데이터를 기존의 추적 알고리즘에 전달합니다. 군집화 알고리즘은 일반적으로 객체의 간격이 조밀한 경우 문제가 발생합니다. 이 예제에서 사용하는 검출기 구성의 경우, 추월하는 다른 차량이 자차량 앞에 있는 차량에 가까워질 때 검출기가 각 차량의 포인트 클라우드를 더 큰 경계 상자 안에 군집화합니다. 아래 애니메이션을 보면 트랙이 자차량 중심에서 멀어진 것을 알 수 있습니다. 몇 스텝 동안 이 트랙은 추정값 측면에서 확실성이 높은 것으로 보고되었기 때문에 융합된 추정값도 처음에 그 영향을 받았습니다. 그러나 불확실성이 증가함에 따라 융합된 추정값과의 연결성이 약해집니다. 이는 공분산 교차 알고리즘이 각 추정값의 확실성을 기준으로, 할당된 트랙 각각에 대한 혼합 가중치를 선택하기 때문입니다.

이러한 영향은 GOSPA 메트릭에서도 포착됩니다. 위의 GOSPA 메트릭 플롯에서 라이다 메트릭이 65번째 시간 스텝 근처에서 피크를 나타내고 있음을 볼 수 있습니다.

레이다 트랙은 크게 두 가지 이유에서 이 이벤트 동안 영향을 받지 않습니다. 첫째, 레이다 센서는 각각의 검출에서 거리 변화율 정보를 출력하는데 이때 추월하는 자동차가 출력하는 정보는 이보다 느리게 이동하는 자동차에 비해 잡음 수준을 넘어서는 차이를 나타냅니다. 그 결과, 검출된 개별 자동차 간의 통계적 거리가 증가하게 됩니다. 둘째, 확장 객체 추적기는 예측된 트랙에 대한 여러 가능한 군집화 가설을 평가하며, 이에 따라 적절하지 않은 군집은 거부되고 적절한 군집은 수락됩니다. 확장 객체 추적기가 최상의 군집을 적절히 선택하기 위해서는 트랙에 대한 필터가 두 군집 간의 차이를 포착할 수 있을 정도로 강인해야 합니다. 공정 잡음이 많이 포함되어 있고 크기가 확실치 않은 트랙은 불확실한 이벤트를 충분하게 나타내기에는 시기적으로 이르고 변화할 가능성도 높기 때문에 군집으로 인정되기 어려울 수 있습니다.

원거리에 있는 표적

표적이 레이다 센서에서 멀어지면 검출기의 낮아진 신호 대 잡음비와 센서의 제한된 분해능 때문에 측정값의 정확도가 떨어지게 됩니다. 이로 인해 측정값의 불확실성이 높아지고, 결과적으로 트랙 정확도가 낮아집니다. 아래의 확대된 디스플레이에서 레이다의 트랙 추정값은 레이다 센서의 ground truth에서 더 멀어지며 더 높은 불확실성을 가진 채로 보고됩니다. 하지만, 라이다 센서는 포인트 클라우드에서 충분한 측정값을 보고하여 "축소된" 경계 상자를 생성합니다. 라이다 추적 알고리즘에 대한 측정 모델에서 모델링된 축소 효과를 통해 추적기는 정확한 크기의 트랙을 관리할 수 있습니다. 이러한 경우, 라이다 혼합 가중치는 레이다보다 높으며 이 가중치를 사용하면 융합된 추정값이 레이다 추정값보다 더 정확할 수 있습니다.

요약

이 예제에서는 레이다 센서와 라이다 센서의 트랙을 융합하는 트랙 수준에서의 융합 알고리즘을 설정하는 방법을 알아보았습니다. 또한 GOSPA(Generalized Optimal SubPattern Assignment) 메트릭 및 이와 관련된 성분을 사용하여 추적 알고리즘을 평가하는 방법도 알아보았습니다.

유틸리티 함수

collectSensorData

현재 시간 스텝에서 레이다 측정값과 라이다 측정값을 생성하는 함수입니다.

function [radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle, radars, lidar, time)

% Current poses of targets with respect to ego vehicle
tgtPoses = targetPoses(egoVehicle);

radarDetections = cell(0,1);
for i = 1:numel(radars)
    thisRadarDetections = step(radars{i},tgtPoses,time);
    radarDetections = [radarDetections;thisRadarDetections]; %#ok<AGROW>
end

% Generate point cloud from lidar
rdMesh = roadMesh(egoVehicle);
ptCloud = step(lidar, tgtPoses, rdMesh, time);

% Compute pose of ego vehicle to track in scenario frame. Typically
% obtained using an INS system. If unavailable, this can be set to
% "origin" to track in ego vehicle's frame.
egoPose = pose(egoVehicle);

end

radar2cental

레이다 상태공간의 트랙을 중앙 상태공간의 트랙으로 변환하는 함수입니다.

function centralTrack = radar2central(radarTrack)

% Initialize a track of the correct state size
centralTrack = objectTrack('State',zeros(10,1),...
    'StateCovariance',eye(10));

% Sync properties of radarTrack except State and StateCovariance with
% radarTrack See syncTrack defined below.
centralTrack = syncTrack(centralTrack,radarTrack);

xRadar = radarTrack.State;
PRadar = radarTrack.StateCovariance;

H = zeros(10,7); % Radar to central linear transformation matrix
H(1,1) = 1;
H(2,2) = 1;
H(3,3) = 1;
H(4,4) = 1;
H(5,5) = 1;
H(8,6) = 1;
H(9,7) = 1;

xCentral = H*xRadar;  % Linear state transformation
PCentral = H*PRadar*H'; % Linear covariance transformation

PCentral([6 7 10],[6 7 10]) = eye(3); % Unobserved states

% Set state and covariance of central track
centralTrack.State = xCentral;
centralTrack.StateCovariance = PCentral;

end

central2radar

중앙 상태공간의 트랙을 레이다 상태공간의 트랙으로 변환하는 함수입니다.

function radarTrack = central2radar(centralTrack)

% Initialize a track of the correct state size
radarTrack = objectTrack('State',zeros(7,1),...
    'StateCovariance',eye(7));

% Sync properties of centralTrack except State and StateCovariance with
% radarTrack See syncTrack defined below.
radarTrack = syncTrack(radarTrack,centralTrack);

xCentral = centralTrack.State;
PCentral = centralTrack.StateCovariance;

H = zeros(7,10); % Central to radar linear transformation matrix
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;  % Linear state transformation
PRadar = H*PCentral*H'; % Linear covariance transformation

% Set state and covariance of radar track
radarTrack.State = xRadar;
radarTrack.StateCovariance = PRadar;
end

syncTrack

State 속성과 StateCovariance 속성을 제외하고, 한 트랙의 속성을 또 다른 트랙과 동기화하는 함수입니다.

function tr1 = syncTrack(tr1,tr2)
props = properties(tr1);
notState = ~strcmpi(props,'State');
notCov = ~strcmpi(props,'StateCovariance');

props = props(notState & notCov);
for i = 1:numel(props)
    tr1.(props{i}) = tr2.(props{i});
end
end

pose

자차량의 자세를 구조체로 반환하는 함수입니다.

function egoPose = pose(egoVehicle)
egoPose.Position = egoVehicle.Position;
egoPose.Velocity = egoVehicle.Velocity;
egoPose.Yaw = egoVehicle.Yaw;
egoPose.Pitch = egoVehicle.Pitch;
egoPose.Roll = egoVehicle.Roll;
end

helperLidarDistance

레이다 상태공간의 트랙 추정값과 할당된 ground truth 사이의 정규화된 거리를 계산하는 함수입니다.

function dist = helperLidarDistance(track, truth)

% Calculate the actual values of the states estimated by the tracker

% Center is different than origin and the trackers estimate the center
rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2];
rot = quaternion([truth.Yaw truth.Pitch truth.Roll],'eulerd','ZYX','frame');
actPos = truth.Position(:) + rotatepoint(rot,rOriginToCenter')';

% Actual speed and z-rate
actVel = [norm(truth.Velocity(1:2));truth.Velocity(3)];

% Actual yaw
actYaw = truth.Yaw;

% Actual dimensions.
actDim = [truth.Length;truth.Width;truth.Height];

% Actual yaw rate
actYawRate = truth.AngularVelocity(3);

% Calculate error in each estimate weighted by the "requirements" of the
% system. The distance specified using Mahalanobis distance in each aspect
% of the estimate, where covariance is defined by the "requirements". This
% helps to avoid skewed distances when tracks under/over report their
% uncertainty because of inaccuracies in state/measurement models.

% Positional error.
estPos = track.State([1 2 6]);
reqPosCov = 0.1*eye(3);
e = estPos - actPos;
d1 = sqrt(e'/reqPosCov*e);

% Velocity error
estVel = track.State([3 7]);
reqVelCov = 5*eye(2);
e = estVel - actVel;
d2 = sqrt(e'/reqVelCov*e);

% Yaw error
estYaw = track.State(4);
reqYawCov = 5;
e = estYaw - actYaw;
d3 = sqrt(e'/reqYawCov*e);

% Yaw-rate error
estYawRate = track.State(5);
reqYawRateCov = 1;
e = estYawRate - actYawRate;
d4 = sqrt(e'/reqYawRateCov*e);

% Dimension error
estDim = track.State([8 9 10]);
reqDimCov = eye(3);
e = estDim - actDim;
d5 = sqrt(e'/reqDimCov*e);

% Total distance
dist = d1 + d2 + d3 + d4 + d5;

end

helperRadarDistance

레이다 상태공간의 트랙 추정값과 할당된 ground truth 사이의 정규화된 거리를 계산하는 함수입니다.

function dist = helperRadarDistance(track, truth)

% Calculate the actual values of the states estimated by the tracker

% Center is different than origin and the trackers estimate the center
rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2];
rot = quaternion([truth.Yaw truth.Pitch truth.Roll],'eulerd','ZYX','frame');
actPos = truth.Position(:) + rotatepoint(rot,rOriginToCenter')';
actPos = actPos(1:2); % Only 2-D

% Actual speed
actVel = norm(truth.Velocity(1:2));

% Actual yaw
actYaw = truth.Yaw;

% Actual dimensions. Only 2-D for radar
actDim = [truth.Length;truth.Width];

% Actual yaw rate
actYawRate = truth.AngularVelocity(3);

% Calculate error in each estimate weighted by the "requirements" of the
% system. The distance specified using Mahalanobis distance in each aspect
% of the estimate, where covariance is defined by the "requirements". This
% helps to avoid skewed distances when tracks under/over report their
% uncertainty because of inaccuracies in state/measurement models.

% Positional error
estPos = track.State([1 2]);
reqPosCov = 0.1*eye(2);
e = estPos - actPos;
d1 = sqrt(e'/reqPosCov*e);

% Speed error
estVel = track.State(3);
reqVelCov = 5;
e = estVel - actVel;
d2 = sqrt(e'/reqVelCov*e);

% Yaw error
estYaw = track.State(4);
reqYawCov = 5;
e = estYaw - actYaw;
d3 = sqrt(e'/reqYawCov*e);

% Yaw-rate error
estYawRate = track.State(5);
reqYawRateCov = 1;
e = estYawRate - actYawRate;
d4 = sqrt(e'/reqYawRateCov*e);

% Dimension error
estDim = track.State([6 7]);
reqDimCov = eye(2);
e = estDim - actDim;
d5 = sqrt(e'/reqDimCov*e);

% Total distance
dist = d1 + d2 + d3 + d4 + d5;

% A constant penality for not measuring 3-D state
dist = dist + 3;

end

helperRadarLidarFusionFcn

중앙 트랙 상태공간의 상태 및 상태 공분산을 융합하는 함수입니다.

function [x,P] = helperRadarLidarFusionFcn(xAll,PAll)
n = size(xAll,2);
dets = zeros(n,1);

% Initialize x and P
x = xAll(:,1);
P = PAll(:,:,1);

onlyLidarStates = false(10,1);
onlyLidarStates([6 7 10]) = true;

% Only fuse this information with lidar
xOnlyLidar = xAll(onlyLidarStates,:);
POnlyLidar = PAll(onlyLidarStates,onlyLidarStates,:);

% States and covariances for intersection with radar and lidar both
xToFuse = xAll(~onlyLidarStates,:);
PToFuse = PAll(~onlyLidarStates,~onlyLidarStates,:);

% Sorted order of determinants. This helps to sequentially build the
% covariance with comparable determinations. For example, two large
% covariances may intersect to a smaller covariance, which is comparable to
% the third smallest covariance.
for i = 1:n
    dets(i) = det(PToFuse(1:2,1:2,i));
end
[~,idx] = sort(dets,'descend');
xToFuse = xToFuse(:,idx);
PToFuse = PToFuse(:,:,idx);

% Initialize fused estimate
thisX = xToFuse(:,1);
thisP = PToFuse(:,:,1);
 
% Sequential fusion
for i = 2:n
    [thisX,thisP] = fusecovintUsingPos(thisX, thisP, xToFuse(:,i), PToFuse(:,:,i));
end

% Assign fused states from all sources
x(~onlyLidarStates) = thisX;
P(~onlyLidarStates,~onlyLidarStates,:) = thisP;

% Fuse some states only with lidar source
valid = any(abs(xOnlyLidar) > 1e-6,1);
xMerge = xOnlyLidar(:,valid);
PMerge = POnlyLidar(:,:,valid);

if sum(valid) > 1
    [xL,PL] = fusecovint(xMerge,PMerge);
elseif sum(valid) == 1
    xL = xMerge;
    PL = PMerge;
else
    xL = zeros(3,1);
    PL = eye(3);
end

x(onlyLidarStates) = xL;
P(onlyLidarStates,onlyLidarStates) = PL;

end

function [x,P] = fusecovintUsingPos(x1,P1,x2,P2)
% Covariance intersection in general is employed by the following
% equations: 
% P^-1 = w1*P1^-1 + w2*P2^-1 
% x = P*(w1*P1^-1*x1 + w2*P2^-1*x2);
% where w1 + w2 = 1 
% Usually a scalar representative of the covariance matrix like "det" or
% "trace" of P is minimized to compute w. This is offered by the function
% "fusecovint". However. in this case, the w are chosen by minimizing the
% determinants of "positional" covariances only.
n = size(x1,1);
idx = [1 2];
detP1pos = det(P1(idx,idx));
detP2pos = det(P2(idx,idx));
w1 = detP2pos/(detP1pos + detP2pos);
w2 = detP1pos/(detP1pos + detP2pos);
I = eye(n);

P1inv = I/P1;
P2inv = I/P2;

Pinv = w1*P1inv + w2*P2inv;
P = I/Pinv;

x = P*(w1*P1inv*x1 + w2*P2inv*x2);

end

참고 문헌

[1] Lang, Alex H., et al. "PointPillars: Fast encoders for object detection from point clouds." Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2019.

[2] Zhou, Yin, and Oncel Tuzel. "Voxelnet: End-to-end learning for point cloud based 3d object detection." Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2018.

[3] Yang, Bin, Wenjie Luo, and Raquel Urtasun. "Pixor: Real-time 3d object detection from point clouds." Proceedings of the IEEE conference on Computer Vision and Pattern Recognition. 2018.