一、LiDAR和IMU位姿标定
- 1.1安装角度标定
- 1.2安装位置标定
- 1.3部分代码
二、点云转换到当地水平坐标系
- 2.1基本理论
- - 2.1.1坐标系
  - - 2.1.1.1激光雷达
    - 2.1.1.2导航坐标系/当地水平坐标系/大地坐标系
  - 2.1.2惯性传感器原理
- 2.2点云转换试验
- - 2.2.1试验场地
  - 2.2.2试验内容
  - 2.2.3点云变换效果

Windows 10(64bits) + VMware 16 Pro + Ubuntu 20.04 + noetic
速腾聚创激光雷达：RS-LIDAR-32
华测导航接收机：CGI-610

一、LiDAR和IMU位姿标定

为了获取更多的车前信息，将激光雷达向下倾斜一定角度安装。这样就会导致点云的Z坐标不对，前方离平台越远的点的Z轴坐标越大，甚至是个正值，这很不利于进行直通滤波等点云预处理。

1.1安装角度标定

在设计雷达安装结构的时候，设计了两个定位销，而且雷达是360°水平视角，暂不考虑点云yaw的安装误差，仅对pitch和roll进行标定。

下面介绍一种简单的半自动标定方法：

紧固连接激光雷达及其连接结构于平台小车
小车静置于走廊，且雷达前方朝着走廊方向
选择无动态物体和人的时刻，采集几秒钟激光点云
将bag格式的点云数据转换为pcd文件
选择50帧pcd文件并用cloudcompare合并为一个整体文件，帧数越多越精确
查看地面点云集中区域的点云坐标，以及两侧墙壁的点云坐标，使用pcl直通滤波提取出地面点
RANSAC拟合出平面表达式，阈值越小越精确
根据拟合所得表达式可以得到平面法向量，求法向量和理想水平地面法向量（0,0,1）的旋转矩阵
使用这个旋转矩阵进行点云变换，查看标定效果

做以下几点解释：

位置变了当然还得重新标定，不如一次性固定好
合并50帧点云后，可以看出来激光雷达的测距误差，即形成一条条的点云带
好多算法都是利用迭代优化的方法，我认为选择50帧点云也是一种形式的优化
设置滤波范围时尽量滤除没有意义的点，减小误差
得到的是点云变换矩阵，而非坐标系变换矩阵

以下图片是滤波提取的走廊地面点云，白色是原始点云，青色是经旋转后的点云，第二张图是主视图视角，旋转的还可以。

1.2安装位置标定

标定完了相对姿态（旋转矩阵），还有激光雷达和组合导航接收机IMU之间的相对位置（平移向量）。
由1.1拟合出的平面方程，可以求出激光雷达中心到地面的垂直距离z，再量取组合导航接收机的安装高度，结合IMU在设备中的位置图纸，即可求出补偿量。
x和y的标定方法：

将小车侧边与墙壁平行，同样用RANSAC拟合平面方程，求得激光雷达Y坐标的补偿量
将小车后部贴着墙壁，且下倾的激光雷达可以扫到前方墙壁，用上述方法求X坐标的补偿量

最终将得到的xyz坐标放到变换矩阵的最后一列，与旋转矩阵组成一个4*4的变换矩阵。

1.3部分代码

直通滤波

pcl::PassThrough<pcl::PointXYZI> pass;
pass.setInputCloud (cloud_nonan);
pass.setFilterFieldName ("x");
pass.setFilterLimits (0.0, 8.0);
pass.setFilterLimitsNegative (false);
pass.filter (*cloud_passthrough);
pcl::io::savePCDFileASCII("../pass_x.pcd",*cloud_passthrough);

RANSAC分割

pcl::SACSegmentation<pcl::PointXYZI> plane_seg;
pcl::PointIndices::Ptr plane_inliers ( new pcl::PointIndices );
pcl::ModelCoefficients::Ptr plane_coefficients ( new pcl::ModelCoefficients );
plane_seg.setOptimizeCoefficients (true);
plane_seg.setModelType ( pcl::SACMODEL_PLANE );
plane_seg.setMethodType ( pcl::SAC_RANSAC );
plane_seg.setDistanceThreshold ( 0.005 );
plane_seg.setInputCloud ( cloud_passthrough );
plane_seg.segment (*plane_inliers, *plane_coefficients);

索引滤波

pcl::ExtractIndices<pcl::PointXYZI> extract;
extract.setInputCloud (cloud_passthrough);
extract.setIndices (plane_inliers);
extract.setNegative (false);
extract.filter (*cloud_ransac);
std::cerr << "PointCloud representing the planar component: "
<< cloud_ransac->points.size() << " data points." << std::endl;

二、点云转换到当地水平坐标系

2.1基本理论

2.1.1坐标系

2.1.1.1激光雷达

激光雷达极坐标（有论文叫做瞬时激光束坐标系）和笛卡尔坐标映射，根据激光束的垂直角度、水平旋转角度和实测距离，将激光雷达极坐标投影到XYZ坐标系上。下图是速腾官方说明书中的说明：

注意：

激光雷达的坐标虽然是X右，Y前，Z上符合右手坐标系，
但是在ROS包里面，X 轴定义指向图 13 中的 Y 正方向， ROS 下面的 Y轴定义指向图 13 中的 X 负方向，同样符合右手坐标系，只是旋转了90度，即一个三维旋转矩阵。

2.1.1.2导航坐标系/当地水平坐标系/大地坐标系

常用的有北东地和东北天两种。相对应的载体坐标系有前右下和右前上两种。
本人感觉东北天坐标系舒服一点，同时选用右前上载体坐标系，按道理组合导航接收机要放在载体小车的中心，但是无法做到，就会产生“杆臂效应”，也就是配置组合导航接收机的那些值，这些数值的初始化和校准由接收机内算法实现，个人认为组合导航接收机的精度和价格主要就是差在这里的融合补偿算法。
东北天坐标系———— 右前上坐标系
X轴：指东 ———— X轴：指向载体右侧
Y轴：指北 ———— Y轴：指向载体前进方向
Z轴：指天 ———— Z轴：指向上
都符合右手坐标系，拿出右手按照下图比划一下，没毛病。
坐标系xyz和手指对应关系记忆：XYZ——大拇指、食指、中指依次

2.1.2惯性传感器原理

IMU通过感知地球重力加速度和自转角速度，输出自身相对于大地坐标系的三轴角速度和角加速度，以及姿态更新推算出的自身姿态，通常用四元数表达。
具体地，可以看以下链接学习一下：
参考链接: 惯性导航学习笔记——惯性技术基础知识

姿态表示方法有欧拉角、四元数和旋转矩阵。
欧拉角：yaw、pitch和roll，存在万向节死锁，便于直观显示，不利于计算
四元数：超复数，很不直观，计算量和旋转矩阵一样，但是存储时只需要存四个数
有好多介绍这几种姿态表达方式的博客，这里只附上用Eigen实现的四元数和欧拉角的转换代码：
欧拉角转四元数

Eigen::Quaterniond q=
Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())*
Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitY())*
Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitX());

四元数转欧拉角

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

2.2点云转换试验

2.2.1试验场地

室外，较为平坦的水平地面，采集数据集1
室外，GNSS信号；小车前轮在平地，后轮放在台阶上，采集数据集2

2.2.2试验内容

验证标定激光雷达安装姿态的变换矩阵
验证组合导航接收机输出的姿态角，使点云转换到当地水平坐标系

2.2.3点云变换效果

数据集1：
白色是原始点云，青色是经旋转后的点云

数据集2：
白色是原始点云，青色是经激光雷达标定矩阵旋转后的点云

青色是经激光雷达标定矩阵旋转后的点云，橙色是IMU位姿变换后的点云

LiDAR点云转换到大地坐标系——简单粗标定相关推荐

自动驾驶 | MINet：嵌入式平台上的实时Lidar点云数据分割算法，速度可达 20-80 FPS！...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达这篇文章是激光雷达点云数据分割算法的嵌入式平台上的部署实现.主要的创新点有两点:一是利用多路分支采用不 ...
点云平面提取_基于LiDAR点云数据滤波方法
基于LiDAR点云数据滤波方法机载激光雷达所获取的数据被称为"点云(points cloud)"它在三维空间中呈现出随机分布的形状.在点云中,有些点属于真实的地形表面的点,有些点 ...
LiDAR点云处理软件
转自https://www.cnblogs.com/l-kong/ 1.主要介绍当前主流的激光雷达数据处理库.开源软件.商业软件等. 英文主页: http://opentopo.sdsc.edu/to ...
超擎地图云平台！最简单易用的坐标转换利器
超擎地图云平台!最简单易用的坐标转换利器说起GIS的坐标系统,它就像谜一般的存在,只要是涉及到坐标系统的相关问题,不管是测绘地信还是国土规划,大家都好像是似懂非懂的状态.但坐标系却是解决各类空间数据 ...
【CVPR 2021】Cylinder3D：用于LiDAR点云分割的圆柱体非对称3D卷积网络
文章目录 Cylindrical and Asymmetrical 3D Convolution Networks for LiDAR Segmentation 做了什么 Cylinder3D 整体框 ...
PCA/PCC软件中一键式超高密度的无人机LiDAR点云滤波和精细地形提取
激光雷达是一种方兴未艾的测量技术.基于搭载平台类型,可以细分为星载.机载.地面.车载.背包.船载LiDAR等.其中,机载LiDAR测量技术较为常见,主要用于获取被测量区域的高精度.高分辨率的数字高程模 ...
【点云处理】Lidar点云障碍物形状估计(1)
自动驾驶场景中对Lidar点云障碍物的形状估计就是要给点云簇找到一个合适的3D矩形框.简单起见,我们只考虑偏航角(Yaw),忽略俯仰和翻滚角,这也符合自动驾驶场景中对一般障碍物的设定.有了这个设定之后 ...
标注成本降低5倍！LaserMix：通用半监督LiDAR点云分割框架(新加坡南洋理工大学)...
点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取后台回复[LMix]获取论文! 后台回复[ECCV2022]获取ECCV2022所有自动驾驶方向论文! 后台回 ...
Cylinder3D ：3D环境下的Lidar 点云分割
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达文章导读导读:在自动驾驶中,3D感知相较于2D感知能够提供更加准确的位置信息.随着3D激光雷达传感器 ...

LiDAR点云转换到大地坐标系——简单粗标定

目录