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

相机的内外参矩阵

坐标系

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

内参矩阵

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

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

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

外参矩阵

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

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

点云到相机的坐标转换

在第 i 个相机图像中，经过矫正的相机坐标3D点 $X=(x,y,z,1{)}^T$ 到点 $Y=(u,v,1{)}^T$的投影表示为：$$Y=P_{rect}^{(i)}X$$
其中，
$$P_{rect}^{(i)}=\left(\begin{array}{cccc}{f}_u^{(i)}& 0& c_u^i& -f_u^{(i)}{b}_x^i\\ 0& f_v^{(i)}& c_v^i& 0\\ 0& 0& 1& 0\end{array}\right)$$
为第i个相机的投影矩阵。$ b_{x}^{i} $表示相对于参考相机0的基线（以米为单位)。为了在参考相机坐标中将3D点X投影到第i个相机图像面上的点Y，相机0的矫正旋转矩阵$ R_{rect}^{(0)} $ 也必须考虑进去，所以：
$$Y=P_{rect}^{(i)}{R}_{rect}^{(0)}X$$
在这里，$ R_{rect}^{(0)} $ 被扩展成了4*4的矩阵，令R(4, 4) = 1，其余为0。

最后再考虑点云到相机的外参矩阵，即$T_{velo}^{cam}$
$$Y=P_{rect}^{(i)}{R}_{rect}^{(0)}{T}_{velo}^{cam}X$$
在上一节代码中：
内参矩阵？(应该是由相机的内参矩阵得到的，彩色摄像机和灰度摄像机有区别)：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

补充

点云的坐标系：X(前)，Y(左)，Z(上)；
相机的坐标系：X(右)，Y(下)，Z(前)；
即X—>Z(0,0,1),Y—>-X(-1,0,0),Z—>-Y(0,-1,0)；
点云坐标系到相机坐标系的旋转矩阵应该为：
$\left(\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\\ 1& 0& 0\end{array}\right)$

MATLAB 四元数转R旋转矩阵的转换顺序为w,x,y,z；
坐标系2到坐标系1的T转换公式为：$T_{2->1}=T_{1->0}^{-1}\ast T_{2->0}$；