KITTI数据集：点云投影到图像上 & Tracklets标注文件三维信息投影--官方代码使用

最近学习了一些自动驾驶的相关的投影知识，结合自身，做了小小的改进，得到了想要的效果，故记录下来！ 

KITTI数据集介绍

1个64线激光雷达：车身正中间

2个GRB相机：得到RGB图像数据

2个灰度图像：得到灰度图像

激光雷达和相机的坐标系：

激光雷达：

x：前   y：左  z：上

相机：

x：右  y：下  z：前

具体的安装位置（俯视图）如图：

更多详细介绍：The KITTI Vision Benchmark Suite

一、点云深度投影到图像

这里所说的深度信息是激光雷达距离物体的水平距离，根据以上的安装可以知道，也就是激光雷达的x轴信息。

1、数据准备

我这里下载的KITTI的数据集是2011_09_26_drive_0009的city数据集，查阅过相关资料的小伙伴可以在很多地方都可以看到这个数据集！（注意，这里最好下载箭头所指的校正过的图像数据，另外需要下载相应的校正txt文件）

下载地址：The KITTI Vision Benchmark Suite

2、下载代码：KITTI官方提供的demo，在KITTI官网主页（邮箱激活即可注册登录）下载如下的小demo

下载下来，可以看见里面文件夹中有run_demoVelodyne.m的matlab脚本文件，该代码就是KITTI官方给出的点云深度信息投影到图像的实例代码，结合KITTI的raw data数据集就可以实现点云投影到图像的效果。

下载地址：The KITTI Vision Benchmark Suite

3、运行过程中易出现的问题：

（1）因为使用MATLB的经验并欠缺，再加上官方给出的是函数的形式，相当于封装为一个整体，看不见中间运行产生的一些数值和结果，所以建议想对这方面有更多了解的可以修改代码的形式，这样可以通过“工作区”剖析相关的过程数据。

官方：可以通过在命令行窗口敲入：

run_demoVelodyne (base_dir,calib_dir)
% 将base_die和calib_dir文件夹替换为具体的文件夹路径即可成功运行

修改：将代码开头属于函数的部分给去掉

修改之后运行，就可以在工作区看到相应的运行结果数值

（2）官方给出的代码，直接运行是不会报错的，但是会发现投影点是完全一个颜色的，得不到想要的结果图。

以2011_0926_drive_0009_sync/image_03/0000000032.png数据为例：

直接跑官方给的代码：

很显然，全部投影点都是一个颜色，表示不出距离的信息，得不到投影的效果，这就需要自己修改代码。

4、修改后的结果

（1）彩色投影效果：

（2）自己添加：灰度图投影效果：

MATLAB中有许多的内置颜色图，这里使用的是jet（注意代码中要用小写！因为MATLAB中内置的是jet.m文件）

前四列分别为x y z，最后一列是反射强度

结果：（按照深度值（x轴数值）确定方向）

彩色：彩色则是暖色表示距离越近，冷色表示距离越远

灰度图：深色表示距离越近，浅色表示越远

最后修改完成的代码：

close all; dbstop error; clc;
disp('======= KITTI DevKit Demo =======');

% options (modify this to select your sequence)

base_dir  = 'D:\programs\Matlab2021\bin\matlab\data\2011_09_26_drive_0009_sync'; % 数据存放的文件夹
calib_dir = 'D:\programs\Matlab2021\bin\matlab\data\2011_09_26_drive_0009_sync'; %校准文件存放的文件夹

cam       = 2; % 选择cam2作为参考相机，也就是将激光雷达的点云投影到cam2相机上
frame     = 32; % 用的是数据的第几帧，这里可以自己修改数据查看不同数据的投影效果

% 使用到的两个校准文件，激光雷达-相机的外参和相机-相机的参数
calib = loadCalibrationCamToCam(fullfile(calib_dir,'calib_cam_to_cam.txt'));
Tr_velo_to_cam = loadCalibrationRigid(fullfile(calib_dir,'calib_velo_to_cam.txt'));

% compute projection matrix velodyne->image plane
R_cam_to_rect = eye(4);
R_cam_to_rect(1:3,1:3) = calib.R_rect{1};
P_velo_to_img = calib.P_rect{cam+1}*R_cam_to_rect*Tr_velo_to_cam; % 投影矩阵=激光雷达-相机的外参*相机的畸变校正参数*相机(cam+1)的校正参数

% load and display image
img = imread(sprintf('%s/image_%02d/data/%010d.png',base_dir,cam,frame)); % 格式强制性转化，KITTI的数据集都是这么定义的
fig = figure('Position',[20 100 size(img,2) size(img,1)]); axes('Position',[0 0 1 1]);
imshow(img); hold on;

% load velodyne points
fid = fopen(sprintf('%s/velodyne_points/data/%010d.bin',base_dir,frame),'rb');
velo = fread(fid,[4 inf],'single')';
velo = velo(1:5:end,:); % remove every 5th point for display speed 考虑到点云数据过大，每五个点投一个，速度会相对快一点
fclose(fid);

% remove all points behind image plane (approximation
% 考虑到激光雷达与相机的安装位置，滤除距离激光雷达5m之内的点
idx = velo(:,1)<5;  
velo(idx,:) = [];

% project to image plane (exclude luminance)
velo_img = project(velo(:,1:3),P_velo_to_img);

% plot points
cols = jet;  % 使用jet颜色图绘制彩色投影图
for i=1:size(velo_img,1)
  col_idx = round(256*5/velo(i,1));  % 根据实际场景修改，已达到投影显示距离远近的效果，最大256
  plot(velo_img(i,1),velo_img(i,2),'o','LineWidth',4,'MarkerSize',1,'Color',cols(col_idx,:));
end

% plot points灰度图

%cols = 1-[0:1/63:1]'*ones(1,3);
%for i=1:size(velo_img,1)
   %col_idx = round(64*5/velo(i,1));
   %plot(velo_img(i,1),velo_img(i,2),'o','LineWidth',4,'MarkerSize',1,'Color',cols(col_idx,:));
%end

二、Tracklets文件标注信息投影

1、数据准备

数据下载地址：The KITTI Vision Benchmark Suite

存放的位置最好是这样安放，这样的话代码中的文件绝对路径可以使用相同的

2、下载代码

代码下载地址：The KITTI Vision Benchmark Suite

3、拆分代码

为了方便看到运行过程数据，和使用的各个数据的情况，建议拆分函数，这样可以在工作区查看各个部分的数据情况。修改之后，右侧工作区的数据如图所示：

4、代码解读

运行的结果是包含所使用的的所有数据集，而不是像run_demoVelodyne.m一样指定帧数。代码中引入键盘值进行查看所有的数据，如下图代码所示：

q：退出

-：上一帧数据

x：+100帧的数据

y：-100帧的数据

除上述键的所有键值：下一帧数据

5、运行结果

注：MATLAB需要在.m文件所在的文件夹级别下运行程序

注：该代码是根据三维标注信息进行三维信息的获取，进而绘制出3D边界框；然后使用projectToImage.m文件将3D边界框投影到图像平面上，最后根据投影信息绘制出二维的边界框，并把objectType加上。3D边界框应该也可以加，但是该代码块加上会显示参数过多！

最后修改完成的代码：

close all; dbstop error; clc;
disp('======= KITTI DevKit Demo =======');

% 使用方法:
%   SPACE: next frame  下一帧
%   '-':   last frame  上一帧
%   'x':   +100 frames +100帧
%   'y':   -100 frames -100帧
%   'q':   quit        退出
%
% Occlusion Coding:
%   green:  not occluded     绿色：无遮挡
%   yellow: partly occluded  黄色：部分遮挡
%   red:    fully occluded   红色：完全遮挡
%   white:  unknown          白色：未知

% options (modify this to select your sequence)
% the base_dir must contain:
%   - the data directories (image_00, image_01, ..)
%   - the tracklet file (tracklet_labels.xml)
% the calib directory must contain:
%   - calib_cam_to_cam.txt
%   - calib_velo_to_cam.txt
% cameras:
%   - 0 = left grayscale
%   - 1 = right grayscale
%   - 2 = left color
%   - 3 = right color

base_dir ='D:\programs\Matlab2021\bin\matlab\data\2011_09_26_drive_0009_sync';
calib_dir = 'D:\programs\Matlab2021\bin\matlab\data\2011_09_26_drive_0009_sync';

cam = 2; % 参考相机

% get image sub-directory
image_dir = fullfile(base_dir, sprintf('/image_%02d/data', cam));

% get number of images for this dataset
nimages = length(dir(fullfile(image_dir, '*.png')));

% set up figure
gh = visualization('init',image_dir);

% read calibration for the day
[veloToCam, K] = loadCalibration(calib_dir);

% read tracklets for the selected sequence
tracklets = readTracklets([base_dir '/tracklet_labels.xml']); % slow version
%tracklets = readTrackletsMex([base_dir '/tracklet_labels.xml']); % fast version

% extract tracklets
% LOCAL OBJECT COORDINATE SYSTEM:
%   x -> facing right
%   y -> facing forward
%   z -> facing up
for it = 1:numel(tracklets)
  
  % shortcut for tracklet dimensions
  w = tracklets{it}.w;
  h = tracklets{it}.h;
  l = tracklets{it}.l;

  % set bounding box corners
  corners(it).x = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2]; % front/back
  corners(it).y = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2]; % left/right
  corners(it).z = [0,0,0,0,h,h,h,h];
  
  % get translation and orientation
  t{it} = [tracklets{it}.poses(1,:); tracklets{it}.poses(2,:); tracklets{it}.poses(3,:)];
  rz{it} = wrapToPi(tracklets{it}.poses(6,:));
  occlusion{it} = tracklets{it}.poses(8,:);
end

% 3D bounding box faces (indices for corners)
face_idx = [ 1,2,6,5   % front face
             2,3,7,6   % left face
             3,4,8,7   % back face
             4,1,5,8]; % right face

% main loop (start at first image of sequence)
img_idx = 0;
while 1
  
  % visualization update for next frame
  visualization('update',image_dir,gh,img_idx,nimages);
  
  % compute bounding boxes for visible tracklets
  for it = 1:numel(tracklets)
    
    % get relative tracklet frame index (starting at 0 with first appearance; 
    % xml data stores poses relative to the first frame where the tracklet appeared)
    pose_idx = img_idx-tracklets{it}.first_frame+1; % 0-based => 1-based MATLAB index

    % only draw tracklets that are visible in current frame
    if pose_idx<1 || pose_idx>(size(tracklets{it}.poses,2))
      continue;
    end

    % compute 3d object rotation in velodyne coordinates
    % 计算激光雷达坐标系中的3D坐标旋转矩阵
    % VELODYNE COORDINATE SYSTEM:
    %   x -> facing forward
    %   y -> facing left
    %   z -> facing up
    R = [cos(rz{it}(pose_idx)), -sin(rz{it}(pose_idx)), 0;
         sin(rz{it}(pose_idx)),  cos(rz{it}(pose_idx)), 0;
                             0,                      0, 1];

    % rotate and translate 3D bounding box in velodyne coordinate system
    % 在激光雷达坐标系中旋转和平移3D边界框
    corners_3D      = R*[corners(it).x;corners(it).y;corners(it).z];
    corners_3D(1,:) = corners_3D(1,:) + t{it}(1,pose_idx);
    corners_3D(2,:) = corners_3D(2,:) + t{it}(2,pose_idx);
    corners_3D(3,:) = corners_3D(3,:) + t{it}(3,pose_idx);
    corners_3D      = (veloToCam{cam+1}*[corners_3D; ones(1,size(corners_3D,2))]);
    
    % generate an orientation vector and compute coordinates in velodyneCS
    orientation_3D      = R*[0.0, 0.7*l; 0.0, 0.0; 0.0, 0.0];
    orientation_3D(1,:) = orientation_3D(1,:) + t{it}(1, pose_idx);
    orientation_3D(2,:) = orientation_3D(2,:) + t{it}(2, pose_idx);
    orientation_3D(3,:) = orientation_3D(3,:) + t{it}(3, pose_idx);
    orientation_3D      = (veloToCam{cam+1}*[orientation_3D; ones(1,size(orientation_3D,2))]);
    
    % only draw 3D bounding box for objects in front of the image plane
    if any(corners_3D(3,:)<0.5) || any(orientation_3D(3,:)<0.5) 
      continue;
    end

    % project the 3D bounding box into the image plane  将3D边界框投影到图像平面
    corners_2D     = projectToImage(corners_3D, K);
    orientation_2D = projectToImage(orientation_3D, K);
    drawBox3D(gh,occlusion{it}(pose_idx),corners_2D,face_idx,orientation_2D)
    
    % compute and draw the 2D bounding box from the 3D box projection
    %从3D边界框投影中计算2且绘制2D边界框
    box.x1 = min(corners_2D(1,:));
    box.x2 = max(corners_2D(1,:));
    box.y1 = min(corners_2D(2,:));
    box.y2 = max(corners_2D(2,:));
    drawBox2D(gh,box,occlusion{it}(pose_idx),tracklets{it}.objectType)
  end

  % force drawing and tiny user interface
  waitforbuttonpress; 
  key = get(gcf,'CurrentCharacter');
  switch lower(key)                         
    case 'q',  break;                                 % quit
    case '-',  img_idx = max(img_idx-1,  0);          % previous frame
    case 'x',  img_idx = min(img_idx+100,nimages-1);  % +100 frames
    case 'y',  img_idx = max(img_idx-100,0);          % -100 frames
    otherwise, img_idx = min(img_idx+1,  nimages-1);  % next frame
  end

end

关于自动驾驶的小学习结束！

发问：如果在无人船领域，如果使用16线的激光雷达（如VLP-16），激光雷达在海上扫描到的信息量很少，且由于海面的反射作用，海面的信息也无法扫描到，可能一个物体只有几条稀疏的点云数据。试问，在这种情况下，怎样充分利用激光雷达的点云数据与图像融合呢？

希望和大家一起学习交流自动驾驶、无人船的感知技术，尤其是多传感器融合的相关知识！