一、 Purpose目的
通常， 感知模块使用雷达点云和相机图像来对物体进行检测和分类，感知数据分别来自雷达和相机，就传感器数据融合准确性，我们需要雷达和相机同一时间捕捉到同一物体(时间间隔尽可能短)，否则一个真实的物体可能被感知处理成两个，尤其是移动的物体。
二、 Measurement timestamp
所有雷达相机都以10fps工作，因此我们在每100ms循环中为每个相机/雷达设定一个触发时间.
雷达mearement time在一个点云包中是最早的时间戳，因此可认为是雷达在一个合规FOV中初始位置的时刻.
1. 相关因素
1.1 雷达设定
同步角: 雷达在一整秒中的角度位置
旋转方向: 雷达位置按顺时针定义，逆时针旋转不会改变其位置。
方位角FOV: 雷达激光(带时间戳)的扫描范围
1.2 驱动配置
Start angle: 相机的数据帧的数据从lidar旋转到start angle开始发布
Early publish: 如果启用Early publish，发布的数据将不是从start angle开始
1.3 相机设定
mearement time: 为FPGA完成图像接收的时刻
曝光时间: 定义FPGA接收图像开始到结束的中间时刻为曝光时间
相机成像过程：
FPGA会发送信号触发相机曝光. 整个过程为: 曝光信号发出→ 等待→ 开始曝光→ FPGA开始接收图像→ 完成曝光→ FPGA完成接收图像.
相机的mearement time 为FPGA完成图像接收的时刻. 在这个过程中我们可以检测出信号发出、图像接收的开始与结束的时刻.
CaSH配置：
Delay map: Timing of the signal
相机术语：
Middle lane readout: Time between signal and exposure timing
Transmit done: Time between exposure timing and done receiving
3. Lidar VS Camera
The timing relationship between lidar and camera are defined in perception workflow, and other modules. trigger_times_online,max_time_offset determine the searching window of sensor. System will match the sensor since main lidar measurement time + trigger_times_online, and window size is max_time_offset.
For example, camera_trigger_times_online of left_0_n_12mm is 50, which means left_0_n_12mmwill exposure 50ms later after main lidar measurement time. Main lidar sync at 0 (rear of the vehicle), after 50ms it will scan at front of the vehicle. So, front camera and lidar will sensoring at the same direction at the same time.
Not all sensers are synchronized with main lidar, for other cases like blindspot fisheye to blindspot qt:
other lidar ------------main lidar ------------- cameras
All sensors link to main lidar (left hesai128 in Gen5 case). After capture main lidar frame, lidar/camera source will match other sensor frame according to the trigger set in workflow.
三、激光雷达与相机标定的时间戳同步问题
相机和激光雷达之间的时间戳同步问题一直是实时跑SLAM的先决条件。本文试图以最清晰的思路去讲明白这个事情。
开始本文之前，先介绍几个基本概念。
1. 相机特性-曝光和读出
相机获取一帧图像分为曝光和读出两个阶段。相机使用的传感器不同，相机的曝光时间和读出时间的重叠关系也有所不同，分为交叠曝光和非交叠曝光两种。交叠曝光和非交叠曝光相比，交叠曝光可以减少曝光时间对出图时间的影响。
- 非交叠曝光是指当前帧的曝光和读出都完成后，再进行下一帧的曝光和读出。非交叠曝光帧周期大于曝光时间与帧读出时间的和。
- 图片1
- 交叠曝光是指当前帧的曝光和前一帧的读出过程有重叠，即前一帧读出的同时，下一帧已经开始曝光。交叠曝光帧周期小于等于曝光时间与帧读出时间的和。
- 图片2
- 对！上一段就是为了告诉你：后文叙述中无论当前帧曝光时间和上一帧的读出时间是否重叠都不要惊讶。
2. 图像采集：触发模式（外部输入）
相机的触发模式分为内触发模式和外触发模式 2 种：
- 内触发模式：相机通过设备内部给出的信号采集图像。
- 外触发模式：相机通过外部给出的信号采集图像。外部信号可以是软件信号，也可以是硬件信号，主要包含软触发和硬件触发。外触发模式如图：
- 图片3
- 软触发：触发信号由软件发出(也可以利用相机SDK提供的API接口进行软触发)。
- 硬件触发：外部设备通过相机的 I/O 接口与相机进行连接，触发脉冲信号由外部设备给到相机进行采图。其实是直接对相机内部寄存器进行读写。下图是海康相机的电源IO的6-pin线缆：
- 图片4 5 6
其中，海康相机有1个光耦隔离输入Line0+，1个可配置输入输出Line2+，可选择一个配置为输入信号。
3. 触发输出
相机触发输出信号为开关信号，可用于控制报警灯、光源、PLC等外部设备。触发输出信号可通过Strobe 信号实现。
相机的曝光发生时，会立即输出一个Strobe信号，该信号为高电平有效。后文我们主要使用该信号对Lidar等其他传感器进行硬触发。这里有一个Strobe的预输出的概念。Strobe信号早于曝光生效。其工作原理为延迟曝光先进行Strobe输出。该功能可应用于响应比较慢的外部设备。Strobe预输出时序如图所示。（后面会讲为啥要延迟曝光）
图片7
现在我们回到正题吧，现在讲起来就很快了。
要同步相机和激光雷达的时间戳主要有三个方式，硬触发、软触发、软触发+硬触发。下面我以手绘示意图的形式一一介绍。
3.1 硬触发
一个MCU产生脉冲信号对三个传感器设备进行硬触发。
图片8
3.2 软触发+硬触发
对于软触发+硬触发来说，可以先用相机SDK的API对一个相机进行软触发，然后利用相机的外触发信号Strobe对雷达和相机等其他传感器进行硬触发。
图片9
这里需要注意一个问题，如果进行软触发的第一个相机在曝光的同时发出Strobe信号，其他被硬触发的传感器总归是晚了一步，不能完全同步。因此引出了之前Strobe的预输出的概念，即先进行Strobe输出再延迟曝光。
配置该模式时注意四点：
上升沿or下降沿的触发方式；
有效电平宽度，电平幅值，你要知道能触发Lidar的最低幅值；
Strobe预输出
两边电平信号是否一样，3v or 5v是否需要升压…
3.3 软触发
首先调用API操作相比于硬触发（对传感器内部寄存器直接进行读写操作）明显是慢了，执行第二句命令API(2)前API(1)就已经花了些时间。
//读取lidar和image数据的线程1
while(1)
{
API(1); //软触发第一个sensor
API(2); //软触发第二个sensor
//假设脉冲周期为0.5s
}
//处理数据线程2
for(i=0;i<nimage;i++)
{
t1 = getTickCount();
//部分程序有互斥锁
t2 = getTickCount();
Time = (t2 - t1) / getTickFrequency(); //当前帧处理时间
sleep(1/fps-Time);
}
如上图示意，如果处理单帧数据的时间超过0.5s，线程1就会读到下一帧数据，就会导致线程2数据混乱。因此线程2单帧处理时间需要小于0.5s，且每帧需要等待（1/fps-当前帧处理时间)。
参考开源的相机雷达融合SLAM方案里面的同步策略
四、其他配置
camera_dev: "/dev/cam18"
frame_id: "left_0_d_6mm"
pixel_format: "yuyv"
io_method: IO_METHOD_MMAP
width: 3840
height: 2160
frame_rate: 10
monochrome: false
brightness: -1
contrast: -1
saturation: -1
sharpness: -1
gain: -1
auto_focus: false
focus: -1
auto_exposure: true
exposure: 100
auto_white_balance: true
white_balance: 4000
bytes_per_pixel: 2
trigger_internal: 0
trigger_fps: 10
channel_name: "/xurban/sensor/camera/left_0_d_6mm/image"
device_wait_ms: 2000
spin_rate: 200
output_type: RGB
enable_resize: false
resize_width: 1080
resize_height: 1080
enable_crop: true
enable_rotate: true
rotate_conf {
degree: 270
}
crop_conf {
left: 840
top: 0
right: 3000
bottom: 2160
}
compress_conf {
output_channel: "/xurban/sensor/camera/left_0_d_6mm/image/compressed"
image_pool_size: 100
enable_gpu: true
gpu_id:2
}
expected_camera_timestamp: 66
五、相机相对位姿
1. 车体坐标系下的相机位姿
# 车体坐标系下的left_0_n_12mm位姿
sensors {
type: CAMERA
name: "left_0_n_12mm"
# 旋转矩阵，可转换为欧拉角（roll、raw、pitch）
rotation: 0.017411604405759951
rotation: -0.00022887741607596768
rotation: 0.99984838032931056
rotation: -0.999832635816136
rotation: 0.0056125925571275381
rotation: 0.017412615017435462
rotation: -0.0056157269320245562
rotation: -0.9999842230855025
rotation: -0.00013111486889849644
# 在车体坐标系下，left_0_n_12mm的x,y,z值
translation: 1.9697127138123502
translation: 0.59083726886812571
translation: 1.7389388576640445
# left_0_n_12mm内参值
intrinsics {
type: OPENCV_CAMERA
# fx值: F/dx F为焦距，dx为x方向一个像素占多少长度单位，即一个像素代表的实际物理值的大小，其是实现图像物理坐标系与像素坐标系转换的关键。
intrinsics: 7297.8068297162345
intrinsics: 0
intrinsics: 1901.9166533314315 # u0: 图像的中心像素坐标和图像原点像素坐标之间相差的横向像素数
intrinsics: 0
# fy值: F/dx F为焦距，dy为y方向一个像素占多少长度单位，即一个像素代表的实际物理值的大小，其是实现图像物理坐标系与像素坐标系转换的关键。
intrinsics: 7295.6526676894682
intrinsics: 1113.9395387987856 # v0: 图像的中心像素坐标和图像原点像素坐标之间相差的纵向像素数
intrinsics: 0
intrinsics: 0
intrinsics: 1
distortion_coefficients: -0.22587575074233643
distortion_coefficients: -1.1739644238946158e-08
distortion_coefficients: 0.0016867427178622757
distortion_coefficients: 7.5756201008206e-05
distortion_coefficients: -1.4486758200704789e-07
width: 3840
height: 2160
}
2. 相对位姿
在主传感器坐标系下的相对位姿
# 在左雷达坐标系下的left_0_n_12mm坐标参数
relative_poses {
from_sensor: "left_0_n_12mm"
from_type: CAMERA
to_sensor: "hs_lidar_left"
to_type: LIDAR
# 旋转矩阵，可转换为欧拉角（roll、raw、pitch）
rotation: -0.0056118996990375336
rotation: -0.0081353988855295614
rotation: 0.99995115974068516
rotation: -0.99993567843801179
rotation: 0.0099017277148572124
rotation: -0.0055312543761649726
rotation: -0.0098562451512205218
rotation: -0.9999178821649477
rotation: -0.0081904431067488064
# 在左雷达坐标系下，left_0_n_12mm的x,y,z值
translation: 0.11476581543684228
translation: 0.0068730292841804319
translation: -0.10572796314954802
}
