坐标系
GS84：大地坐标系，是目前广泛使用的GPS全球卫星定位系统使用的坐标系。
GCJ02：又称火星坐标系，是由中国国家测绘局制定的地理坐标系统，是由WGS84加密后得到的坐标系。是国内最广泛使用的坐标体系，
高德、腾讯、Google中国地图都使用它
BD09：为百度坐标系，在GCJ02坐标系基础上再次加密。其中bd09ll表示百度经纬度坐标，bd09mc表示百度墨卡托米制坐标。
定位和时间同步--授时
latitude longitude height
north_velocity east_velocity up_velocity
roll pitch azimuth
基于时间的坐标转换
全球导航卫星系统定位,简称GNSS(Global Navigation Satellite System)
相机
1.相机的内参和外参以及标定
01.相机的焦距、像素大小 --针孔模型是凸透镜成像的一种简化模型
从相机坐标系转换到像素坐标系中，相机内参的作用
相机内参 是相机出厂就定下来的，可以通过相机标定的方式人为计算出
fx，fy为焦距，一般情况下，二者相等，
x0、y0为主点坐标（相对于成像平面），
s为坐标轴倾斜参数
02.相机外参数是在世界坐标系中的参数，比如相机的位置、旋转方向
从世界坐标系转换到相机坐标系中，相机外参的作用
相机外参是旋转矩阵 和平移向量 构成
03.相机的标定
标定： 标定就是已知标定控制点的世界坐标和像素坐标 去解算这个映射关系
标定过程就是确定摄像机的几何和光学参数，以及摄像机相对于世界坐标系的方位
标定方法： 通过标定板标定
2.坐标系
相机坐标系 转 图像物理坐标系
像素坐标系
世界坐标系
3.矩阵
内参矩阵
激光雷达
车身坐标系
激光雷达自身坐标系
三维旋转表示
几种三维旋转表示（欧拉角，四元数，旋转矩阵，轴角）
01.欧拉角(Euler Angle)与旋转矩阵(Rotation Matrix)
欧拉角Eulerian angles用来确定定点转动刚体位置的3个一组独立角参量，由章动角 θ、旋进角（即进动角）ψ和自转角j组成
α 是 x-轴与交点线的夹角，β 是 z-轴与Z-轴的夹角，γ 是交点线与X-轴的夹角
ZXY顺规，分别对应着Z-Roll，X-Pitch，Y-Yaw。
yaw(偏航), pitch(俯仰), roll(横滚).三个变量一般对应(车体,飞行器的)z,y,x三个坐标轴.
y 俯仰角 pitch 坡度
z 横摆角 偏航角 yaw 曲率
x 旋转角 滚转角 roll
X轴、Y轴、Z轴方向的3个转动自由度和 沿 轴、Y轴、Z轴的三个平动自由度
02.四元数引入到计算机图形学中。
四元数在一些方面优于Euler angles(欧拉角)和matrices。
任意一个三维空间中的定向(orientation，即调置朝向)都可以被表示为一个绕某个特定轴的旋转
四元数由四部分组成，一个实部，三个虚部。三个虚部与旋转轴密切相关，而旋转角度影响四个部分
般来说，我们都会用单位四元数来表示旋转，那么给定一个单位四元数，可以构造旋转矩阵(column major)
03.旋转矩阵
坐标
让机器人去哪，机器人需要先知道自己在哪
map和base_link都很好理解，一个是绝对坐标系，另一个是以机器人自身为原点的坐标系
里程计坐标系，这里主要区分odom topic，这是两个概念，
一个是坐标系，
一个是根据编码里程计计算得到的里程计信息
坐标系 里程计坐标系
里程计
里程计(Odometry)--航迹推演(Odometry)
位姿(位置和转角)
速度(前进速度和转向速度)
多传感器融合
摄像头的图像信息以15Hz速率获取并更新，
而GNSS位置确定则只有在1Hz或5Hz速率下。
当将多传感器数据进行融合时，数据同步是非常重要的一方面。
为了简化多速率系统中的数据同步，ROS提供了基于时间戳的同步原理-消息过滤器（Message Filter），
它从多个数据源采集不同类型的数据，只有每个信息源的信息具有相同时间戳的时候才会将信息发布出去。
时间同步
传感器时间同步的方法主要有两种，一种是软同步，另一种是硬同步
odom 50Hz、Imu 100Hz、camera 25Hz
IMU（Inertial Measurement Unit），即惯性测量单元
变换
表示
非齐次表述
齐次表述：齐次坐标（homogeneous coordinates）
齐次坐标就是将一个原本是n维的向量用一个n+1维向量来表示，是指一个用于投影几何里的坐标系统
齐次位姿变换(RT矩阵)
旋转和平移 旋转 Rotation Matrix 平移 translation
变换矩阵：对于齐次坐标，我们可以把旋转和平移写在一个矩阵里--变换矩阵（Transform Matrix）
旋转向量和欧拉角
四元数表示旋转
变换：
欧式变换： 保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转
仿射变换： 仿射变换也叫正交投影，经过仿射变换之后，立方体就不再是方的了，但是各个方面仍然是平行四边形
射影变换： 是最一般的变换，又称为投影变换。
相似变换： 比欧式变换多了一个自由度，它允许物体进行均匀缩放
欧氏变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，
不改变它自身的样子。但现实中由于角度问题，总会发生畸变，
所以需要相似、仿射、射影变换
时间戳软同步
### 时间到 1675652209 秒，是10位
### 时间戳 1675652247664 毫秒 是13位
### 1m/s=3.6km/h。
# -*- coding: utf-8 -*-
import os
import os.path
import argparse
from glob import glob
def get_pcd_timestamp(pcd_file):
#"1672356612.725828.jpg"
return float(pcd_file.split('/')[-1][0:-4])
def get_img_timestamp(img_file):
# "1672356612.703884.pcd"
return float(img_file.split('/')[-1][0:-4])
def find_match_img(pcd_file, img_list,match_gap=1):
#match_gap=1 1s
match_img = None
for img_file in img_list:
time_gap = abs(get_img_timestamp(img_file) - get_pcd_timestamp(pcd_file))
if time_gap <= match_gap:
match_img = img_file
match_gap = time_gap
if not match_img:
raise Exception('No match img ', pcd_file)
return match_img
if __name__ == "__main__":
parser = argparse.ArgumentParser( description='查看时间戳之间的插值')
parser.add_argument('--pcd_path', type=str, default='/data/cloud/exp/pcd', help='bag path')
parser.add_argument('--img_path', type=str, default='/data/cloud/exp/img', help='bag path')
args = parser.parse_args()
pcd_path = args.pcd_path
pcd_pattern = os.path.join(pcd_path, '*.pcd')
pcd_list = sorted(glob(pcd_pattern))
img_path = args.img_path
img_pattern = os.path.join(img_path, '*.jpg')
img_list = glob(img_pattern)
for i,sig_pcd in enumerate(pcd_list):
match_sig_img = find_match_img(sig_pcd,img_list)
gap_time =abs(get_img_timestamp(match_sig_img) - get_pcd_timestamp(sig_pcd))
print(i,sig_pcd,match_sig_img,round(gap_time,6) )
