MOT （Beyond Pixels: Leveraging Geometry and Shape Cues for Online Multi-Object Tracking）demo阅读
变量：
-
all_wts:3D-3D,3D-2D和2D-2D权重 global_id：车辆总数 K_all：相机内参 angleMapdetectionsQ_dnos：原帧车辆回归框号 detectionsT_dnos：下一帧车辆回归框号 result_for_kitti_eval：预估结果 total_detecions总检测数 params_carCuboid(avgCar_Sz, sz_ub,sz_lb)：车辆参数 sz_lb：车参上界 sz_ub：车参下界 avgCar_Sz[l, h, w]：车参：长，宽，高 params_2D3D = struct( K, n, h); K：相机内参 n[0，1，0]：道路平面 h：摄像机离地高度 detection = struct( 'dno', -1.0, ... 'bbox', zeros（4,1), ... 'sigma_2D', zeros(3,3), ... 'yaw', 0.0, ... 'bvolume', [], ... 'bvolume_proj', [], ... 'k', [], ... 'origin', zeros(3,1), ... 'start_frame', -1.0, ... 'end_frame', -1.0, ... 'sigma_3D', zeros(4,4) ... ); dno：车辆号 bbox：车辆检测区域 sigma_2D：2D激活函数 yaw：偏航角 bvolume：3D边界体积（bvolume），它是将偏航和LBH错误应用于平均汽车长方体边界后得到的点的凸包bvolume_projk：边界体积凸包 origin：车辆位置原点 start_frame：起始帧 end_frame：终止帧 sigma_3D：3D激活函数 num_matches：匹配数 pose：Monocular ORBSLAM获取参数[平移量，旋转量，偏航量] detections：车辆检测矩阵 feautes_2D2D：图像特征矩阵 start_fno：起始帧号 end_fno：终止帧号 scoreMatrices_for_seq(end_fno, 1)当前帧得分矩阵 first_time：是否是首帧 num_detectionsQ：当前帧检测出的车辆数 num_detectionsT：下一帧检测出的车辆数 detectionsQ：当前帧检测出的车辆位置回归框 detectionsT：下一帧检测出的车辆位置回归框 motion[1*4]：帧移动量
函数
-
getCanonicalCuboid(avg_sz):在图像二维图像上生成长方体 getOffsetBasedOnYaw([车宽，车高]，yaw):计算要应用到X的偏移量，即回归框底线的中心，以实际到达车辆的中心。此偏移只能应用于Z方向。 generateScoreMatrices(detectionsQ, detectionsT)：生成得分矩阵 get3D3DMatchScore(detectionsT{j}.bvolume, detectionsQ{i}.bvolume)：3D车辆物体相交程度 get3D2DMatchScore(detectionsT{j}.bbox, detectionsQ{i}.bvolume_proj)：3d投影相交程度 generate2D2DScoreMatrix(features_2D2D{i}, features_2D2D{i+1})：车辆回归框特征相似度矩阵
初始化权重，车参，结构体，导入pose矩阵，bounding box矩阵，feature矩阵，angle_map矩阵：
addpath('../third_party');
% 33 32 22 vp
all_wts = [0.6 0.4 0.2 0.0];
dcolors = distinguishable_colors(800);
for ww = 1:size(all_wts,1)
c33 = all_wts(ww,1);
c32 = all_wts(ww,2);
c22 = all_wts(ww,3);
cvp = all_wts(ww,4);
% 道路平面和相机高度
n = [0; 1; 0];
h = 1.72;
% 平均车参 avgCar_Sz = [4.3; 2; 2];
sz_ub = [34; 31.5; 31.5];
sz_lb = [-34; -31.5; -31.5];
K_all = load('../Data/calib/calib_all_test.txt');
angleMap = containers.Map([ 90:270 90:-1:0 360:-1:270], [0:-1:-180 0:90 90:180]);
hungarian_association = false;
detectionsQ_dnos = [];
detectionsT_dnos = [];
for s = [2]
seqNo = s;
result_for_kitti_eval = [];
total_detecions = 0;
%% SETUP PARAMETERS
image_path = sprintf('../Data/images/image_02/test/%04d',seqNo);
pose_path = sprintf('../Data/ORBSLAM_pose/test/%04d/KITTITrajectoryComplete_new',seqNo);
K = [ K_all(seqNo+1,1:3); K_all(seqNo+1,5:7); K_all(seqNo+1,9:11)];
% 定义车辆参数，相机参数和车辆结构体
params_carCuboid = struct('avg_Sz', avgCar_Sz, ...
'sz_ub', sz_ub, ...
'sz_lb', sz_lb ...
);
params_2D3D = struct('K', K, ...
'n', n, ...
'h', h ...
);
detection = struct( 'dno', -1.0, ...
'bbox', zeros(4,1), ...
'sigma_2D', zeros(3,3), ...
'yaw', 0.0, ...
'bvolume', [], ...
'bvolume_proj', [], ...
'k', [], ...
'origin', zeros(3,1), ...
'start_frame', -1.0, ...
'end_frame', -1.0, ...
'sigma_3D', zeros(4,4) ...
);
num_matches = 0;
num_false_positive = 0;
%% SETUP STRUCTS
global_id = 1;
%% 导入数据 :
% load pose (Monocular ORBSLAM)
pose = load(pose_path);
%load all detections cell array - variable name 'detections'
load(sprintf('../Data/RRC_Detections_mat/test/%04d/detections_rrc_test_%02d.mat', seqNo,seqNo));
% feautes_2D2D
load(sprintf('../Data/Features2D_mat/test/%04d/features_2_rrc_test_%02d.mat', seqNo,seqNo));
start_fno = 1;
end_fno = 35;%size(pose,1);
scoreMatrices_for_seq = cell(end_fno, 1);
%% RUN SCORING
first_time = 1;
tempDQ = [];
figure(1);
for i = 1:end_fno - 1
disp(sprintf('Seq<%02d> | frame : %04d', seqNo, i));
if(first_time)
num_detectionsQ = size(detections{i},1);
end
num_detectionsT = size(detections{i+1},1);如果输入是图像第一帧，将每个车辆位置，车辆序号，车辆偏航角，车辆3D转2D举证填入结构体，因为是第一帧，每填入一辆车，车号加一：
if(first_time)
% load the detections in to the query and train struct cell arrays x1 y1 x2 y2 confidence ID
for j = 1:num_detectionsQ
detectionsQ{j} = detection; % emtpy detection struct
detectionsQ{j}.dno = global_id;
detectionsQ{j}.bbox = detections{i}(j,1:4);
detectionsQ{j}.yaw = deg2rad(-90) ;
detectionsQ{j}.sigma_3D = [1.3 0 0 0; 0 1.1 0 0; 0 0 1.1 0; 0 0 0 deg2rad(0)];
global_id = global_id + 1; % increment the global ID
end
end非第一帧，将当前帧下一帧的检测出来的所有车辆序号置一，填入当前帧下一帧的位置填入，偏航角置为90°，填入3D激活函数：
for j = 1:num_detectionsT
detectionsT{j} = detection; % emtpy detection struct
detectionsT{j}.dno = -1; % will be given after association
detectionsT{j}.bbox = detections{i+1}(j,1:4);
detectionsT{j}.yaw = deg2rad(-90) ;
detectionsT{j}.sigma_3D = [1.3 0 0 0; 0 1.1 0 0; 0 0 1.1 0; 0 0 0 deg2rad(0)];;%
%1.6, 1.1, 1.1 : 0.8 0.1 0.1
end计算两帧之间摄像头的平移，偏航角，旋转角变化，将θ置为0，然后将motion归一化：
motion = [-(pose(i+1,11)-pose(i,11));
-(pose(i+1,12)-pose(i,12));
-(pose(i+1,13)-pose(i,13));
deg2rad(0)];
% scale the motion
motion(1:3,1) = motion(1:3,1).*(1.72/44);将前后两帧的结构体，车参，相机参数，摄像头移动参数输入，完善结构体数据：
[detectionsQ, detectionsT] = propagateDetections(detectionsQ, ...
detectionsT, ...
params_2D3D, ...
params_carCuboid, ...
motion ...
);其中，propagateDetections函数将2D图像转成3D,公式为
：
for i = 1:length(detectionsQ)
b1Q = [];
B1Q = [];
bvolumeQ1 = [];
% check if the detection is first time i.e. we have to start with
% the canonical cuboid. Else we already have the cuboid.
if(length(detectionsQ{i}.bvolume) == 0 )
%获取标准长方体
canonicalCuboid = getCanonicalCuboid( params_carCuboid.avg_Sz);
% 找寻底部中心
b1Q = [detectionsQ{i}.bbox(1) + (detectionsQ{i}.bbox(3) - detectionsQ{i}.bbox(1))/2 ;
detectionsQ{i}.bbox(4);
1.0];
% 投影成3d
B1Q = (params_2D3D.h * inv(params_2D3D.K) * b1Q) / (params_2D3D.n' * inv(params_2D3D.K) * b1Q);
% 应用偏移量，这是偏航的一个函数，并获得汽车的原点（记住近似值）
offset_Z = getOffsetBasedOnYaw([params_carCuboid.avg_Sz(1); params_carCuboid.avg_Sz(3)], detectionsQ{i}.yaw);
% 当前帧中的汽车原点
B1Q = B1Q + [0; 0; offset_Z];
% 转换成标准长方体
canonicalCuboid = canonicalCuboid + repmat(B1Q', 8,1);
% 获取当前帧边界体积
[bvolumeQ1, k1] = getBoundingVolume(B1Q, canonicalCuboid, detectionsQ{i}.yaw, ...
detectionsQ{i}.sigma_3D, ...
params_carCuboid.sz_ub, params_carCuboid.sz_lb ...
);
bvolumeQ1 = bvolumeQ1 - repmat([0 params_carCuboid.avg_Sz(2)/2 0], size(bvolumeQ1,1), 1);
else
B1Q = detectionsQ{i}.origin;
bvolumeQ1 = detectionsQ{i}.bvolume;
end
% TODO
% propagate noise and find new noise .... for now same noise
%
% 车在当前帧的坐标原点
B2Q = motion(1:3) + B1Q ;
bvolumeQ1 = bvolumeQ1 + repmat(motion(1:3)', size(bvolumeQ1,1),1);
% CUBOID IN TRAIN FRAME
[bvolumeQ2, k2] = getBoundingVolume(B2Q, bvolumeQ1, motion(4), ...
detectionsQ{i}.sigma_3D, ...
params_carCuboid.sz_ub, params_carCuboid.sz_lb ...
);
% offset the h/2 as car ar to be on road
% 计算边界体积投影并保存为bvolume_proj变量
bvolume_proj = (params_2D3D.K*bvolumeQ2')';
bvolume_proj(:,1:3) = bvolume_proj(:,1:3)./repmat(bvolume_proj(:,3), 1,3);
kbvolume_proj = convhull(bvolume_proj(:,1:2));
bvolume_proj = bvolume_proj(kbvolume_proj,:);
% 更新结构体数据
detectionsQ{i}.bvolume = bvolumeQ2;
detectionsQ{i}.bvolume_proj = bvolume_proj;
detectionsQ{i}.yaw = detectionsQ{i}.yaw+motion(4);
detectionsQ{i}.origin = B2Q;
detectionsQ{i}.k = k2;
end将结构体输入得分矩阵计算函数，获取2D-2D，3D-3D和3D-2D得分，并将2D-2D得分矩阵归一化：
[scoreMat_3D3D, scoreMat_3D2D] = generateScoreMatrices(detectionsQ, detectionsT);
[scoreMat_2D2D] = generate2D2DScoreMatrix(features_2D2D{i}, features_2D2D{i+1});
%normalize scoreMat2D2D from -1:1 to 0:1
max2D2D = max(scoreMat_2D2D');
max2D2D = repmat(max2D2D', 1, size(scoreMat_2D2D,2));
min2D2D = min(scoreMat_2D2D');
min2D2D = repmat(min2D2D', 1, size(scoreMat_2D2D,2));
scoreMat_2D2D = (scoreMat_2D2D - min2D2D) ./ (max2D2D-min2D2D +0.0000001) ;
scoreMat_viewpoint = zeros(size(scoreMat_3D3D)); 其中，generateScoreMatrices函数输入前后两帧的结构体，生成3D-3D和3D-2D得分矩阵：
function [scoreMatrix_3D3D, scoreMatrix_3D2D] = generateScoreMatrices(detectionsQ, detectionsT)
scoreMatrix_3D3D = zeros(length(detectionsQ), length(detectionsT));
scoreMatrix_3D2D = zeros(length(detectionsQ), length(detectionsT));
% 3D 3D 得分
for i = 1:length(detectionsQ)
for j = 1:length(detectionsT)
scoreMatrix_3D3D(i,j) = get3D3DMatchScore(detectionsT{j}.bvolume, detectionsQ{i}.bvolume);
scoreMatrix_3D2D(i,j) = get3D2DMatchScore(detectionsT{j}.bbox, detectionsQ{i}.bvolume_proj);
end
end
end其中，get3D3DMatchScore函数：
function score = get3D3DMatchScore(bvolumeQ, bvolumeT)
% 获取物体底部位置; 接着计算3D-3D得分
kbvolumeQ_xz = convhull(bvolumeQ(:,1), bvolumeQ(:,3));%计算物体凸包
bvolumeQ_xz = [bvolumeQ(kbvolumeQ_xz,1) bvolumeQ(kbvolumeQ_xz,3)];
kbvolumeT_xz = convhull(bvolumeT(:,1), bvolumeT(:,3));
bvolumeT_xz = [bvolumeT(kbvolumeT_xz,1) bvolumeT(kbvolumeT_xz,3)];
% 比较车辆位置和搜索区域交集
[x_in, y_in] = polybool('intersection', bvolumeQ_xz(:,1), bvolumeQ_xz(:,2), bvolumeT_xz(:,1), bvolumeT_xz(:,2));
% 计算车辆位置在搜索区域占比
area_in = polyarea(x_in, y_in);
bvolumeQ_xz_area = polyarea(bvolumeQ_xz(:,1), bvolumeQ_xz(:,2));
score = area_in / bvolumeQ_xz_area;
endget3D2DMatchScore函数：
% 计算车辆位置与搜索区域交集
function [score] = get3D2DMatchScore(bbox, searchRegion)
convShapeQ = [bbox(1) bbox(2);
bbox(3) bbox(2);
bbox(3) bbox(4);
bbox(1) bbox(4)];
convShapeT = searchRegion;
% find the intersection of bbox and search region
[x_in, y_in] = polybool('intersection', convShapeQ(:,1), convShapeQ(:,2), convShapeT(:,1), convShapeT(:,2));
% compute percentage area of bbox i.e. Q present in the search region
area_in = polyarea(x_in, y_in);
convShapeQ_area = polyarea(convShapeQ(:,1), convShapeQ(:,2));
score = area_in / convShapeQ_area;
end将2D-2D，3D-3D和3D-2D得分加权相加后保存到scoreMat_all：
if(~isempty(scoreMat_2D2D))
scoreMat_3D2D_mask = (scoreMat_3D2D > 0);
scoreMat_2D2D = scoreMat_2D2D .* scoreMat_3D2D_mask;
scoreMat_all = c33*scoreMat_3D3D + c32*scoreMat_3D2D + c22*scoreMat_2D2D + cvp*scoreMat_viewpoint;
else
scoreMat_all = c33*scoreMat_3D3D + c32*scoreMat_3D2D + cvp*scoreMat_viewpoint;
end 使用匈牙利算法计算两个物体间的联系：
if(hungarian_association)
% 使用Hungarian算法找循两个物体间的联系
[assign, cost]= munkres(-scoreMat_all);
% give IDs to the Ts based on assignment or give new ID
for ii = 1:length(assign)
% ADD: scoreMat_all(ii,assign(ii)) > 0.005 , to handle
% death and birth issues. Not very useful though.
if(assign(ii) > 0 && scoreMat_all(ii,assign(ii)) > 0.0 )
detectionsT{assign(ii)}.dno = detectionsQ{ii}.dno;
end
end
end若没有使用匈牙利算法，则通过加权得分矩阵判断前后两帧检测的物体是同一辆车：
if(~hungarian_association)
for ii = 1:length(detectionsQ)
scoreQT = scoreMat_all(ii,:);
[max_v, max_id] = max(scoreQT)
% 如果最大得分超过阈值，则判定与前一帧检测出的车辆相同
if(max_v > 0.15)
% give the Q's id to matching T
detectionsT{max_id}.dno = detectionsQ{ii}.dno;
end
end
end若当前帧的车辆未在下一帧找到匹配车辆，车辆总数加1：
for ii = 1:length(detectionsT)
if(detectionsT{ii}.dno == -1)
detectionsT{ii}.dno = global_id;
global_id = global_id + 1;
end将下一帧结构体赋值给当前帧，继续循环：
first_time = 0;
tempDQ = detectionsQ;
detectionsQ = detectionsT;