在使用M2DGR数据集进行多传感器融合估计时,既需要明确所使用传感器的话题名称,也需要知道各传感器之间的外参情况。
本文介绍M2DGR数据集主要传感器的名称、在论文中的设备名称、在rosbag中话题topic名称、在标定文件calibration_results.txt中的名称之间的对应关系。各名具体参见本文文末附录。
数据集使用的传感器有
各传感器安装位置如下:
上图左上角的数字5应该是数字8 。
按照位置安装图的顺序介绍。
主要传感器
1 fish-eye camera
在calibration_results.txt中的名称:
%% Cam-left 8823
c图左下角相机%% Cam-right 8828
c图左上角相机
%% Cam-midleft8830
c图下方中间相机
%% Cam-midright 8827
c图上方中间相机
%% Cam-backleft6450
c图右下角相机
%% Cam-backright6548
c图右上角相机
依次对应话题topic:
/camera/left/image_raw/compressed ,
/camera/right/image_raw/compressed ,
/camera/third/image_raw/compressed ,
/camera/fourth/image_raw/compressed ,
/camera/fifth/image_raw/compressed ,
/camera/sixth/image_raw/compressed 。
对应关系参考文章:
2 Sky-pointing camera
在calibration_results.txt中的名称:
%% Cam-head 3199
对应话题topic:
/camera/head/image_raw/compressed
3 LIDAR
Device:LiDAR
Type:Velodyne VLP-32C
Spec.:32 beam,360 H-FOV,40V-FOV
LIDAR Velodyne VLP-32C, 360 Horizontal Field of View (FOV),-30 to +10 vertical FOV,10Hz,Max Range 200 m,Range Resolution 3 cm, Horizontal Angular Resolution 0.2°.
在calibration_results.txt中的名称:
%%velodyne VLP-32C
对应话题topic:
/velodyne_points 4606 msgs : sensor_msgs/PointCloud2
4 GNSS-IMU
Device:RTK/INS
Type:Xsens Mti 680G
Spec.:GNSS-RTK,localization precision 2cm,100Hz;
IMU 9-axis,100 Hz;
在calibration_results.txt中的名称:
%% Xsens IMU
对应话题:
5 IMU
Device:IMU
Type:Handsfree A9
Spec.:9-axis,150Hz;
在calibration_results.txt中的名称:
%% Handsfree IMU
对应话题topic:
/handsfree/imu 65753 msgs : sensor_msgs/Imu
/handsfree/mag 65753 msgs : sensor_msgs/MagneticField
6 Antenaa天线
Device:GNSS Receiver
Type:Ublox M8T
Spec.:GPS/BeiDou, 1Hz
在calibration_results.txt中的名称:
%% UBLOX,Xsens GNSS
对应话题:
/ublox/aidalm 460 msgs : ublox_msgs/AidALM
/ublox/aideph 461 msgs : ublox_msgs/AidEPH
/ublox/fix 460 msgs : sensor_msgs/NavSatFix
/ublox/fix_velocity 460 msgs : geometry_msgs/TwistWithCovarianceStamped
/ublox/monhw 462 msgs : ublox_msgs/MonHW
/ublox/navclock 461 msgs : ublox_msgs/NavCLOCK
/ublox/navpvt 460 msgs : ublox_msgs/NavPVT
/ublox/navsat 23 msgs : ublox_msgs/NavSAT
/ublox/navstatus 461 msgs : ublox_msgs/NavSTATUS
/ublox/rxmraw 1846 msgs : ublox_msgs/RxmRAWX
7 Infrared Camera
Device:Infrared Camera
Type:Gaode PLUG 617
Spec.:640*512,90.2 H-FOV,70.6 V-FOV,25Hz;
在calibration_results.txt中的名称:
%% Cam-thermal
对应话题topic:
/thermal_image_raw 11538 msgs : sensor_msgs/Image
8 VI-senser
Device:VI-sensor
Type:Realsense d435i
Spec.:RGB: 640*480, 69 H-FOV, 42.5V-FOV
IMU: 6-axis
在calibration_results.txt中的名称:
%% Cam-pinhole-color realsense d435i
%% realsense d435i IMU
对应话题topic:
/camera/color/image_raw/compressed 6823 msgs : sensor_msgs/CompressedImage
/camera/imu 87842 msgs : sensor_msgs/Imu
9 Event Camera
Device:Event Camera
Type:Inivation DVXplorer
Spec.:640*480, 65.2 H-FOV,51.3 V-FOV,15Hz;
在calibration_results.txt中的名称:
%% Cam-event
对应话题topic:
/dvs/events 6923 msgs : dvs_msgs/EventArray
/dvs/imu 372669 msgs : sensor_msgs/Imu
/dvs_rendering/compressed 6923 msgs : sensor_msgs/CompressedImage
其他
运动捕捉系统Motion-capture System
Device:Mocap System
Type:Vicon Vero 2.2
Spec.:localization accuracy 1mm, 50 Hz
在calibration_results.txt中的名称:
无
对应话题topic:
无
激光跟踪器Laser Tracker
Device:Laser Tracker
Type:Leica Nova MS60
Spec.:localization accuracy 1mm + 1.5ppm,10 Hz
在calibration_results.txt中的名称:
%% leica
对应话题topic:
无
msgs数量是根据door_01序列的数量。
附录
项目地址
https://github.com/SJTU-ViSYS/M2DGR
设备名
话题名
rosbag info door_01.bag中的topics:
topics: /camera/color/image_raw/compressed 6823 msgs : sensor_msgs/CompressedImage
/camera/fifth/image_raw/compressed 6923 msgs : sensor_msgs/CompressedImage
/camera/fourth/image_raw/compressed 6923 msgs : sensor_msgs/CompressedImage
/camera/head/image_raw/compressed 6922 msgs : sensor_msgs/CompressedImage
/camera/imu 87842 msgs : sensor_msgs/Imu
/camera/left/image_raw/compressed 6923 msgs : sensor_msgs/CompressedImage
/camera/right/image_raw/compressed 6923 msgs : sensor_msgs/CompressedImage
/camera/sixth/image_raw/compressed 6925 msgs : sensor_msgs/CompressedImage
/camera/third/image_raw/compressed 6923 msgs : sensor_msgs/CompressedImage
/dvs/events 6923 msgs : dvs_msgs/EventArray
/dvs/imu 372669 msgs : sensor_msgs/Imu
/dvs_rendering/compressed 6923 msgs : sensor_msgs/CompressedImage
/handsfree/imu 65753 msgs : sensor_msgs/Imu
/handsfree/mag 65753 msgs : sensor_msgs/MagneticField
/thermal_image_raw 11538 msgs : sensor_msgs/Image
/ublox/aidalm 460 msgs : ublox_msgs/AidALM
/ublox/aideph 461 msgs : ublox_msgs/AidEPH
/ublox/fix 460 msgs : sensor_msgs/NavSatFix
/ublox/fix_velocity 460 msgs : geometry_msgs/TwistWithCovarianceStamped
/ublox/monhw 462 msgs : ublox_msgs/MonHW
/ublox/navclock 461 msgs : ublox_msgs/NavCLOCK
/ublox/navpvt 460 msgs : ublox_msgs/NavPVT
/ublox/navsat 23 msgs : ublox_msgs/NavSAT
/ublox/navstatus 461 msgs : ublox_msgs/NavSTATUS
/ublox/rxmraw 1846 msgs : ublox_msgs/RxmRAWX
/velodyne_points 4606 msgs : sensor_msgs/PointCloud2
外参名
calibration_results.txt原文:
%% SLAM-Scenes Dataset Calibration
%%All translation in meters%% Cam-head 3199
% Extrinsic [to LIDAR], [opencv-matrix]
%to be changed
data: [ 0., -1, 0., 0.07410,
-1, 0., 0., 0.00127,
0., 0., -1, 0.65608,
0., 0., 0., 1. ]
u8
% Intrinsic, [opencv-matrix]
data: [ 542.993253538048, 0., 629.0025857364897,
0., 541.3882904458247, 503.71809588651786,
0., 0., 1. ]% Distortion Coefficients [opencv-matrix]
data: [-0.057963907006683066, -0.026465594265953234, 0.011980216320790046, -0.003041081642470451]% Image Size
data: [1280, 1024]%% Cam-left 8823
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., 1., 0.24221,
-1., 0., 0., 0.16123,
0., -1., 0., -0.16711,
0., 0., 0., 1. ]% Intrinsic, [opencv-matrix]
data: [ 540.645056202188, 0., 626.4125666883942,
0., 539.8545023658869, 523.947634226782,
0., 0., 1. ]% Distortion Coefficients [opencv-matrix]
data: [-0.07015146608431883, 0.008586142263125124, -0.021968993685891842, 0.007442211946112636]% Image Size
data: [1280, 1024]%% Cam-right 8828
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., 1., 0.242013,
-1., 0., 0., -0.16025,
0., -1., 0., -0.16724,
0., 0., 0., 1. ]% Intrinsic, [opencv-matrix]
data: [ 540.6832252229977, 0., 632.9173957218305,
0., 539.3921307247979, 503.3766864767991,
0., 0., 1. ]% Distortion Coefficients [opencv-matrix]
data: [-0.07147685334620411, 0.006423830171528276, -0.02354604292216998, 0.009181757660952325]% Image Size
data: [1280, 1024]%% Cam-midleft8830
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 1., 0., 0., 0.00109,
0., 0., 1., 0.16004,
0., -1., 0., -0.16718,
0., 0., 0., 1. ]% Intrinsic, [opencv-matrix]
data: [ 538.3154329292029, 0., 632.4020370259001,
0., 537.4277766778052, 509.3609761132556,
0., 0., 1. ]% Distortion Coefficients [opencv-matrix]
data: [-0.061526128889893804, -0.00867447574360461], -0.00984399833727642, 0.004810173767131135]% Image Size
data: [1280, 1024]%% Cam-midright 8827
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ -1., 0., 0., 0.00021,
0., 0., -1., -0.16013,
0., -1., 0., -0.16674,
0., 0., 0., 1. ]% Intrinsic, [opencv-matrix]
data: [ 537.2294180909289, 0., 635.5687263167875,
0., 536.6425889117285, 491.9422582452684,
0., 0., 1. ]% Distortion Coefficients [opencv-matrix]
data: [-0.06329338788105426, -0.005282288794043655, -0.01439687642303018, 0.006593296524853555]% Image Size
data: [1280, 1024]%% Cam-backleft6450
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., -1, -0.24175,
1., 0., 0., 0.16031,
0., -1., 0., -0.16715,
0., 0., 0., 1. ]% Intrinsic, [opencv-matrix]
data: [ 539.834690518987, 0.0, 630.8171732844409,
0., 538.7141533225924, 501.86380820583685,
0., 0., 1. ]% Distortion Coefficients [opencv-matrix]
data: [-0.057504608980455875, -0.03555561603037192, 0.030555976552383957, -0.014358151534147164]% Image Size
data: [1280, 1024]%% Cam-backright6548
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., -1., -0.24313,
1., 0., 0., -0.16037,
0, -1, 0, -0.16689,
0., 0., 0., 1. ]% Intrinsic, [opencv-matrix]
data: [ 543.4124571628414, 0., 642.967852391304,
0., 542.2071506815953, 504.2993692252895,
0., 0., 1. ]% Distortion Coefficients [opencv-matrix]
data: [-0.06681929469733765, -0.005533273650165602, -0.006167142895316966, 0.0018089751112068567]% Image Size
data: [1280, 1024]
%% Cam-pinhole-color realsense d435i
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., 1., 0.30456,
-1, 0., 0., 0.00065,
0., -1, 0., 0.65376,
0., 0., 0., 1. ]% Intrinsic, [opencv-matrix]
data:[617.971050917033,0,0,
0,616.445131524790,0,
327.710279392468,253.976983707814,1]% Distortion Coefficients [opencv-matrix]
data: [0.148000794688248,-0.217835187249065,0,0]
"depth_to_color_extrinsics"rotation: [0.9999781847000122, -0.006335411686450243, -0.001878829556517303, 0.006338413339108229, 0.9999786615371704, 0.0015958998119458556, 0.0018686787225306034, -0.0016077737091109157, 0.9999969601631165]
translation: [0.014734288677573204, -0.00018310551240574569, 0.000172588144778274]% Image Size
data: [640, 480]
%% Cam-thermal
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., 1., 0.30456,
-1, 0., 0., 0.17065,
0., -1, 0., 0.65376,
0., 0., 0., 1. ]
% Intrinsic, [opencv-matrix]
data:[435.836180277211,0,0
0,435.909430631362,0
324.452753232850,255.554320082190,1]% Distortion Coefficients [opencv-matrix]
data: [-0.436107594318055,0.166413618922992,0,0]% Image Size
data: [640, 512]%% Cam-event
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., 1, 0.30474,
-1, 0., 0., -0.17065,
0., -1, 0., 0.65376,
0., 0., 0., 1. ]% Image Size
data: [640, 480]
%% realsense d435i IMU
% Extrinsic [to LIDAR], [opencv-matrix]
data: [ 0., 0., 1., 0.30456,
-1, 0., 0., 0.00065,
0., -1, 0., 0.65376,
0., 0., 0., 1. ]gyr_n: 2.4710787075320089e-03
gyr_w: 1.7963145905200798e-05
acc_n: 2.6848761610624401e-02
acc_w: 8.5216274964016023e-04
%% Xsens IMU
% Extrinsic [to LIDAR], [opencv-matrix]
data: [1., 0., 0., 0.15905,
0., 1, 0., 0.00067,
0., 0., 1., -0.16824]gyr_n: 2.1309311394972831e-03
gyr_w: 3.6603917782528627e-05
acc_n: 1.2820343288774358e-02
acc_w: 1.3677912958097768e-03
%% UBLOX,Xsens GNSS
% Extrinsic [to LIDAR]
data: [1., 0., 0., -0.09825,
0., 1, 0., 0.00582,
0., 0., 1., 0.72673]
%% leica
% Extrinsic [to LIDAR]
data: [1., 0., 0., -0.21374,
0., 1, 0., 0.00146,
0., 0., 1., 0.68356]%% Handsfree IMU
% Extrinsic [to LIDAR]
data: [1., 0., 0., -0.27255,
0., 1, 0., 0.00053,
0., 0., 1., -0.17954]
gyr_n: 2.3417543020438883e-03
gyr_w: 1.4428407712885209e-05
acc_n: 3.7686306102624571e-02
acc_w: 1.1416642385952368e-03%%velodyne VLP-32C
N_SCAN = 32;
Horizon_SCAN = 1800
ang_res_x = 360.0/(Horizon_SCAN)
ang_res_y = 41.33/(N_SCAN-1)
ang_bottom = 30.67
groundScanInd = 20