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

首先给大家推荐一下我老师大神的人工智能教学网站。教学不仅零基础,通俗易懂,而且非常风趣幽默,还时不时有内涵黄段子!点这里可以跳转到网站

相机的内外参矩阵

坐标系

1

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

内参矩阵

2

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

3
4
5

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

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

外参矩阵

6

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

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

点云到相机的坐标转换

1

在第 i 个相机图像中,经过矫正的相机坐标3D点 X=(x,y,z,1)TX = (x, y, z, 1)^TX=(x,y,z,1)T 到点 Y=(u,v,1)TY = (u, v, 1)^TY=(u,v,1)T的投影表示为:Y=Prect(i)X Y = P_{rect}^{(i)}X Y=Prect(i)​X
其中,
Prect(i)=(fu(i)0cui−fu(i)bxi0fv(i)cvi00010) P_{rect}^{(i)}= \begin{pmatrix} 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{pmatrix} Prect(i)​=⎝⎜⎛​fu(i)​00​0fv(i)​0​cui​cvi​1​−fu(i)​bxi​00​⎠⎟⎞​
为第i个相机的投影矩阵。$ b_{x}^{i} 表示相对于参考相机0的基线(以米为单位)。为了在参考相机坐标中将3D点X投影到第i个相机图像面上的点Y,相机0的矫正旋转矩阵表示相对于参考相机0的基线(以米为单位)。为了在参考相机坐标中将3D点X投影到第i个相机图像面上的点Y,相机0的矫正旋转矩阵表示相对于参考相机0的基线(以米为单位)。为了在参考相机坐标中将3D点X投影到第i个相机图像面上的点Y,相机0的矫正旋转矩阵 R_{rect}^{(0)} $ 也必须考虑进去,所以:
Y=Prect(i)Rrect(0)X Y = P_{rect}^{(i)} R_{rect}^{(0)}X Y=Prect(i)​Rrect(0)​X
在这里,$ R_{rect}^{(0)} $ 被扩展成了4*4的矩阵,令R(4, 4) = 1,其余为0。

最后再考虑点云到相机的外参矩阵,即TvelocamT_{velo}^{cam}Tvelocam​
Y=Prect(i)Rrect(0)TvelocamX Y = P_{rect}^{(i)} R_{rect}^{(0)}T_{velo}^{cam}X Y=Prect(i)​Rrect(0)​Tvelocam​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));

相机的标定

7

相机的标定,即为通过某个已知的目标,求取相机内参矩阵的过程。最常用的标定目标就是棋盘格。用相机对棋盘格从不同角度拍摄多张照片,然后将这些照片导入标定程序或算法,即可自动求出相机的内参。
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);
点云坐标系到相机坐标系的旋转矩阵应该为:
(0−1000−1100)\begin{pmatrix} 0 &amp; -1 &amp;0 \\ 0 &amp; 0 &amp;-1 \\ 1&amp;0 &amp;0 \end{pmatrix}⎝⎛​001​−100​0−10​⎠⎞​

MATLAB 四元数转R旋转矩阵的转换顺序为w,x,y,z;
坐标系2到坐标系1的T转换公式为:T2−&gt;1=T1−&gt;0−1∗T2−&gt;0T_{2-&gt;1} = T_{1-&gt;0}^{-1} * T_{2-&gt;0}T2−>1​=T1−>0−1​∗T2−>0​;

点这里可以跳转到人工智能网站

发表评论