北京大学信息科学技术学院 赵卉菁 Zhaohj@cis.pku.edu.cn 20100108 自定义文件格式及坐标系 北京大学信息科学技术学院 赵卉菁 Zhaohj@cis.pku.edu.cn.

Presentation on theme: "北京大学信息科学技术学院 赵卉菁 Zhaohj@cis.pku.edu.cn 20100108 自定义文件格式及坐标系 北京大学信息科学技术学院 赵卉菁 Zhaohj@cis.pku.edu.cn."— Presentation transcript:

1 北京大学信息科学技术学院 赵卉菁 Zhaohj@cis.pku.edu.cn
自定义文件格式及坐标系 北京大学信息科学技术学院 赵卉菁

2 北京大学智能车实验平台 Poss・v POSS-v L5 L4 GPS/IMU LadyBug2 Laser Scanner
L2,L1,L3 Video Camera POSS-v

3 数据格式 导航数据 POS： XYZ： NAV 激光数据 LMS DS PT 标定参数文件 Calib

4 POS data format （ASCII）
1 GPS秒 2 纬度（deg) 3 经度（deg） 4 高度（m） 5 北向速度（m/s） 6 东向速度（m/s） 7 天向速度（m/s） 8 . 9 10 航向角（deg）、Heading 11 府仰角（deg）、Pitch 12 基线（m） 13 14 15 16 UTC时间（hh:mm:ss） 17 18 19 横滚角（deg）、Roll 20 21 22 23 POS data format （ASCII） 惯性导航厂家自定义 每行数据项列表 -》

5 2. XYZ格式（ASCII） 自定义 每行数据项列表 1 北京时间毫秒 （hhmmss.sss->millisec) 2 X（m）
3 Y（m） 4 Z（m） 5 roll（rad） 6 pitch（rad） 7 yaw（rad） 某直角坐标系 与POS文件内容一致

6 3.NAV格式（ASCII） 自定义 每行数据项列表 1 北京时间毫秒 （hhmmss.sss->millisec) 2
roll（rad） 3 pitch（rad） 4 yaw（rad） 5 X（m） 6 Y（m） 7 Z（m） 8 V（m/s) 9 瞬时yaw（rad） 与XYZ数据内容一致 NAV坐标系定义

7 4. LMS data format （Binary）
計測角度範囲(例、180), float 自定义 角度解像度(例、0.5), float 距離値単位(例、100), float 背景データ（現在無用）,LMSDATBUF MAXDATLEN = range/resolution + 1 e.g. 180/0.5+1 = 361 100/ = 401 Scan #1,LMSDATBUF Scan #2,LMSDATBUF typedef struct { long milli; unsigned short dat[MAXDATLEN]; } LMSDATBUF; Scan #n,LMSDATBUF

8 5. DS文件格式（Binary） 自定义 typedef struct { double x,y,z; } point3d;
計測角度範囲(例、180), float typedef struct { double x,y,z; } point3d; 角度解像度(例、0.5), float 距離値単位(例、100), float MAXDATLEN = range/resolution + 1 e.g. 180/0.5+1 = 361 100/ = 401 Scan #1,DSDATBUF Scan #2,DSDATBUF typedef struct { point3d rot; point3d shv; long milli; unsigned short dat[MAXDATLEN]; } DSDATBUF; Scan #n,LMSDATBUF

9 6. PT文件格式（Binary） 自定义 typedef struct { double x,y,z; } point3d;
Scan #1,Laser point #1, RGBpoint3d Scan #1,Laser point #2, RGBpoint3d typedef struct { double x,y,z; } point3d; Scan #1,Laser point #m1, RGBpoint3d Scan #2,Laser point #1, RGBpoint3d typedef struct { point3d p; BYTE r, g, b; } RGBpoint3d; Scan #n,Laser point #mn, RGBpoint3d

10 7. Calib文件格式 自定义 每行数据项列表 No Field Example 1 LD token 2 Name FL 3
举例 自定义 每行数据项列表 LD FL rot shv No Field Example 1 LD token 2 Name FL 3 roll（deg） -45 4 pitch（deg） 5 yaw（deg） 6 X(m) -0.79 7 Y(m) 8 Z(m)

11 Camera-Laser Calibration
Totally 11 unknowns Pinhole Model Tsai’s Method (x,y,z) (u,v) World Coordinate System (3D) Intrinsic Parameters: 5 Image Coordinate System (2D) Camera Coordinate System (3D) Extrinsic Parameters: 6

12 T T T T P Video camera’s C.S. vc Vehicle’s body C.S. at time k wc (k )
4. Transform from vehicle’s body C.S. to video camera C.S. T vc Video Image captured at time k c Vehicle’s body C.S. at time k c Laser point P measured at time k l T wc (k ) c 1. Transform from laser scanner C.S. to vehicle’s body C.S. 3. Transform from global C.S. to vehicle’s body C.S. at time k c T lv Laser scanner’s C.S. 2. Transform from vehicle’s body C.S. at time k to global C.S. l T vw (k ) l Vehicle’s body C.S. at time k l C.S. : coordinate system Global C.S.

13 惯性导航数据格式及坐标系转换 GPS84_TokyoXYZ Xyz2Navi 注意
从 WGS84 （经度纬度坐标）转换到 东京直角坐标系数据，输出XYZ，PLT，PNT等格式数据 命令行例、 GPS84_TokyoXYZ.exe -pos dat.POS -oziplt dat.plt GPS84_TokyoXYZ.exe -pos dat.POS -ozipnt dat.pnt GPS84_TokyoXYZ.exe -pos dat.POS -xyz dat.xyz Xyz2Navi 对XYZ数据进行分析，输出NAV数据 注意 GPS时钟有跳秒问题。 POS数据中的UTC为修正跳秒，GPS时为原始数据。相差14秒。 UTC时间加上8小时为北京时间 INS数据与激光数据有时间差约3.841秒

14 惯性导航数据坐标系定义 北 俯仰角 Pitch 航向角 heading + 东 地心 横滚角 roll

15 NAV坐标系定义 y 俯仰角 Pitch x z vehicle heading 横滚角 roll 航向角 heading
Vehicle Frame

16 y x z Sensor Frame x y z x y z Vehicle Frame

17 Calibration Parameters (ur.calib)
y x z UR Laser CS x y z Vehicle body CS Calibration Parameters (ur.calib) For any laser points (x, y, 0) in UR laser CS, First rotate (R) then shift according to calibration parameters. The rotation parameter means that from the Vehicle body CS, first rotate Z by -90, then rotate Y by -90, finally rotate X by 0, to achieve UR laser CS. R = Rz(-90)・Ry(-90)・Rx(0) LD UR rot shv

18 Calibration Parameters (ur.calib)
y z UL Laser CS x x y z Vehicle body CS Calibration Parameters (ur.calib) For any laser points (x, y, 0) in UR laser CS, First rotate (R) then shift according to calibration parameters. The rotation parameter means that from the Vehicle body CS, first rotate Z by 90, then rotate Y by 90, finally rotate X by 0, to achieve UR laser CS. R = Rz(90)・Ry(90)・Rx(0) LD UL rot Shv

19 Calibration Parameters (ur.calib)
Vehicle body CS x y z x y z FL Laser CS Calibration Parameters (ur.calib) For any laser points (x, y, 0) in UR laser CS, First rotate (R) then shift according to calibration parameters. The rotation parameter means that from the Vehicle body CS, first rotate Z by 0, then rotate Y by 0, finally rotate X by -45, to achieve UR laser CS. R = Rz(0)・Ry(0)・Rx(-45) LD FL rot shv

20 三维激光点阵数据生成 通过车体的运动及垂直二维激光扫描合成为三维激光点阵数据 LMS+Calib+NAV-》3D Laser Points
NAV数据有两个：惯性导航，FM激光SLAM

21 三维激光点阵数据生成 Lms2Ri Lms2Ds Ds2Pt Datviewer 将LMS数据转换为距离图像
修正LMS时钟参数（-3.841s），将LMS数据与NAV数据合成，输出DS格式数据 Ds2Pt 根据激光扫描仪与车体的标定参数将DS数据转换成全局坐标系中的激光点阵，输出Pt格式数据 Datviewer 显示三维激光点阵数据