12345qiupeng
/
Fast-Lio
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add timestamp unit setup

main
xw 2022-03-13 17:46:05 +08:00
parent 8ad5dd2ba2
commit ad692a628c
8 changed files with 46 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
README.md
View File

@ -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

1
config/ouster64.yaml
View File

@ -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:

3
config/velodyne.yaml
View File

@ -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

30
rviz_cfg/loam_livox.rviz
View File

@ -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

2
src/IMU_Processing.hpp
View File

@ -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);};

1
src/laserMapping.cpp
View File

@ -773,6 +773,7 @@ int main(int argc, char** argv)
nh.param<double>("preprocess/blind", p_pre->blind, 0.01);
nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA);
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, US);
nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false);

27
src/preprocess.cpp
View File

@ -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<<added_pt.curvature<<endl;
if (!given_offset_time)
{

4
src/preprocess.h
View File

@ -11,6 +11,7 @@ typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> 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<orgtype> 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;