利用深度图建立三维点云地图笔记

  前言：这几天在独立地研究对RGBD图像序列，建立其三维点云地图。这是我研究生期间，毕业论文中的一点小工作。由于我并没有借鉴像RTAB-MAP等SLAM方法，所以本文仅仅能够帮助学习和理解是三维建图的过程，对于实际的三维建图应用，意义并不大。

  本文的方法非常的简单粗暴，思路是首先求取每一帧深度图像的位姿，其次，将每一帧深度图转换为点云，最后将点云转换到世界坐标系下。对于求取深度图的位姿，可以使用ORB-SLAM2算法得到（如果有小车机器人平台的话，也可以通过其里程计得到）；对于深度图转点云，可以通过投影模型计算点云坐标；对于将点云转换到世界坐标系下，仅通过三维刚体变换得到。

深度图转点云

  对于求RGBD图像序列的位姿，网上有很多资料和教程帮助初学者配置ORB-SLAM2，并跑通自己的数据集，这里不做介绍。对于将深度图像转换为点云，这里着重介绍一下。首先是相机的投影模型，深度图像和点云之间的转换如下式所示：
$$\{\begin{array}{rlr}z& =& d\\ x& =& \frac{u-c_x}{f_x}\cdot z\\ y& =& \frac{v-c_y}{f_y}\cdot z\end{array}$$
  其中x,y,z是相机坐标系下的三维坐标，u,v为图像中像素的位置（所在的行和列），$c_x$和$c_y$是相机光学在图像坐标系下的坐标，若相机没有畸变，$c_x$和$c_y$应分别为$\frac{W}{2}$和$\frac{H}{2}$，其中W和H分别为图像的宽度和高度。$f_x$和$f_y$分别为相机在x轴和y轴的焦距。下面是深度图像转点云图的matlab代码（depth2pointcloud.m）：
（下面代码部分参考了https://blog.csdn.net/u013925378/article/details/82978995）

function [ pt ] = depth2pointcloud( depth_image, fx, fy, cx, cy, depth_factor )
% fx,fy,cx,cy分别为x轴和y轴的焦距和图像中心，它们均是相机内参。
% depth_factor = 1000.0; % depthMapFactor， 深度图像中的像素是unsigned char类型，它通常是16
% 位，将（u,v）对应位置的像素的值除以depth_factor，就能够得到该位置所对应的三维空间的尺度。
[h, w] = size(depth_image);
u = repmat(1:w, [h, 1]);
v = repmat(1:h, [w, 1])';
% 利用投影模型，求图像中每个像素所对应的三维空间坐标。
ptz = double(depth_image) / depth_factor;
ptx = (ptz .* (double(u) - cx)) / fx;
pty = (ptz .* (double(v) - cy)) / fy;
% 上面是利用matlab矩阵的特性进行的操作，直接求取的整个图像的x,y,z坐标，也可以使用两个for循环，挨个儿遍历像素的方法求。
% 通常无法求得深度的位置的像素值为0，因此需要删除无用的点。
ptx(ptz(:) == 0) = [];
pty(ptz(:) == 0) = [];
ptz(ptz(:) == 0) = [];
% 由于不同的相机的深度测量范围不同，因此深度太大的值可能会不准确，同样删除掉。下面含义为删除测量距离>10m的点。
if sum(ptz(:) > 10)
    ptx(ptz(:) > 10) = [];
    pty(ptz(:) > 10) = [];
    ptz(ptz(:) > 10) = [];
end
pt = [ptx(:), pty(:), ptz(:)];
end

利用该代码，得到转换后深度图在转换为点云前后的想过如下图所示：

其中左图为彩色图，中间为其对应的深度图，右边为深度图转换成点云后的点云图。

点云拼接

  在点云拼接中，相机在运动过程中的位姿轨迹由里程计得到，因此只要将不同位姿下的每一帧深度图对应的点云的坐标转换到世界坐标系下即可。先展示效果，再分析代码和过程。

建图效果：

真实环境及轨迹：

上图是建立的点云地图效果，左边为实验环境，相机安装在小车上，先走“线路1”直线，转90°，再走“线路2”直线。直接拼接点云的matlab代码如下：

close all; clear all; clc

% --------------------加载bag文件：--------------------
f = '/home/robot/bags0511/2019-05-11-23-28-26.bag';  
bg = rosbag(f);

% --------------------获取里程计的位姿，时间戳等信息：--------------------
whmsg = select(bg, 'MessageType', 'nav_msgs/Odometry')
wh = readMessages(whmsg);
len_wh = length(wh);
tws = zeros(len_wh, 1);
poses = zeros(len_wh, 7);
for dn = 1 : len_wh
    t1 = wh{dn, 1}.Header.Stamp.Sec;
    t2 = wh{dn, 1}.Header.Stamp.Nsec;
    tws(dn) = str2double([num2str(t1) '.' num2str(t2)]);
    pose = wh{dn, 1}.Pose.Pose;
    poses(dn, :) = [pose.Position.X, pose.Position.Y, pose.Position.Z+0.4, pose.Orientation.X, pose.Orientation.Y, pose.Orientation.Z, pose.Orientation.W];
    euler = quatern2euler([pose.Orientation.W, pose.Orientation.X, pose.Orientation.Y, pose.Orientation.Z]);
    yaw(dn) = euler(:,3);
end
% poses中保存的内容为：x,y,z,qx,qy,qz,qw
dlmwrite('pose.txt', poses, 'delimiter', ' ');
yaw = rad2deg(yaw);
tws = sort(tws);

% --------------------获取深度图以及时间戳：--------------------
dpmsg = select(bg, 'MessageType', 'sensor_msgs/Image', 'Topic', '/camera/depth/image_rect_raw');
dp = readMessages(dpmsg);
h = dp{1, 1}.Height;
w = dp{1, 1}.Width;
len_dp = length(dp);
tds = zeros(len_dp, 1);
for dn = 1 : len_dp
    t1 = dp{dn, 1}.Header.Stamp.Sec;
    t2 = dp{dn, 1}.Header.Stamp.Nsec;
    tds(dn) = str2double([num2str(t1) '.' num2str(t2)]);
end
tds = sort(tds);

% --------------------找出里程计与相机对齐的时间戳：--------------------
idxs = zeros(len_wh, 1);
for n = 1 : len_wh
    [mins(n), idx] = min(abs(tds-tws(n)));
    idxs(n) = idx;
end

% --------------------相机内参：--------------------
fx = 383.353;
fy = 383.353;
cx = 315.986;
cy = 236.030;
depth_factor = 1000.0; % depthMapFactor
% --------------------点云拼接，并保存相机点云运动过程为gif图：--------------------
h = figure;
axis tight manual  % 保证getframe()函数返回的帧是固定的大小
filename = 'testAnimated.gif';
pts = [];
for n = 1 : length(idxs)
    img = readImage(dp{(idxs(n)), 1});
    pt = depth2pointcloud(img, fx, fy, cx, cy, depth_factor);
    % 得到的pt是相机坐标系下的点云，在本实验使用的相机中，其坐标系为：前z，右x，下y；
    % 而里程计的坐标为：前x，左y，上z；
    % 因此在旋转之前要想把它们的坐标系统一，这里将相机坐标系转到里程计坐标系下：
    [x, y] = rotateXY(pt(:,3), -pt(:,1), -yaw(n));
    % 相机的位置在车体坐标系下的（0.24, 0, 0.4）位置，即在车体质心前方0.24m，高0.4m的地方；
    % 0.24*cos(deg2rad(-yaw(n)))和0.24*sin(deg2rad(-yaw(n)))表示将车体坐标系的坐标转到相机坐标系下；
    pcshow([x+poses(n, 1)+0.24*cos(deg2rad(-yaw(n))), y+poses(n, 2)+0.24*sin(deg2rad(-yaw(n))), -pt(:,2)])
    axis([0 15 -10 10 -2 6]);
    view(270, 60)
    drawnow
    % 保存相机点云运动gif格式（如上面的动图所示）
    frame = getframe(h); 
      im = frame2im(frame); 
      [imind,cm] = rgb2ind(im,256); 
      % Write to the GIF File 
      if n == 1 
          imwrite(imind,cm,filename,'gif', 'Loopcount',inf); 
      else 
          imwrite(imind,cm,filename,'gif','WriteMode','append'); 
      end 
      % 点云拼接；
    pts = cat(1, pts, [x+poses(n, 1)+0.24*cos(deg2rad(-yaw(n))), y+poses(n, 2)+0.24*sin(deg2rad(-yaw(n))), -pt(:,2)]);
end
% 下面是以10倍采样，保持点云的所有xyz坐标，运行时间很长；
% dlmwrite('points.txt', pts(1:10:end, :));

  函数rotateXY.m的内容如下：

function [outx,outy] = rotateXY(x,y, theta)
% 将列向量点集的x和y坐标旋转theta度，返回旋转后的坐标
xy = [x, y];
theta = deg2rad(theta);
R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
Rxy = R * xy';
outx = Rxy(1,:)';
outy = Rxy(2,:)';
end

  后记，本程序的软件运行环境为ubuntu16.04，MATLAB2015版本。上述代码中的bag文件请见百度云链接链接: https://pan.baidu.com/s/1i4YyuXUUhJQMyDm59umhYQ 提取码: 3itz ，若有需要，读者可以下载尝试运行。需要注意，该代码仅适用于二维平面的运动过程，且仅对本数据有效，因为其它数据的车体和相机的坐标系不一定和本实验的一致，若要运行其它数据，需要做相应的修改。还有，如果使用matlab在运行时报如下错误：

可在，预设(Preference)->常规(General)->Java堆内存(Java Heap Memory)，将其改到最大，通常可以解决问题。但是如果bag文件很大，Java Heap Memory不够，仍然会报该错误，此时就无力回天了。如果有人有解决办法，请一定告诉我。