From ad692a628c8cb88c518d05e111b21b8974579098 Mon Sep 17 00:00:00 2001 From: xw Date: Sun, 13 Mar 2022 17:46:05 +0800 Subject: [PATCH] add timestamp unit setup --- README.md | 7 ++++--- config/ouster64.yaml | 1 + config/velodyne.yaml | 3 ++- rviz_cfg/loam_livox.rviz | 30 +++++++++++------------------- src/IMU_Processing.hpp | 2 +- src/laserMapping.cpp | 1 + src/preprocess.cpp | 27 +++++++++++++++++++++++---- src/preprocess.h | 4 +++- 8 files changed, 46 insertions(+), 29 deletions(-) diff --git a/README.md b/README.md index c274f311..587cf005 100644 --- a/README.md +++ b/README.md @@ -123,9 +123,10 @@ Edit ``` config/velodyne.yaml ``` to set the below parameters: 1. LiDAR point cloud topic name: ``` lid_topic ``` 2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine) -3. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ``` -4. Translational extrinsic: ``` extrinsic_T ``` -5. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) +3. Set the parameter ```timestamp_unit``` based on the unit of **time** (Velodyne) or **t** (Ouster) field in PoindCloud2 rostopic +4. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ``` +5. Translational extrinsic: ``` extrinsic_T ``` +6. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) - The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). Step B: Run below diff --git a/config/ouster64.yaml b/config/ouster64.yaml index 3f7f8f02..06154363 100644 --- a/config/ouster64.yaml +++ b/config/ouster64.yaml @@ -6,6 +6,7 @@ common: preprocess: lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 64 + timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. blind: 4 mapping: diff --git a/config/velodyne.yaml b/config/velodyne.yaml index 4a8eed37..c2d24b76 100644 --- a/config/velodyne.yaml +++ b/config/velodyne.yaml @@ -7,7 +7,8 @@ preprocess: lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 scan_rate: 10 # only need to be set for velodyne, unit: Hz, - blind: 4 + timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 2 mapping: acc_cov: 0.1 diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index 4dda6b6f..ea3cdf35 100644 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -16,7 +16,7 @@ Panels: - /Odometry1/Odometry1/Covariance1/Orientation1 - /MarkerArray1/Namespaces1 Splitter Ratio: 0.6432291865348816 - Tree Height: 1144 + Tree Height: 811 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -85,9 +85,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 159 Min Color: 238; 238; 236 - Min Intensity: 0 Name: surround Position Transformer: XYZ Queue Size: 1 @@ -100,7 +98,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Alpha: 0.15000000596046448 + - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 15 @@ -115,9 +113,7 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 159 Min Color: 0; 0; 0 - Min Intensity: 0 Name: currPoints Position Transformer: XYZ Queue Size: 100000 @@ -145,9 +141,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 151 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 @@ -275,9 +269,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 138; 226; 52 - Max Intensity: 248 Min Color: 138; 226; 52 - Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 @@ -326,33 +318,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 135.68701171875 + Distance: 46.0853271484375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 11.471270561218262 - Y: 33.732704162597656 - Z: -18.399494171142578 + X: -4.982542037963867 + Y: -15.83572006225586 + Z: -3.063523054122925 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.04979678615927696 + Pitch: 0.399796724319458 Target Frame: global Value: Orbit (rviz) - Yaw: 4.707205772399902 + Yaw: 1.277182698249817 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1383 + Height: 1028 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001c8000004b5fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000061f00000052fc0100000002fb0000000800540069006d006501000000000000061f000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000004b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c800000368fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002700000368000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000061f00000052fc0100000002fb0000000800540069006d006501000000000000061f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004510000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -363,4 +355,4 @@ Window Geometry: collapsed: true Width: 1567 X: 67 - Y: 27 + Y: 24 diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp index 32369dbe..0774911f 100644 --- a/src/IMU_Processing.hpp +++ b/src/IMU_Processing.hpp @@ -26,7 +26,7 @@ /// *************Preconfiguration -#define MAX_INI_COUNT (20) +#define MAX_INI_COUNT (10) const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 0f7fd941..a9632c4b 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -773,6 +773,7 @@ int main(int argc, char** argv) nh.param("preprocess/blind", p_pre->blind, 0.01); nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); + nh.param("preprocess/timestamp_unit", p_pre->time_unit, US); nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); nh.param("point_filter_num", p_pre->point_filter_num, 2); nh.param("feature_extract_enable", p_pre->feature_enabled, false); diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 61cb94a2..490fb45a 100644 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -49,6 +49,25 @@ void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, Point void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { + switch (time_unit) + { + case SEC: + time_unit_scale = 1.e3f; + break; + case MS: + time_unit_scale = 1.f; + break; + case US: + time_unit_scale = 1.e-3f; + break; + case NS: + time_unit_scale = 1.e-6f; + break; + default: + time_unit_scale = 1.f; + break; + } + switch (lidar_type) { case OUST64: @@ -200,7 +219,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) if (yaw_angle <= -180.0) yaw_angle += 360.0; - added_pt.curvature = pl_orig.points[i].t / 1e6; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; if(pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); @@ -249,7 +268,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; - added_pt.curvature = pl_orig.points[i].t / 1e6; // curvature unit: ms + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms pl_surf.points.push_back(added_pt); } @@ -318,7 +337,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms + added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // units: ms if (!given_offset_time) { @@ -387,7 +406,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].time / 1000.0; // curvature unit: ms + added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // curvature unit: ms // cout< PointCloudXYZI; enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3} +enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; enum Surround{Prev, Next}; enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; @@ -94,7 +95,8 @@ class Preprocess PointCloudXYZI pl_full, pl_corn, pl_surf; PointCloudXYZI pl_buff[128]; //maximum 128 line lidar vector typess[128]; //maximum 128 line lidar - int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; + float time_unit_scale; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; double blind; bool feature_enabled, given_offset_time; ros::Publisher pub_full, pub_surf, pub_corn;