点云 Point cloud
本质是对三维世界几何形状的低分辨率重采样，因此只能提供片面的几何信息
pcd文件
pcd点云格式是pcl库种常常使用的点云文件格式。
一个pcd文件中通常由两部分组成：分别是文件说明和点云数据
包括点云的数目，点云的类型（是pointxyz还是pointxyzrgb），还有点云的宽度和高度
pcd数据
读取的数据
Open3d读取pcd格式点云文件的函数为o3d.io.read_point_cloud，
它读取一个点云文件，并返回PointCloud类的实例。 open3d.geometry.PointCloud
。支持的扩展名是：pcd，ply，xyz，xyzrgb，xyzn，pts
数据转换
open3d.utility.Vector3dVector
将形状（n,d3）的float64 numpy数组转换为Open3D格式。
std::vector<Eigen::Vector3d> 数值计算库 Eigen：Vector3d
用NumPy包裹Eigen类型
np.ones(shape, dtype=None, order='C')
齐次矩阵（homogeneous matrix）
写入数据
o3d.io.write_point_cloud(path, pcd , write_ascii=True)
坐标系
IMU的全称是inertial measurement unit，即惯性测量单元
(IMU)作为一个嵌入了三轴线性加速度计和三轴角速度陀螺仪的模块，可测量六个自由度(“6 DOF或六轴”)。
通过组成六轴结构的线性运动 (三维空间)和旋转测量组件(滚动，俯仰和偏航)，IMU能够捕获车辆运动状态的全部分量。
IMU不仅仅可用于安全气囊和车辆稳定性控制，并且可以实时跟踪计算车辆的位置和方向
辅助卡尔曼滤波器定位估计
坐标原点： 自车的坐标原点(后轴中心) 传感器的坐标原点+平移矩阵
激光雷达到IMU，IMU到自车
激光雷达到自车
点云数据配准
如何让得到坐标变换的参数R（旋转矩阵）和T（平移向量），
使得两视角下测得的三维数据经坐标变换后的距离最小，，目前配准算法按照过程可以分为整体配准和局部配准
Voxel downsampling 体素下采样
体素下采样使用规格体素网格进行标准下采样。
通常作为点云任务的预处理。算法有两步：
1.将点放入体素
2.每个被占用的体素通过平均内部的所有点来生成一个点
Vertex normal estimation 顶点法线估计
点的法线估计。按N查看法线。-+可以控制法线显示的长度。
定位
高精定位方面，GPS可以为车辆提供精度为米级的绝对定位，
差分GPS或RTK GPS可以为车辆提供精度为厘米级的绝对定位，然而现实是并非所有的路段在所有时间都可以得到良好的GPS信号。
因此，在自动驾驶领域，RTK GPS的输出一般都要与IMU和汽车自身的传感器，如轮速计、方向盘转角传感器等进行融合。
旋转的表示方式：
旋转矩阵 Rotation matrix
欧拉角 Euler angles
轴角 Axis and angle
四元数 Quaternion
李群 Lie group
欧拉角＜--＞四元数＜--＞旋转矩阵
旋转实例
四元数 -> 旋转矩阵
from scipy.spatial.transform import Rotation
ret =Rotation.from_quat() 从四元数初始化
输入参数： quat： 数组，形状 (N, 4) 或 (4,)
输出返回： 包含由输入四元数表示的旋转的对象
旋转对象转换为矩阵 ret.as_matrix()
旋转矩阵 -> 四元数
ret =Rotation.from_matrix(matrix)
Q = ret.as_quat() # 得到四元数
旋转矩阵 ->欧拉角 默认是 弧度制， 角度制，需要设置 degrees = True
ret = Rotation.from_euler('zyx',euler,degree =True)
大写的'ZYX' 代表外旋，'zyx'代表内旋(固定轴);大部分情况需要写成[Rz,Ry,Rx]
E = ret.as_euler('ZYX',degree=True) # 得到欧拉角 欧拉角中，返回值E的顺序和'zyx'对应，为[Rz,Ry,Rx]
RT矩阵-旋转/平移矩阵
空间刚性姿态的方式有多种：欧拉角，四元素，RT矩阵
//激光坐标系点到IMU系，先旋转，后平移
Pimu = Pldr * R + T
//IMU坐标系点到激光系
Pldr = (Pimu - T)* R.inv()
numpy
（1）np.linalg.inv()：矩阵求逆
（2）np.linalg.det()：矩阵求行列式（标量）
np.dot()函数主要有两个功能，向量点积和矩阵乘法
01. 如果处理的是一维数组，则得到的是两数组的內积 一维时是內积
02. np.dot(x, y), 当x为二维矩阵，y为一维向量，这时y会转换一维矩阵进行计算
03. np.dot(a ,b), 其中a和b都是二维矩阵，此时dot就是进行的矩阵乘法运算
x.dot(y) 等价于 np.dot(x,y) x是m*n 矩阵 ，y是n*m矩阵 则x.dot(y) 得到m*m矩
图像去畸变
向矫正是通过畸变坐标算出标准坐标，而逆向矫正是通过标准坐标算出畸变坐标。
undistort()函数一次性完成
Opencv中UndistortPoints就是执行的正向矫正过程，
initUndistortRectifyMap执行的是逆向矫正过程。
正向矫正的流程为：畸变像素坐标→畸变物理坐标→标准物理坐标→标准像素坐标
逆向矫正的流程为：标准像素坐标→标准物理坐标→畸变物理坐标→畸变像素坐标
cv2.fisheye.undistortImage
# map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2)
# undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
cv2.fisheye.undistortImage(img,undistorted_img,K,D,K,DIM)
点云格式
点云格式
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7 //PCD文件版本
FIELDS x y z r g b intensity timestamp //每个点包含哪些维度，xyz表示XYZ三维坐标，rgb表示颜色（可以分开表示，也可以一个浮点数表示）
SIZE 4 4 4 1 1 1 1 8 //每个维度的数据占用字节大小
TYPE F F F U U U U F //每个维度的数据类型，I表示有符号类型int8（char）、int16(short)、int32(int)，
U表示无符号类型uint8(unsigned char)、uint16(unsigned short)、uint32(unsigned int)，
F表示浮点型
COUNT 1 1 1 1 1 1 1 1 //每个维度含有多少个元素（如果未提供COUNT属性，默认值为1）
WIDTH 32 //用点的数量表示点云数据集的宽度。有两种含义：1.无序数据集的点云中点的数量 2.有序点云数据集的宽度（一行中点的数量），有序点云数据集中，点云类似图片或矩阵的结构，分为行和列，这种数据通常来自于立体摄像机（stereo camera）、时间飞行摄像机（Time Of Flight camera，使用红外线或者光脉冲来估计光线从发射到检测到的时间延迟来测量距离），知道点的相邻关系，使算法计算更高效。
HEIGHT 2172 //用点云数据集中点的数量表示点云数据集的高度。高度有如下两种含义：1.有序的点云数据集中，行的数量 2.无序点云数据集中，高度为1（可以用来判断一个数据集是有序的还是无序的）
VIEWPOINT 0 0 0 1 0 0 0 //指定数据集合中点的采集视点。可以用来后续可能的坐标转换，或者求平面法线坐标。格式是平移(tx ty tz) + 四元数(qw qx qy qz)，默认是0 0 0 1 0 0 0。
POINTS 69504 //点云中点的总数（冗余字段）
DATA binary_compressed //点云数据的存储类型，0.7版本支持两种存储方式：ascii和binary
ASCII格式：每行一个点的数据，比如x y z rgb格式的数据
binary格式：数据是pcl::PointCloud::points数组或者vector的完整内存复制。Linux系统中，使用mmap/munmap操作使得数据读/写尽可能快。
FIELDS
//每个点包含哪些维度，xyz表示XYZ三维坐标，rgb表示颜色（可以分开表示，也可以一个浮点数表示），
intensity表示激光反射强度，timestamp表示时间戳，normal_x、normal_y、normal_z表示平面法线三维坐标，j1、j2、j3表示不变矩。
SIZE - 以字节为单位指定每个尺寸的大小。例子：
unsigned char/char has 1 byte
unsigned short/short has 2 bytes
unsigned int/int/float has 4 bytes
double 8 bytes
TYPE - 将每个维度的类型指定为char。目前接受的类型是：
I - 表示有符号类型int8 (char)，int16 (short)和int32 (int)
U - 表示无符号类型uint8 (unsigned char)，uint16 (unsigned short)，uint32 (unsigned int)
F - 代表float类型
COUNT - 指定每个维度有多少个元素。例如，x数据通常具有1个元素，但是像VFH的特征描述符 具有308个。
基本上这是一种在每个点处引入nD直方图描述符的方法，并将它们视为单个连续的存储器块。默认情况下，如果COUNT不存在，则所有维度的计数设置为1。
WIDTH - 指定点云数据集中点的宽度。WIDTH有两个含义：
1、它可以指定云中点的总数（与POINTS相同，见下文），用于无组织的数据集;
2、它可以指定有组织的点云数据集的宽度（连续点的总数）。
HEIGHT - 指定点云数据集中点的高度。高度有两个含义：
1、它可以指定有组织的点云数据集的高度（总行数）
2、对于未组织的数据集它被设置为1（因此用于检查数据集是否被组织）。
VIEWPOINT -指定数据集中的点采集的视点。这可能稍后可能用于在不同坐标系之间建立变换，或用于辅助诸如曲面法线之类的需要一致定向的特征。
视点信息被指定为平移(tx ty tz)+四元数(qw qx qy qz)。默认值是：
VIEWPOINT 0 0 0 1 0 0 0
POINTS - 指定云中的总点数。
DATA - 指定点云数据存储的数据类型。从版本0.7开始，支持两种数据类型：ascii和binary
点云操作
Open3d读取pcd格式点云文件的函数为 o3d.io.read_point_cloud，读取的点云存储为PointCloud类
保存点云文件的函数为o3d.io.write_point_cloud
open3d.io.write_point_cloud(path, pcd , write_ascii=True)
write_ascii (bool, optional, default=False) –
Set to True to output in ascii format,
otherwise binary format will be used.
参考
Open3d-Point cloud （Open3D点云） https://blog.csdn.net/qq_41068877/article/details/124117332
传感器标定 坐标系旋转 https://blog.csdn.net/littleleafboy/article/details/123270915?spm=1001.2014.3001.5502
【自动驾驶传感器融合系列】03 传感器之间的标定 https://blog.csdn.net/Kefenggewu_/article/details/121621777
