CN103822644B - 一种三维激光成像系统的相机标定方法 - Google Patents

Abstract

本发明公开了一种三维激光成像系统的相机标定方法，采用特征点标定相机坐标系的方法，通过微分计算得出相机坐标系与载体坐标系的六个偏差参数，完成相机坐标系的标定，从而得到精确的载体坐标系到相机坐标系的转换矩阵。目前还没有相应的方法能处理三维激光成像系统的相机标定方法，该方法思路简单明了，编程容易实现，提高了三维激光成像系统中点云数据与照片融合的精度。

Description

一种三维激光成像系统的相机标定方法

技术领域

本发明涉及一种三维激光成像系统的相机标定方法，尤其涉及一种采用微分方法标定相机坐标系与载体坐标系关系的六个偏差参数的方法，属于图像处理技术领域。

背景技术

三维激光成像系统是指将激光扫描仪、全球定位系统、惯性测量系统等集成为一套设备，装在载体上对地面目标进行扫描，得到地面目标的三维信息，通过处理得到需要的三维空间图像。由于获取的反射回来的激光点数据呈星云状密集分布，所以形象地称为激光点云（Point Cloud），意思为无数的点以测量的规则在计算机里呈现物体的结果。

通过三维激光成像系统获得激光点云图能够呈现被扫描物的三维形状特征，能够在三维点云模型中看出被扫描物体的位置，大小，形状信息。但缺乏被扫描物体的颜色信息，这样就缺少了被扫描物体的纹理，不能很好的表述被扫描物体的细节。因此，有必要找到一种方法能够自动快速的处理照片和点云之间的关系，并将照片中被扫描物体与三维立体模型进行融合，赋予被扫描物体的模型以原本的颜色，得到真彩色的立体模型。在2013年提交的申请号为201310557041.6的专利申请《一种三维激光成像系统的点云模型真彩色处理方法》中对于载体坐标系与相机坐标系进行了理想化处理，认为两者之间的关系能够精确的已知，但在实际情况中，相机安装方式为非固定安装，故载体坐标系和相机坐标系之间的关系不能完全精确通过测量已知，需要进行修正。

理想状态下点云从载体坐标系转换到相机坐标系的转换方式为：

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_b-r_{\mathrm{bx}}\\ y_b-r_{\mathrm{by}}\\ z_b-r_{\mathrm{bz}}\end{array}\right]{\mathrm{\θ}}_{\mathrm{pk}}$

其中， $\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]$ 表示点云在相机坐标系中的坐标， $\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]$ 表示点云在载体坐标系中的坐标， $\left[\begin{array}{c}{r}_{\mathrm{bx}}\\ r_{\mathrm{by}}\\ r_{\mathrm{bz}}\end{array}\right]$ 表示惯组中心到相机焦点在载体坐标系中的矢量。

在实际情况下点云从载体坐标系转换到相机坐标系的转换方式为：

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\Δ\ψ}& \sin \mathrm{\Δ\ψ}& 0\\ -\sin \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\ψ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\Δ\β}& \sin \mathrm{\Δ\β}\\ 0& -\sin \mathrm{\Δ\β}& \cos \mathrm{\Δ\β}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\Δ\γ}& 0& -\sin \mathrm{\Δ\γ}\\ 0& 1& 0\\ \sin \mathrm{\Δ\γ}& 0& \cos \mathrm{\Δ\γ}\end{array}\right]\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_b-r_{\mathrm{bx}}-\mathrm{\δ}{r}_{\mathrm{bx}}\\ y_b-r_{\mathrm{by}}-\mathrm{\δ}{r}_{\mathrm{by}}\\ z_b-r_{\mathrm{bz}}-\mathrm{\δ}{r}_{\mathrm{bz}}\end{array}\right]$

因此，如果能够标定得到六个参数 $\left[\begin{array}{c}{\mathrm{\δr}}_{\mathrm{bx}}\\ {\mathrm{\δr}}_{\mathrm{by}}\\ {\mathrm{\δr}}_{\mathrm{bz}}\\ \mathrm{\Δ\γ}\\ \mathrm{\Δ\β}\\ \mathrm{\Δ\ψ}\end{array}\right],$ 并将其反馈回点云模型真彩色中的载体坐标系转换到相机坐标系公式中，则可以得出点云模型的精确配准。

因此，为了能够得到影响数据与点云数据的精确配准，就需要确定相机坐标系与载体坐标系之间精确的关系，即对相机坐标系进行标定。目前还没有对三维激光成像系统的相机进行标定的方法。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供一种三维激光成像系统的相机标定方法，该方法能够精确的确定相机坐标系与载体坐标系的六个偏差参数，得到点云从载体坐标系到相机坐标系精确的转换关系，从而使点云数据与影像数据的融合更为精确，精度更高。

本发明的技术解决方案是：一种三维激光成像系统的相机标定方法，步骤如下：

（1）获得惯组中心在地球坐标系下的导航时间T、导航极坐标位置信息和导航姿态信息(γ,θ,ψ)、点云极坐标信息和照片，其中分别表示惯组中心在地球坐标系下的经度、纬度和高度，分别表示点云在地球坐标系下的经度、纬度和高度，(γ,θ,ψ)分别表示惯组中心的俯仰角、方位角和横滚角；

（2）根据照片拍摄的时间T_i计算得到该照片拍摄时刻惯组中心在地球坐标系下的极坐标位置信息和姿态信息(γ_i,θ_i,ψ_i)，其中分别表示照片拍摄时刻惯组中心在地球坐标系下的经度、纬度和高度，(γ_i,θ_i,ψ_i)分别表示照片拍摄时刻惯组中心的俯仰角、方位角和横滚角；

（3）根据惯组中心在地球坐标系下的极坐标位置信息得到地球坐标系到东北天坐标系的转换矩阵根据惯组中心在地球坐标系下的姿态信息(γ_i,θ_i,ψ_i)得到东北天坐标系到载体坐标系的转换矩阵

（4）将惯组中心在地球坐标系下的极坐标位置信息转换为惯组中心在地球坐标系下的直角坐标位置(x_i,y_i,z_i)，在点云数据中找出不在同一直线上的三个特征点在地球坐标系下的极坐标信息并将三个特征点从地球坐标系下的极坐标信息转换为三个特征点在地球坐标系下的直角坐标位置(x_ek,y_ek,z_ek)，其中k=1,2,3；

（5）从照片中找出步骤（4）选定的三个特征点的对应像素点在照片坐标系下的坐标(m_k,n_k)，并根据相机CCD的宽A、长B和照片的焦距f，计算得出照片中这三个特征点的对应像素点在相机直角坐标系中的俯仰角方位角θ_pk，其中k=1,2,3；

（6）利用惯组中心在地球坐标系下的直角坐标位置(x_i,y_i,z_i)和点云数据中的三个特征点在地球坐标系下的直角坐标位置(x_ek,y_ek,z_ek)计算地球坐标系下惯组中心到三个特征点的矢量r_ek，利用转换矩阵将矢量r_ek从地球坐标系转换为东北天坐标系下的矢量r_nk，再利用转换矩阵将矢量r_nk从东北天坐标系转换为载体坐标系下的矢量r_bk′，再根据矢量r_bk′和惯组中心到相机焦点在载体坐标系下的矢量r_b计算得到相机焦点到点云在载体坐标系下的矢量r_pk′，再将矢量r_pk′从载体坐标系转换为相机直角坐标系下的矢量r_pk，计算矢量r_pk在相机直角坐标系下的极坐标其中R_pk′表示距离，θ_pk′表示方位角，表示俯仰角，其中k=1,2,3；

（7）计算三个特征点与照片中的这三个特征点的对应像素点的俯仰角偏差方位角的偏差δθ_pk，其中k=1,2,3；

（8）根据俯仰角偏差和方位角偏差δθ_pk计算得到相机坐标系相对于载体坐标系的六个偏差参数，即方位角误差Δγ、俯仰角误差Δβ、横滚角误差Δψ、相机坐标系相对于载体坐标系的位置误差δr=[δr_bx δr_by δr_bz]′，从而完成相机坐标系相对于载体坐标系的六个偏差参数的标定；

将偏差参数代入到点云从载体坐标系转换到相机坐标系的公式 $\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\Δ\ψ}& \sin \mathrm{\Δ\ψ}& 0\\ -\sin \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\ψ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\Δ\β}& \sin \mathrm{\Δ\β}\\ 0& -\sin \mathrm{\Δ\β}& \cos \mathrm{\Δ\β}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\Δ\γ}& 0& -\sin \mathrm{\Δ\γ}\\ 0& 1& 0\\ \sin \mathrm{\Δ\γ}& 0& \cos \mathrm{\Δ\γ}\end{array}\right]\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_b-r_{\mathrm{bx}}-\mathrm{\δ}{r}_{\mathrm{bx}}\\ y_b-r_{\mathrm{by}}-\mathrm{\δ}{r}_{\mathrm{by}}\\ z_b-r_{\mathrm{bz}}-\mathrm{\δ}{r}_{\mathrm{bz}}\end{array}\right]$ 中，从而得到点云在相机坐标系中的实际坐标，其中 $\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]$ 表示点云在相机坐标系中的坐标， $\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]$ 表示点云在载体坐标系中的坐标， $\left[\begin{array}{c}{r}_{\mathrm{bx}}\\ r_{\mathrm{by}}\\ r_{\mathrm{bz}}\end{array}\right]$ 表示惯组中心到相机焦点在载体坐标系中的矢量，其中k=1,2,3。

所述步骤（2）的实现方法为：

根据导航时间信息T，计算照片拍摄时间T_i所在的导航区间s，s=floor(T_i-T(1))/ΔT+1，ΔT为惯组采样周期，根据导航区间s计算出惯组中心在地球坐标系下的极坐标位置信息和姿态信息(γ_i,θ_i,ψ_i)，计算方法为：

λ_i=λ(s)+(λ(s+1)-λ(s))/ΔT*(T_i-T(s))；

h_i=h(s)+(h(s+1)-h(s))/ΔT*(T_i-T(s))；

γ_i=γ(s)+(γ(s+1)-γ(s))/ΔT*(T_i-T(s))；

θ_i=θ(s)+(θ(s+1)-θ(s))/ΔT*(T_i-T(s))；

ψ_i=ψ(s)+(ψ(s+1)-ψ(s))/ΔT*(T_i-T(s))。

所述步骤（3）中根据惯组中心在地球坐标系下的极坐标位置信息得到地球坐标系到东北天坐标系的转换矩阵的方法为：

根据惯组中心在地球坐标系下的姿态信息(γ_i,θ_i,ψ_i)得到东北天坐标系到载体坐标系的转换矩阵的方法为：

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_i& 0& \sin {\mathrm{\γ}}_i\\ 0& 1& 0\\ \sin {\mathrm{\γ}}_i& 0& \cos {\mathrm{\γ}}_i\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\θ}}_i& \sin {\mathrm{\θ}}_i\\ 0& -\sin {\mathrm{\θ}}_i& \cos {\mathrm{\θ}}_i\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_i& -\sin {\mathrm{\ψ}}_i& 0\\ \sin {\mathrm{\ψ}}_i& \cos {\mathrm{\ψ}}_i& 0\\ 0& 0& 1\end{array}\right].$

所述步骤（4）的实现方法为：

其中 $e=\frac{a-b}{a},$ a为地球长半轴，b为地球短半轴，R_e=6378137m，k=1,2,3。

所述步骤（5）中三个特征点的对应像素点在相机直角坐标系中的俯仰角的计算公式为三个特征点的对应像素点在相机直角坐标系中的方位角的计算公式为其中k=1,2,3；

其中d表示每一个感光片的宽，由公式d=A/m计算得到，m为像素纵向个数，A为相机CCD宽；l表示每一个感光片的长，由公式l=B/n计算得到，n为像素横向个数，B为相机CCD长。

所述步骤（6）中计算在地球坐标系下惯组中心到三个特征点的矢量r_ek的方法为：

$r_{\mathrm{ek}}=\left[\begin{array}{c}{x}_{\mathrm{ek}}\\ y_{\mathrm{ek}}\\ z_{\mathrm{ek}}\end{array}\right]-\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right];$

所述步骤（6）中将矢量r_ek从地球坐标系转换为东北天坐标系下的矢量r_nk的方法为：

$r_{\mathrm{nk}}=C_e^n{r}_{\mathrm{ek}};$

所述步骤（6）中将矢量r_nk从东北天坐标系转换为载体坐标系下的矢量r_bk′的方法为：

$r_{\mathrm{bk}}^{\′}=C_n^b{r}_{\mathrm{nk}};$

所述步骤（6）中根据矢量r_bk′和惯组中心到相机焦点在载体坐标系下的矢量r_b计算得到相机焦点到点云在载体坐标系的矢量r_pk′的方法为：

r_pk′=r_bk′-r_b；

其中 $r_b=\left[\begin{array}{c}{r}_{\mathrm{bx}}\\ r_{\mathrm{by}}\\ r_{\mathrm{bz}}\end{array}\right],$ 由相机与惯组安装位置得出，其中r_bx表示r_b在x方向的分量，r_by表示r_b在y方向的分量，r_bz表示r_b在z方向的分量；

$r_{\mathrm{pk}}^{\′}=\left[\begin{array}{c}{x}_{\mathrm{pk}}^{\′}\\ y_{\mathrm{pk}}^{\′}\\ z_{\mathrm{pk}}^{\′}\end{array}\right];$

所述步骤（6）中将矢量r_pk′从载体坐标系转换为相机直角坐标系下的矢量r_pk的方法为：

$r_{\mathrm{pk}}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]r_{\mathrm{pk}}^{\′};$

所述步骤（6）中计算矢量r_pk在相机直角坐标系下的极坐标的方法为：

$R_{\mathrm{pk}}^{\′}=\sqrt{(x_{\mathrm{pk}}^2+y_{\mathrm{pk}}^2+z_{\mathrm{pk}}^2)};$

如果x_pk=0，y_pk>0，则θ_pk′=90°；如果x_pk=0，y_pk<0，则θ_pk′=270°；如果x_pk>0，则 ${\mathrm{\&theta;}}_{\mathrm{pk}}^{\&prime;}=\arctan \left(\frac{y_{\mathrm{pk}}}{x_{\mathrm{pk}}}\right);$ 如果x_pk<0，则

其中 $r_{\mathrm{pk}}=\left[\begin{array}{c}{x}_{\mathrm{pk}}\\ y_{\mathrm{pk}}\\ z_{\mathrm{pk}}\end{array}\right];$

上述公式中，k=1,2,3。

所述步骤（7）的实现方法为：

三个特征点与照片中的这三个特征点的对应像素点的俯仰角偏差的计算公式为三个特征点与照片中的这三个特征点的对应像素点的方位角偏差的计算公式为δθ_pk=θ_pk′-θ_pk，其中k=1,2,3。

所述步骤（8）实现的方法为：

相机坐标系相对于载体坐标系的六个偏差参数，即方位角误差Δγ、俯仰角误差Δβ、横滚角误差Δψ、相机坐标系相对于载体坐标系的位置误差δr=[δr_bx δr_by δr_bz]′的计算公式为：

其中C的计算公式如下：

本发明与现有技术相比的优点如下：

（1）本发明能够得到相机坐标系与载体坐标系之间的精确关系，对三维激光系统扫描产生的点云赋予颜色信息，并且这个颜色为被扫描物体自身的真彩色，能使得点云数据和影像数据融合更精确。

(2)这种方法可以半全自动运行，不需要加入过多的人工参与，能够标定出相机坐标系与载体坐标系之间的六个偏差参数，理论清晰简单，编程容易。

（3）目前尚没有能够对三维激光成像系统的相机标定的方法，本方法是一种创新，为得到更为精细更为清晰的数字城市模型提供了一种处理办法。

附图说明

图1为本发明实现流程图；

图2为点从相机直角坐标系向照片坐标系变换，且同时说明照片的中心成像原理；

图3为相机扫描仪及惯组安装方式；

图4为以惯组中心为原点的坐标系与以相机焦点为原点的坐标系的关系；

图5为示例中使用的照片；

图6为未标定前示例中照片的点云真彩色处理效果图；

图7为标定后示例中照片的点云真彩色处理效果图。

具体实施方式

本发明的实现原理是：采用特征点标定相机坐标系的方法，通过微分计算得出相机坐标系与载体坐标系的六个偏差参数，完成相机坐标系的标定，得到精确的载体坐标系到相机坐标系的转换矩阵。本发明能够得到相机坐标系与载体坐标系之间的精确关系，对三维激光系统扫描产生的点云赋予颜色信息，并且这个颜色为被扫描物体自身的真彩色，能使得点云数据和影像数据融合更精确。除此之外，本发明具有思路简单明了，编程容易实现，可以半全自动运行，不需要加入过多的人工参与。

如图1所示，三维激光成像系统的相机标定方法步骤如下：

（1）获得惯组中心在地球坐标系下的导航时间T、导航极坐标位置信息和导航姿态信息(γ,θ,ψ)、点云极坐标信息和照片，其中分别表示惯组中心在地球坐标系下的经度、纬度和高度，分别表示点云在地球坐标系下的经度、纬度和高度，(γ,θ,ψ)分别表示惯组中心的俯仰角、方位角和横滚角；

（2）根据照片拍摄的时间T_i计算得到该照片拍摄时刻惯组中心在地球坐标系下的极坐标位置信息和姿态信息(γ_i,θ_i,ψ_i)，其中分别表示照片拍摄时刻惯组中心在地球坐标系下的经度、纬度和高度，(γ_i,θ_i,ψ_i)分别表示照片拍摄时刻惯组中心的俯仰角、方位角和横滚角，i为自然数；

根据导航时间信息T，计算照片拍摄时间T_i所在的导航区间s，s=floor(T_i-T(1))/ΔT+1，ΔT为惯组采样周期，根据导航区间s计算出惯组中心在地球坐标系下的极坐标位置信息和姿态信息(γ_i,θ_i,ψ_i)，计算方法为：

λ_i=λ(s)+(λ(s+1)-λ(s))/ΔT*(T_i-T(s))；

h_i=h(s)+(h(s+1)-h(s))/ΔT*(T_i-T(s))；

γ_i=γ(s)+(γ(s+1)-γ(s))/ΔT*(T_i-T(s))；

θ_i=θ(s)+(θ(s+1)-θ(s))/ΔT*(T_i-T(s))；

ψ_i=ψ(s)+(ψ(s+1)-ψ(s))/ΔT*(T_i-T(s))。

（3）根据惯组中心在地球坐标系下的极坐标位置信息得到地球坐标系到东北天坐标系的转换矩阵根据惯组中心在地球坐标系下的姿态信息(γ_i,θ_i,ψ_i)得到东北天坐标系到载体坐标系的转换矩阵

根据惯组中心在地球坐标系下的极坐标位置信息得到地球坐标系到东北天坐标系的转换矩阵的方法为：

根据惯组中心在地球坐标系下的姿态信息(γ_i,θ_i,ψ_i)得到东北天坐标系到载体坐标系的转换矩阵的方法为：

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\&gamma;}}_i& 0& \sin {\mathrm{\&gamma;}}_i\\ 0& 1& 0\\ \sin {\mathrm{\&gamma;}}_i& 0& \cos {\mathrm{\&gamma;}}_i\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\&theta;}}_i& \sin {\mathrm{\&theta;}}_i\\ 0& -\sin {\mathrm{\&theta;}}_i& \cos {\mathrm{\&theta;}}_i\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\&psi;}}_i& -\sin {\mathrm{\&psi;}}_i& 0\\ \sin {\mathrm{\&psi;}}_i& \cos {\mathrm{\&psi;}}_i& 0\\ 0& 0& 1\end{array}\right].$

（4）将惯组中心在地球坐标系下的极坐标位置信息转换为惯组中心在地球坐标系下的直角坐标位置(x_i,y_i,z_i)，在点云数据中找出不在同一直线上的三个特征点在地球坐标系下的极坐标信息并将三个特征点从地球坐标系下的极坐标信息转换为三个特征点在地球坐标系下的直角坐标位置(x_ek,y_ek,z_ek)，其中k=1,2,3；

其中 $e=\frac{a-b}{a},$ a为地球长半轴，b为地球短半轴，R_e=6378137m，k=1,2,3。

（5）从照片中找出步骤（4）选定的三个特征点的对应像素点在照片坐标系下的坐标(m_k,n_k)，并根据相机CCD的宽A、长B和照片的焦距f，计算得出照片中这三个特征点的对应像素点在相机直角坐标系中的俯仰角方位角θ_pk，其中k=1,2,3；

三个特征点的对应像素点在相机直角坐标系中的俯仰角的计算公式为三个特征点的对应像素点在相机直角坐标系中的方位角的计算公式为其中k=1,2,3；

其中d表示每一个感光片的宽，由公式d=A/m计算得到，m为像素纵向个数；l表示每一个感光片的长，由公式l=B/n计算得到，n为像素横向个数。

（6）利用惯组中心在地球坐标系下的直角坐标位置(x_i,y_i,z_i)和点云数据中的三个特征点在地球坐标系下的直角坐标位置(x_ek,y_ek,z_ek)计算地球坐标系下惯组中心到三个特征点的矢量r_ek，利用转换矩阵将矢量r_ek从地球坐标系转换为东北天坐标系下的矢量r_nk，再利用转换矩阵将矢量r_nk从东北天坐标系转换为载体坐标系下的矢量r_bk′，再根据矢量r_bk′和惯组中心到相机焦点在载体坐标系下的矢量r_b计算得到相机焦点到点云在载体坐标系下的矢量r_pk′，再将矢量r_pk′从载体坐标系转换为相机直角坐标系下的矢量r_pk，计算矢量r_pk在相机直角坐标系下的极坐标其中R_pk′表示距离，θ_pk′表示方位角，表示俯仰角，其中k=1,2,3；

计算在地球坐标系下惯组中心到三个特征点的矢量r_ek的方法为：

$r_{\mathrm{ek}}=\left[\begin{array}{c}{x}_{\mathrm{ek}}\\ y_{\mathrm{ek}}\\ z_{\mathrm{ek}}\end{array}\right]-\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right];$

将矢量r_ek从地球坐标系转换为东北天坐标系下的矢量r_nk的方法为：

$r_{\mathrm{nk}}=C_e^n{r}_{\mathrm{ek}};$

将矢量r_nk从东北天坐标系转换为载体坐标系下的矢量r_bk′的方法为：

$r_{\mathrm{bk}}^{\&prime;}=C_n^b{r}_{\mathrm{nk}};$

根据矢量r_bk′和惯组中心到相机焦点在载体坐标系下的矢量r_b计算得到相机焦点到点云在载体坐标系的矢量r_pk′的方法为：

r_pk′=r_bk′-r_b；

其中 $r_b=\left[\begin{array}{c}{r}_{\mathrm{bx}}\\ r_{\mathrm{by}}\\ r_{\mathrm{bz}}\end{array}\right],$ 由相机与惯组安装位置得出，其中r_bx表示r_b在x方向的分量，r_by表示r_b在y方向的分量，r_bz表示r_b在z方向的分量；

$r_{\mathrm{pk}}^{\&prime;}=\left[\begin{array}{c}{x}_{\mathrm{pk}}^{\&prime;}\\ y_{\mathrm{pk}}^{\&prime;}\\ z_{\mathrm{pk}}^{\&prime;}\end{array}\right](k=\mathrm{1,2,3})$

将矢量r_pk′从载体坐标系转换为相机直角坐标系下的矢量r_pk的方法为：

$r_{\mathrm{pk}}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]r_{\mathrm{pk}}^{\&prime;};$

计算矢量r_pk在相机直角坐标系下的极坐标的方法为：

$R_{\mathrm{pk}}^{\&prime;}=\sqrt{(x_{\mathrm{pk}}^2+y_{\mathrm{pk}}^2+z_{\mathrm{pk}}^2)};$

如果x_pk=0，y_pk>0，则θ_pk′=90°；如果x_pk=0，y_pk<0，则θ_pk′=270°；如果x_pk>0，则 ${\mathrm{\&theta;}}_{\mathrm{pk}}^{\&prime;}=\arctan \left(\frac{y_{\mathrm{pk}}}{x_{\mathrm{pk}}}\right);$ 如果x_pk<0，则

其中 $r_{\mathrm{pk}}=\left[\begin{array}{c}{x}_{\mathrm{pk}}\\ y_{\mathrm{pk}}\\ z_{\mathrm{pk}}\end{array}\right];$

上述各公式中，k=1,2,3。

（7）计算三个特征点与照片中的这三个特征点的对应像素点的俯仰角偏差方位角的偏差δθ_pk，其中k=1,2,3；

三个特征点与照片中的这三个特征点的对应像素点的俯仰角偏差的计算公式为三个特征点与照片中的这三个特征点的对应像素点的方位角偏差的计算公式为δθ_pk=θ_pk′-θ_pk，其中k=1,2,3。

（8）根据俯仰角偏差和方位角偏差δθ_pk计算得到相机坐标系相对于载体坐标系的六个偏差参数，即方位角误差Δγ、俯仰角误差Δβ、横滚角误差Δψ、相机坐标系相对于载体坐标系的位置误差δr=[δr_bx δr_by δr_bz]′，从而完成相机坐标系相对于载体坐标系的六个偏差参数的标定；

将偏差参数代入到点云从载体坐标系转换到相机坐标系的公式 $\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\&Delta;\&psi;}& \sin \mathrm{\&Delta;\&psi;}& 0\\ -\sin \mathrm{\&Delta;\&psi;}& \cos \mathrm{\&Delta;\&psi;}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&Delta;\&beta;}& \sin \mathrm{\&Delta;\&beta;}\\ 0& -\sin \mathrm{\&Delta;\&beta;}& \cos \mathrm{\&Delta;\&beta;}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\&Delta;\&gamma;}& 0& -\sin \mathrm{\&Delta;\&gamma;}\\ 0& 1& 0\\ \sin \mathrm{\&Delta;\&gamma;}& 0& \cos \mathrm{\&Delta;\&gamma;}\end{array}\right]\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_b-r_{\mathrm{bx}}-\mathrm{\&delta;}{r}_{\mathrm{bx}}\\ y_b-r_{\mathrm{by}}-\mathrm{\&delta;}{r}_{\mathrm{by}}\\ z_b-r_{\mathrm{bz}}-\mathrm{\&delta;}{r}_{\mathrm{bz}}\end{array}\right]$ 中，从而得到点云在相机坐标系中的实际坐标，其中 $\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]$ 表示点云在相机坐标系中的坐标， $\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]$ 表示点云在载体坐标系中的坐标， $\left[\begin{array}{c}{r}_{\mathrm{bx}}\\ r_{\mathrm{by}}\\ r_{\mathrm{bz}}\end{array}\right]$ 表示惯组中心到相机焦点在载体坐标系中的矢量，其中k=1,2,3。

相机坐标系相对于载体坐标系的六个偏差参数，即方位角误差Δγ、俯仰角误差Δβ、横滚角误差Δψ、相机坐标系相对于载体坐标系的位置误差δr=[δr_bx δr_by δr_bz]′的计算公式为：

其中C的计算公式如下：

通过上述方法即可完成三维激光成像系统的相机标定方法。图2为点从相机直角坐标系向照片坐标系变换，且同时说明照片的中心成像原理，在图2中O-XYZ表示相机直角坐标系，x-y表示CCD坐标系，x′-y′表示照片坐标系，点p为点云中的点，点p′为照片上的一点，且表示在相机成像中点p在照片中的投影；图3为相机扫描仪及惯组安装方式，给出了惯组中心与相机焦点的关系；图4给出了为以惯组中心为原点的载体坐标系与以相机焦点为原点的相机直角坐标系的转换关系，这种转换关系是非精确已知的，实际中存在着偏差。通过实际应用可以发现，经过相机标定后点云模型与照片的配准更加精准。图5给出了示例中使用的照片，图6给出了未标定前示例中照片的点云模型真彩色效果图，图7给出了标定后示例中照片的点云模型真彩色效果图，将图6和图7进行对比可以发现，标定后点云模型与照片的配准更加精准。

本发明未详细描述内容为本领域技术人员公知技术。

Claims (7)

1.一种三维激光成像系统的相机标定方法，其特征在于步骤如下：

(1)获得惯组中心在地球坐标系下的导航时间T、导航极坐标位置信息和导航姿态信息(γ,θ,ψ)、点云极坐标信息和照片，其中分别表示惯组中心在地球坐标系下的经度、纬度和高度，分别表示点云在地球坐标系下的经度、纬度和高度，(γ,θ,ψ)分别表示惯组中心的俯仰角、方位角和横滚角；

(2)根据照片拍摄的时间T_i计算得到该照片拍摄时刻惯组中心在地球坐标系下的极坐标位置信息和姿态信息(γ_i,θ_i,ψ_i)，其中分别表示照片拍摄时刻惯组中心在地球坐标系下的经度、纬度和高度，(γ_i,θ_i,ψ_i)分别表示照片拍摄时刻惯组中心的俯仰角、方位角和横滚角；

(3)根据惯组中心在地球坐标系下的极坐标位置信息得到地球坐标系到东北天坐标系的转换矩阵根据惯组中心在地球坐标系下的姿态信息(γ_i,θ_i,ψ_i)得到东北天坐标系到载体坐标系的转换矩阵

(4)将惯组中心在地球坐标系下的极坐标位置信息转换为惯组中心在地球坐标系下的直角坐标位置(x_i,y_i,z_i)，在点云数据中找出不在同一直线上的三个特征点在地球坐标系下的极坐标信息并将三个特征点从地球坐标系下的极坐标信息转换为三个特征点在地球坐标系下的直角坐标位置(x_ek,y_ek,z_ek)，其中k＝1,2,3；

(5)从照片中找出步骤(4)选定的三个特征点的对应像素点在照片坐标系下的坐标(m_k,n_k)，并根据相机CCD的宽A、长B和照片的焦距f，计算得出照片中这三个特征点的对应像素点在相机直角坐标系中的俯仰角方位角θ_pk，其中k＝1,2,3；

(6)利用惯组中心在地球坐标系下的直角坐标位置(x_i,y_i,z_i)和点云数据中的三个特征点在地球坐标系下的直角坐标位置(x_ek,y_ek,z_ek)计算地球坐标系下惯组中心到三个特征点的矢量r_ek，利用转换矩阵将矢量r_ek从地球坐标系转换为东北天坐标系下的矢量r_nk，再利用转换矩阵将矢量r_nk从东北天坐标系转换为载体坐标系下的矢量r′_bk，再根据矢量r′_bk和惯组中心到相机焦点在载体坐标系下的矢量r_b计算得到相机焦点到点云在载体坐标系下的矢量r′_pk，再将矢量r′_pk从载体坐标系转换为相机直角坐标系下的矢量r_pk，计算矢量r_pk在相机直角坐标系下的极坐标其中R′_pk表示距离，θ′_pk表示方位角，表示俯仰角，其中k＝1,2,3；

(7)计算三个特征点与照片中的这三个特征点的对应像素点的俯仰角偏差方位角的偏差δθ_pk，其中k＝1,2,3；

(8)根据俯仰角偏差和方位角偏差δθ_pk计算得到相机坐标系相对于载体坐标系的六个偏差参数，即方位角误差Δγ、俯仰角误差Δβ、横滚角误差Δψ、相机坐标系相对于载体坐标系的位置误差δr＝[δr_bx δr_by δr_bz]′，从而完成相机坐标系相对于载体坐标系的六个偏差参数的标定；

将偏差参数代入到点云从载体坐标系转换到相机坐标系的公式

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\&Delta;}\mathrm{\&psi;}& \sin \mathrm{\&Delta;}\mathrm{\&psi;}& 0\\ -\sin \mathrm{\&Delta;}\mathrm{\&psi;}& \cos \mathrm{\&Delta;}\mathrm{\&psi;}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&Delta;}\mathrm{\&beta;}& \sin \mathrm{\&Delta;}\mathrm{\&beta;}\\ 0& -\sin \mathrm{\&Delta;}\mathrm{\&beta;}& \cos \mathrm{\&Delta;}\mathrm{\&beta;}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\&Delta;}\mathrm{\&gamma;}& 0& -\sin \mathrm{\&Delta;}\mathrm{\&gamma;}\\ 0& 1& 0\\ \sin \mathrm{\&Delta;}\mathrm{\&gamma;}& 0& \cos \mathrm{\&Delta;}\mathrm{\&gamma;}\end{array}\right]\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_b-r_{bx}-{\mathrm{\&delta;r}}_{bx}\\ y_b-r_{by}-{\mathrm{\&delta;r}}_{by}\\ z_b-r_{bz}-{\mathrm{\&delta;r}}_{bz}\end{array}\right]$ 中，从而得到点云在相机坐标系中的实际坐标，其中表示点云在相机坐标系中的坐标，表示点云在载体坐标系中的坐标，表示惯组中心到相机焦点在载体坐标系中的矢量，其中k＝1,2,3。

2.根据权利要求1所述的一种三维激光成像系统的相机标定方法，其特征在于：所述步骤(3)中根据惯组中心在地球坐标系下的极坐标位置信息得到地球坐标系到东北天坐标系的转换矩阵的方法为：

根据惯组中心在地球坐标系下的姿态信息(γ_i,θ_i,ψ_i)得到东北天坐标系到载体坐标系的转换矩阵的方法为：

3.根据权利要求1所述的一种三维激光成像系统的相机标定方法，其特征在于：所述步骤(4)的实现方法为：

其中a为地球长半轴，b为地球短半轴，R_e＝6378137m，k＝1,2,3。

4.根据权利要求1所述的一种三维激光成像系统的相机标定方法，其特征在于：所述步骤(5)中三个特征点的对应像素点在相机直角坐标系中的俯仰角的计算公式为三个特征点的对应像素点在相机直角坐标系中的方位角的计算公式为其中k＝1,2,3；

其中d表示每一个感光片的宽，由公式d＝A/m计算得到，m为像素纵向个数，A为相机CCD宽；l表示每一个感光片的长，由公式l＝B/n计算得到，n为像素横向个数，B为相机CCD长。

5.根据权利要求1所述的一种三维激光成像系统的相机标定方法，其特征在于：所述步骤(6)中计算在地球坐标系下惯组中心到三个特征点的矢量r_ek的方法为：

$r_{ek}=\left[\begin{array}{c}{x}_{ek}\\ y_{ek}\\ z_{ek}\end{array}\right]-\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right];$

所述步骤(6)中将矢量r_ek从地球坐标系转换为东北天坐标系下的矢量r_nk的方法为：

$r_{nk}=C_e^n{r}_{ek};$

所述步骤(6)中将矢量r_nk从东北天坐标系转换为载体坐标系下的矢量r′_bk的方法为：

$r_{bk}^{\&prime;}=C_n^b{r}_{nk};$

所述步骤(6)中根据矢量r′_bk和惯组中心到相机焦点在载体坐标系下的矢量r_b计算得到相机焦点到点云在载体坐标系的矢量r′_pk的方法为：

r′_pk＝r′_bk-r_b；

其中由相机与惯组安装位置得出，其中r_bx表示r_b在x方向的分量，r_by表示r_b在y方向的分量，r_bz表示r_b在z方向的分量；

$r_{pk}^{\&prime;}=\left[\begin{array}{c}{x}_{pk}^{\&prime;}\\ y_{pk}^{\&prime;}\\ z_{pk}^{\&prime;}\end{array}\right];$

所述步骤(6)中将矢量r′_pk从载体坐标系转换为相机直角坐标系下的矢量r_pk的方法为：

$r_{pk}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]r_{pk}^{\&prime;};$

所述步骤(6)中计算矢量r_pk在相机直角坐标系下的极坐标的方法为：

$R_{pk}^{\&prime;}=\sqrt{(x_{pk}^2+y_{pk}^2+z_{pk}^2)};$

如果x_pk＝0，y_pk>0，则θ′_pk＝90°；如果x_pk＝0，y_pk<0，则θ′_pk＝270°；如果x_pk>0，则如果x_pk<0，则

其中

上述公式中，k＝1,2,3。

6.根据权利要求5所述的一种三维激光成像系统的相机标定方法，其特征在于：所述步骤(7)的实现方法为：

三个特征点与照片中的这三个特征点的对应像素点的俯仰角偏差的计算公式为三个特征点与照片中的这三个特征点的对应像素点的方位角偏差的计算公式为δθ_pk＝θ′_pk-θ_pk，其中k＝1,2,3。

7.根据权利要求5或6所述的一种三维激光成像系统的相机标定方法，其特征在于：所述步骤(8)实现的方法为：

相机坐标系相对于载体坐标系的六个偏差参数，即方位角误差Δγ、俯仰角误差Δβ、横滚角误差Δψ、相机坐标系相对于载体坐标系的位置误差

δr＝[δr_bx δr_by δr_bz]′的计算公式为：

其中C的计算公式如下：