竞赛细则说明

总体要求

 
根据给定的激光雷达点云数据集（包含IMU坐标系下的点云数据、IMU数据、数据格式和参数说明），通过提取激光点云（含虚拟）特征点、线、面加以几何计算方法或者迭代最近点搜索法，实现点云配准，为激光测程法提供观测信息，结合INS导航，通过SLAM数据处理方法，最终获得位姿准确的运动轨迹和点云结构正确的建图结果。
 
数据说明
 
比赛数据采集于武汉市某科技园，通过武汉际上导航科技有限公司手持激光扫描设备GS-100G采集，数据采集时设备运动速度约1m/s，运动轨迹闭环。数据采集前1~2min和后1min左右，设备在同一点静止；中间时刻为手持设备以正常行走速度采集数据。
 
技术方法
 
在本数据集中，由于有IMU进行惯性导航计算，所以不仅仅可以去除激光雷达帧内畸变，而且可以为点云配准提供良好的初值。本数据集使用的IMU器件精度，可以使1秒内的惯性导航误差小于20厘米。这些特性，可以有助于两帧点云同名特征点、线、面的配对以及ICP全局最优解的形成。
不管是使用特征匹配几何计算还是使用迭代最近点搜索，点云匹配后的参数信息，必须和INS导航系统进行组合解算，对于组合方式使用松散组合即可，解算后的信息可以用来修正INS导航误差和IMU器件误差。关于激光点云点点、点线、点面匹配后几何计算方程，以及姿态转角观测方程见附录。
 
开发语言和运行环境
 
语言：C++语言，Matlab语言。
运行环境：Windows、ROS
 
成果提交
 
最终建图点云，点云在闭环轨迹基础上生成。数据格式使用las格式、txt格式、pcd格式、自定义格式都可以，自定义格式需要附格式说明。
最终INS运动轨迹，不需要闭环，数据中含时间、位置、速度、姿态以及它们的精度信息。
算法实现的源代码，以备成果再现验证。含有第三方类库的，必须一并提供。matlab工具盒、PCL库不必提交。
特征提取的中间成果。对于每一帧点云所提取出的特征点、特征线、特征面都可以输出到中间文件，其中点记录坐标，线记录方向矢量，面记录法向量和常数D，文件格式为txt格式。中间成果对于评分有重要影响，请务必注意。
点云匹配的中间成果。对于每两帧点云匹配后的转换参数，可以输出到中间文件，转换参数为平移三个分量和旋转矩阵九元素。中间成果对于评分有重要影响，请务必注意。
成果、源代码、中间成果的说明文档。说明文档对于评分有重要影响，请务必注意。
 
计分细则
 
总分100分。
技术方法的成熟度25分，技术水平从高到低，计为25、20、15、10、5分共五档。
算法的程序编写水平，从算法精良、代码清晰到算法粗燥、代码混乱，计为25、20、15、10、5分共五档。
成果的精度，精度从高到低，计为25、20、15、10、5分共五档。
程序编写语言和环境，C++/Windows 25分，C++/ROS 20分，Matlab/Windows 15分。
 
 
 
附录1：
激光点云匹配方程
 
一、点点匹配
 
对于待匹配帧中某激光点X^1，存在参考帧中同名点X^0，有如下关系：
X^0=T+RX^1 (1)
其中T为两帧点云所在坐标系原点的偏移，R为两坐标系之间的轴系旋转矩阵，含有三个线元素、三个角元素共六个未知参数。
由于方程1为非线性方程，所以在R求解时，需采用迭代求解方法：R=(I-E)R^0，E为R迭代求解时欧拉角残差对应的反对称矩阵，则方程1可以写成：
X^0=T+(I-E)R^0 X^1 (2)
令(X^1 ) ̅=R^0 X^1，(X^1 ) ̃为矢量(X^1 ) ̅对应的反对称矩阵，则：
X^0=(X^1 ) ̅+T+(X^1 ) ̃φ (3)
其中φ为E对应的角度误差矢量。写成观测方程形式，即为：
X^0-(X^1 ) ̅=[■(I&(X^1 ) ̃ )][■(T@φ)] (4)
由于方程4为三维矢量，所以只要在两帧间找到两个以上的同名点，即可求解出T和φ六个位姿参数。
 
二、点线匹配
 
点点匹配虽然效率和精度都很高，但是对于地学应用场景的稀疏激光点云，在两帧间寻找同名激光点却非常困难。固然通过点云族形成特征描述子后仍然可以通过点点匹配来求解位姿参数，但是也可以通过点线匹配来求解，因为特征线的生成较为容易。
对于待匹配帧中某激光点X^1 (x_1,y_1,z_1)，存在参考帧中点上直线l，l有如下描述：
X^l=[■(x^l@y^l@z^l )]=[■(a_0+mt@b_0+nt@c_0+pt)] (5)
其中A=(a_0,b_0,c_0)为直线基准点，M=(m,n,p)为直线l的方向向量。点X^1的同名点X^0 (x_0,y_0,z_0)必须符合方程4，即：
X^l=T+RX^1 (6)
结合方程3、5、6，展开，可得：
[■(a_0+mt@b_0+nt@c_0+pt)]=[■((x^1 ) ̅@(y^1 ) ̅@(z^1 ) ̅ )]+[■(T_x@T_y@T_z )]+[■(0&-(z^1 ) ̅&(y^1 ) ̅@(z^1 ) ̅&0&-(x^1 ) ̅@-(y^1 ) ̅&(x^1 ) ̅&0)][■(φ_x@φ_y@φ_z )] (7)
方程7中2、3式减去1式，消去未知数t，得到：
[■((mb_0-na_0 )-(m(y^1 ) ̅-n(x^1 ) ̅)@(mc_0-pa_0 )-(m(z^1 ) ̅-p(x^1 ) ̅))]=[■((mT_y-nT_x )+(m(z^1 ) ̅φ_x-m(x^1 ) ̅φ_z )-(-n(z^1 ) ̅φ_y+n(y^1 ) ̅φ_z)@(mT_z-pT_x )+(-m(y^1 ) ̅φ_x+m(x^1 ) ̅φ_y )-(-p(z^1 ) ̅φ_y+p(y^1 ) ̅φ_z))]       (8)
写成矢量形式，即为：
[■((mb_0-na_0 )-(m(y^1 ) ̅-n(x^1 ) ̅)@(mc_0-pa_0 )-(m(z^1 ) ̅-p(x^1 ) ̅))]=[■(■(-n&m)&■(0&m(z^1 ) ̅ )&■(n(z^1 ) ̅&-m(x^1 ) ̅-n(y^1 ) ̅ )@■(-p&0)&■(m&-m(y^1 ) ̅ )&■(m(x^1 ) ̅+p(z^1 ) ̅&-p(y^1 ) ̅ ))][■(■(T_x@T_y )@■(T_z@φ_x )@■(φ_y@φ_z ))] (9)
由方程9可见，点线匹配相对于点点匹配，由三维方程降低为二维，降低了观测性。这是因为直线方程多了一个自由度所致。要使方程9有解，必须至少在待匹配帧中选取3个线上激光点，并且这些点不可同线，才能有解。为了能使解的性质良好，这些线必须有良好的分布，并且方向向量朝向四面八方，不要平行。待匹配帧中的线上点，也可以使用虚拟点，不一定要是真是存在的点。同时要注意虚拟观测值方差的传递。
 
三、点面匹配
 
对于待匹配帧中某激光点X^1 (x_1,y_1,z_1)，存在参考帧中点上平面P，P有如下描述：
Ax^P+By^P+Cz^P+D=0 (10)
其中(A,B,C)为平面P的单位法向量n ⃗，即A^2+B^2+C^2=1。点X^1的同名点X^0 (x_0,y_0,z_0)必须符合方程10，即：
X^P=T+RX^1 (11)
展开方程11，可得：
X^P=[■(x^P@y^P@y^P )]=[■((x^1 ) ̅+T_x-(z^1 ) ̅φ_y+(y^1 ) ̅φ_z@(y^1 ) ̅+T_y+(z^1 ) ̅φ_x-(x^1 ) ̅φ_z@(z^1 ) ̅+T_z-(y^1 ) ̅φ_x+(x^1 ) ̅φ_y )] (12)
结合方程10、12，可得：
A((x^1 ) ̅+T_x-(z^1 ) ̅φ_y+(y^1 ) ̅φ_z )+B((y^1 ) ̅+T_y+(z^1 ) ̅φ_x-(x^1 ) ̅φ_z )+C((z^1 ) ̅+T_z-(y^1 ) ̅φ_x+(x^1 ) ̅φ_y )+D=0 (13)
整理，得到：
(A(x^1 ) ̅+B(y^1 ) ̅+C(z^1 ) ̅ )+(AT_x+BT_y+CT_z )+(B(z^1 ) ̅-C(y^1 ) ̅ ) φ_x+(-A(z^1 ) ̅+C(x^1 ) ̅ ) φ_y+(A(y^1 ) ̅-B(x^1 ) ̅ ) φ_z+D=0 (14)
写成矩阵形式，得到：
-(A(x^1 ) ̅+B(y^1 ) ̅+C(z^1 ) ̅+D)=[■(■(A&B)&■(C&B(z^1 ) ̅-C(y^1 ) ̅ )&■(-A(z^1 ) ̅+C(x^1 ) ̅&A(y^1 ) ̅-B(x^1 ) ̅ ))][■(■(T_x@T_y )@■(T_z@φ_x )@■(φ_y@φ_z ))] (15)
由方程15可见，点面匹配一个点只能形成一个观测方程，相对于点点匹配，降低了观测性。这是因为平面方程多了两个自由度所致。要使方程15有解，必须至少在待匹配帧中选取6个面上激光点，并且这些点不可同面，才能有解。为了能使解的性质良好，这些面必须有良好的分布，并且法向量朝向四面八方，不要平行。待匹配帧中的面上点，也可以使用虚拟点，不一定要是真是存在的点。同时要注意虚拟观测值方差的传递。
 

 
附录2：
转角修正方程
 
角度修正或者姿态修正，是组合导航的一个重要内容，包括了矢量、矩阵、反对称矩阵、误差矩阵等知识点。本文档对这些内容加以描述。
 
一、基本知识和概念
 
1，对于矢量a、b，A、B分别为他们的反对称矩阵，则
a×b=Ab=-Ba (1)
2，旋转矩阵的导数为：
R ̇_b^i=R_b^i Ω_ib^b，R ̇_b^e=R_b^e Ω_eb^b，  (2)
其中Ω_ib^b为i系到b系的旋转角速度在b系的表达的反对称矩阵，Ω_eb^b类似，注意旋转顺序和上标。
3，如果存在一个小转角ε=(■(x@y@z))，则对应的反对称矩阵为：
E=[■(0&-z&y@z&0&-x@-y&x&0)] (3)
且此转角形成的旋转矩阵为：
R=(I-E)=[■(1&z&-y@-z&1&x@y&-x&1)] (4)
由于转角存在方向以及坐标系概念，所以对应的旋转矩阵也存在方向和坐标系。如ε记为ε_ab^e，表示ε角度为a系转到b系的旋转欧拉角在e系中的表达，对应的，E可记为E_ab^e。注意只有小角度旋转公式(4)才成立，反对称矩阵的记法也才有意义，否则没有意义。
 
二、e系姿态误差方程
 
根据旋转矩阵导数方程，对公式2两边微分，得：
〖δR ̇〗_b^e=〖δR〗_b^e Ω_eb^b+R_b^e 〖δΩ〗_eb^b (5)
同时根据误差角定义，从b系到e系含有小角误差的旋转矩阵为：
R ̃_b^e=(I-E_be^e ) R_b^e=R_b^e-E_be^e R_b^e (6)
其中R ̃_b^e为含有误差的旋转矩阵，R_b^e为正确的旋转矩阵，E_be^e为误差角对应的反对称矩阵。
因为R ̃_b^e=R_b^e+δR_b^e，所以由公式6可得：
δR_b^e=-E_be^e R_b^e (7)
对公式7求导，可得：
δR ̇_b^e=-E ̇_be^e R_b^e-E_be^e R ̇_b^e=-E ̇_be^e R_b^e-E_be^e R_b^e Ω_eb^b (8)
结合公式7和公式8，可得：
〖δR ̇〗_b^e=〖δR〗_b^e Ω_eb^b-E ̇_be^e R_b^e (9)
比较公式5和公式9，可得：
〖-E ̇〗_be^e R_b^e=R_b^e 〖δΩ〗_eb^b (10)
即：
E ̇_be^e=-R_b^e 〖δΩ〗_eb^b R_e^b (11)
写成矢量形式：
ε ̇_be^e=-R_b^e 〖δω〗_eb^b (12)
其中〖δω〗_eb^b为e系到b系旋转角速度误差，可以写成：
〖δω〗_eb^b=〖δω〗_ei^b+〖δω〗_ib^b==-〖δω〗_ie^b+〖δω〗_ib^b (13)
ω_ie^b=R_e^b ω_ie^e (14)
〖δω〗_ie^b=〖δR〗_e^b ω_ie^e+R_e^b 〖δω〗_ie^e=〖δR〗_e^b ω_ie^e (15)
〖δR〗_e^b=-(E_be^e R_b^e )^T=R_e^b E_be^e (16)
结合公式13、15和16，可得：
〖δω〗_eb^b=-R_e^b E_be^e ω_ie^e+〖δω〗_ib^b (17)
〖δω〗_ib^b为角速度观测误差，有：
〖δω〗_ib^b=ω ̃_ib^b-ω_ib^b (18)
其中ω ̃_ib^b为受到误差影响的角速度观测值，ω_ib^b为理论观测值，ω ̃_ib^b=ω_ib^b+d， d为陀螺漂移，故有：
〖δω〗_ib^b=ω_ib^b+d-ω_ib^b=d (19)
结合公式12、17和19，可得：
ε ̇_be^e=-R_b^e (-R_e^b E_be^e ω_ie^e+d) (20)
整理得到：
ε ̇_be^e=-Ω_ie^e ε_be^e-R_b^e d (21)
其中Ω_ie^e为ω_ie^e对应的反对称矩阵，ω_ie^e为地球自转角速度在e系中的表达。
 
三、e系速度误差方程
 
根据e系速度微分方程：
V ̇^e=R_b^e f^b-2Ω_ie^e V^e+g^e           (22)
两边微分，得：
〖δV ̇〗^e=〖δR〗_b^e f^b+R_b^e 〖δf〗^b-2Ω_ie^e 〖δV〗^e+〖δg〗^e    (23)
由公式7可得：
〖δV ̇〗^e=-E_be^e R_b^e f^b+R_b^e 〖δf〗^b-2Ω_ie^e 〖δV〗^e+〖δg〗^e    (24)
〖δf〗^b为加速度观测误差，有：
〖δf〗^b=f ̃^b-f^b (25)
其中f ̃^b为受到误差影响的加速度观测值，f^b为理论观测值，f ̃^b=f^b+b，b为加速度计零位偏置，故有：
〖δf〗^b=f^b+b-f^b=b (26)
结合公式24和26，忽略〖δg〗^e，可得：
〖δV ̇〗^e=F^e ε_be^e+R_b^e b-2Ω_ie^e 〖δV〗^e    (27)
整理得到：
〖δV ̇〗^e=-2Ω_ie^e 〖δV〗^e+F^e ε_be^e+R_b^e b    (28)
其中F^e为f^e对应的反对称矩阵，f^e为加速度观测量在e系中的表达。
 
四、ZUPT角速度误差方程
 
对于ZUPT，理论上不但位置不变，而且姿态也不变，所以对于一段时间后，因为姿态误差的影响，旋转矩阵发生了变化，含有误差的旋转矩阵R ̃_b^e=(I-E_be^e ) R_b^e，则误差矩阵：
R=I-E_be^e=R ̃_b^e R_e^b (29)
根据公式4，可以得到误差角观测量ε ̃_be^e，也可以根据欧拉角定义直接获得，以及角速度误差：
ε ̃  ̇_be^e=ε ̃_be^e/ΔT (30)
结合公式21和30，可得：
ε ̃_be^e/ΔT=-Ω_ie^e ε_be^e-R_b^e d-R_b^e W_ib^b k_ω (31)
 
五、激光点云松组合SLAM角速度误差方程
 
对于激光点云SLAM，和ZUPT类似，同时有位置变化和姿态变化观测信息，但是这些信息都属于改正数信息，对于姿态角改正数形成的旋转矩阵R_v，存在如下关系：
R_v R ̃_b^e=R_b^e， R_v=R_b^e R ̃_e^b (32)
比较公式29和32可得，误差矩阵R为改正数矩阵的逆阵：
R=R_v^T (33)
然后使用公式30-31，即可形成激光点云SLAM的松组合观测方程。