自定义文件格式及坐标系

北京大学信息科学技术学院 赵卉菁Zhaohj@cis.pku.edu.cn

20100108

北京大学智能车实验平台 Poss ・ v

L5L4

GPS/IMU

LadyBug2

Laser ScannerL2,L1,L3

Video Camera

POSS-v

数据格式• 导航数据

– POS ：– XYZ ：– NAV

• 激光数据– LMS– DS– PT

• 标定参数文件– Calib

1. 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 .

惯性导航厂家自定义

每行数据项列表 - 》

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文件内容一致

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 ）

NAV坐标系定义

与 XYZ数据内容一致

計測角度範囲 ( 例、 180), float

角度解像度 ( 例、 0.5), float

距離値単位 ( 例、 100), float

背景データ（現在無用） ,LMSDATBUF

Scan #1,LMSDATBUF

Scan #2,LMSDATBUF

Scan #n,LMSDATBUF

4. LMS data format （ Binary ）

typedef struct {long milli;unsigned short dat[MAXDATLEN];

} LMSDATBUF;

MAXDATLEN = range/resolution + 1

e.g. 180/0.5+1 = 361 100/0.25+1 = 401

自定义

5. DS 文件格式（ Binary ）計測角度範囲 ( 例、 180), float

角度解像度 ( 例、 0.5), float

距離値単位 ( 例、 100), float

Scan #1,DSDATBUF

Scan #2,DSDATBUF

Scan #n,LMSDATBUF

typedef struct {point3d rot;point3d shv;long milli;unsigned short dat[MAXDATLEN];

} DSDATBUF;

MAXDATLEN = range/resolution + 1

e.g. 180/0.5+1 = 361 100/0.25+1 = 401

typedef struct {double x,y,z;

} point3d;

自定义

6. PT 文件格式（ Binary ）

typedef struct {point3d p;BYTE r, g, b;

} RGBpoint3d;

typedef struct {double x,y,z;

} point3d;

Scan #1,Laser point #1, RGBpoint3d

Scan #1,Laser point #2, RGBpoint3d

Scan #1,Laser point #m1, RGBpoint3d

Scan #2,Laser point #1, RGBpoint3d

Scan #n,Laser point #mn, RGBpoint3d

自定义

7. Calib 文件格式

• 自定义• 每行数据项列表

No Field Example1 LD token2 Name FL3 roll （ deg ） -454 pitch （ de

g ）0

5 yaw （ deg ）

0

6 X(m) -0.797 Y(m) 08 Z(m) 0

LD FLrot -45 0 0 shv -0.79 0 0

举例

Camera-Laser Calibration

10

tRT

(u,v)

(x,y,z)

World Coordinate System(3D)

Camera Coordinate System(3D)

Image Coordinate System(2D)

Pinhole Model

0100

00

0

0

0

vf

uf

v

u K

z

y

x

v

u

TK

1

Totally 11 unknowns

Extrinsic Parameters: 6

Intrinsic Parameters: 5

Tsai’s Method

Laser scanner’s C.S.

Video camera’s C.S.

Global C.S.

1. Transform from laser scanner C.S. to vehicle’s body C.S.

Vehicle’s body C.S. at time k l

Laser point P measured at time k l

Pc

Vehicle’s body C.S. at time kc

Tlv

Tvw(k )l

Tvc

Twc(k )c

2. Transform from vehicle’s body C.S. at time k to global C.S. l

3. Transform from global C.S. to vehicle’s body C.S. at time k c

4. Transform from vehicle’s body C.S. to video camera C.S.

C.S. : coordinate system

Video Imagecaptured at time kc

惯性导航数据格式及坐标系转换• GPS84_TokyoXYZ

– 从 WGS84 （经度纬度坐标）转换到 东京直角坐标系数据，输出XYZ ， PLT ， PNT 等格式数据

命令行例、GPS84_TokyoXYZ.exe -pos dat.POS -oziplt dat.pltGPS84_TokyoXYZ.exe -pos dat.POS -ozipnt dat.pntGPS84_TokyoXYZ.exe -pos dat.POS -xyz dat.xyz

• Xyz2Navi– 对 XYZ 数据进行分析，输出 NAV 数据

• 注意– GPS 时钟有跳秒问题。– POS 数据中的 UTC 为修正跳秒， GPS 时为原始数据。相差 14 秒。– UTC 时间加上 8 小时为北京时间– INS 数据与激光数据有时间差约 3.841 秒

惯性导航数据坐标系定义

东

北

地心

航向角heading

+

俯仰角Pitch

横滚角roll

NAV 坐标系定义

x

y

z

航向角heading

俯仰角Pitch

横滚角roll

vehicleheading

Vehicle Frame

x

y

z

y

x

z

x

y

z

Vehicle Frame

Sensor Frame

x

y

z

y

x

z

Vehicle body CS

UR Laser CS

LD URrot 0 -90 -90 shv 0.79 -4.2 0

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)

x

y

z

y

x

z

Vehicle body CS

UL Laser CS

LD ULrot 0 90 90 Shv -0.79 -4.2 0

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)

x

y

z

Vehicle body CS

FL Laser CS

LD FLrot -45 0 0 shv -0.79 0 0

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)

x

y

z

三维激光点阵数据生成

• 通过车体的运动及垂直二维激光扫描合成为三维激光点阵数据– LMS+Calib+NAV- 》 3D Laser Points– Calib 定义了激光扫描仪坐标系与车体坐标系（ NA

V ）的关系– NAV 数据有两个：惯性导航， FM 激光 SLAM

三维激光点阵数据生成• Lms2Ri

– 将 LMS 数据转换为距离图像• Lms2Ds

– 修正 LMS 时钟参数（ -3.841s ），将 LMS 数据与NAV 数据合成，输出 DS 格式数据

• Ds2Pt– 根据激光扫描仪与车体的标定参数将 DS 数据转换成全

局坐标系中的激光点阵，输出 Pt 格式数据• Datviewer

– 显示三维激光点阵数据