处理点云数据(五)：坐标系的转换

相机的内外参矩阵

坐标系

主要有两类坐标系，一类为相机坐标系，一类为世界坐标系(即物体所处真实世界)

内参矩阵

设空间中有一点P，若世界坐标系与相机坐标系重合，则该点在空间中的坐标为(X, Y, Z)，其中Z为该点到相机光心的垂直距离。设该点在像面上的像为点p，像素坐标为(x, y)。



由上图可知，这是一个简单的相似三角形关系，从而得到



但是，图像的像素坐标系原点在左上角，而上面公式假定原点在图像中心，为了处理这一偏移，设光心在图像上对应的像素坐标为(cx,cy)，则

外参矩阵

以上情况是世界坐标系与相机坐标系重合的特殊情况，而在一般情况下，世界坐标系中的某一点P要投影到像面上时，先要将该点的坐标转换到相机坐标系下。设P在世界坐标系中的坐标为X，P到光心的垂直距离为s（即上文中的Z），在像面上的坐标为x，世界坐标系与相机坐标系之间的相对旋转为矩阵R（R是一个三行三列的旋转矩阵），相对位移为向量T（三行一列），则



其中[R T]是一个三行四列的矩阵，称为外参矩阵，它和相机的参数无关，只与相机在世界坐标系中的位置有关。

点云到相机
4000
的坐标转换

在第 i 个相机图像中，经过矫正的相机坐标3D点 X=(x,y,z,1)T 到点 Y=(u,v,1)T的投影表示为：Y=P(i)rectX

其中，

P(i)rect=⎛⎝⎜⎜f(i)u000f(i)v0ciuciv1−f(i)ubix00⎞⎠⎟⎟

为第i个相机的投影矩阵。bix表示相对于参考相机0的基线（以米为单位)。为了在参考相机坐标中将3D点X投影到第i个相机图像面上的点Y，相机0的矫正旋转矩阵R(0)rect 也必须考虑进去，所以：

Y=P(i)rectR(0)rectX

在这里，R(0)rect 被扩展成了4*4的矩阵，令R(4, 4) = 1，其余为0。



最后再考虑点云到相机的外参矩阵，即Tcamvelo

Y=P(i)rectR(0)rectTcamveloX

在上一节代码中：

内参矩阵？(应该是由相机的内参矩阵得到的，彩色摄像机和灰度摄像机有区别)：calib.P_rect{cam+1}

基于相机0的旋转矩阵：R_cam_to_rect

外参矩阵：Tr_velo_to_cam

投影矩阵=内参矩阵 * 基于相机0的旋转矩阵 * 外参矩阵

P_velo_to_img = calib.P_rect{cam+1} * R_cam_to_rect * Tr_velo_to_cam

% 计算点云到图像平面的投影矩阵
R_cam_to_rect = eye(4);
R_cam_to_rect(1:3,1:3) = calib.R_rect{1};  % 参考相机0到相机xx图像平面的旋转矩阵
P_velo_to_img = calib.P_rect{cam+1}*R_cam_to_rect*Tr_velo_to_cam; % 内外参矩阵

点云坐标：p_in

点云到图像的投影矩阵：T

function p_out = project(p_in,T)
% p_in为点云坐标，T为点云到图像的投影矩阵

% 数据和投影矩阵的维数
dim_norm = size(T,1); % 3维
dim_proj = size(T,2); % 4维

% 在齐次坐标中进行转换
p2_in = p_in;
if size(p2_in,2)<dim_proj
p2_in(:,dim_proj) = 1;
end
p2_out = (T*p2_in')';

% 归一化齐次坐标：
p_out = p2_out(:,1:dim_norm-1)./(p2_out(:,dim_norm)*ones(1,dim_norm-1));

相机的标定

相机的标定，即为通过某个已知的目标，求取相机内参矩阵的过程。最常用的标定目标就是棋盘格。用相机对棋盘格从不同角度拍摄多张照片，然后将这些照片导入标定程序或算法，即可自动求出相机的内参。

matlab中有自带的APP可以进行相机的标定：https://www.ilovematlab.cn/thread-267670-1-1.html