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

may better support 32 line or above

main
xw 2021-07-19 10:08:36 -04:00
parent 5cd5be3201
commit f159d57da8
2 changed files with 13 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
rviz_cfg/loam_livox.rviz
View File

@ -85,7 +85,7 @@ Visualization Manager:
Enabled: true Enabled: true
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 163 Max Intensity: 234
Min Color: 0; 0; 0 Min Color: 0; 0; 0
Min Intensity: 0 Min Intensity: 0
Name: surround Name: surround
@ -326,25 +326,25 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/ThirdPersonFollower Class: rviz/ThirdPersonFollower
Distance: 247.9972686767578 Distance: 89.18838500976562
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
Focal Point: Focal Point:
X: 165.75035095214844 X: -4.903029441833496
Y: -44.61741256713867 Y: -19.24248504638672
Z: 1.1274762073298916e-5 Z: -2.85572896245867e-5
Focal Shape Fixed Size: false Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806 Focal Shape Size: 0.05000000074505806
Invert Z Axis: false Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.009999999776482582 Near Clip Distance: 0.009999999776482582
Pitch: 1.5697963237762451 Pitch: 0.29979732632637024
Target Frame: global Target Frame: global
Value: ThirdPersonFollower (rviz) Value: ThirdPersonFollower (rviz)
Yaw: 4.727225303649902 Yaw: 4.977229118347168
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:

22
src/preprocess.cpp
View File

@ -263,8 +263,6 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
// pub_func(pl_surf, pub_corn, msg->header.stamp); // pub_func(pl_surf, pub_corn, msg->header.stamp);
} }
#define MAX_LINE_NUM 64
void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
{ {
pl_surf.clear(); pl_surf.clear();
@ -276,11 +274,13 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
int plsize = pl_orig.points.size(); int plsize = pl_orig.points.size();
pl_surf.reserve(plsize); pl_surf.reserve(plsize);
bool is_first[MAX_LINE_NUM]; /*** These variables only works when no point timestamps given ***/
double yaw_fp[MAX_LINE_NUM]={0}; // yaw of first scan point
double omega_l=3.61; // scan angular velocity double omega_l=3.61; // scan angular velocity
float yaw_last[MAX_LINE_NUM]={0.0}; // yaw of last scan point std::vector<bool> is_first(N_SCANS,true);
float time_last[MAX_LINE_NUM]={0.0}; // last offset time std::vector<double> yaw_fp(N_SCANS, 0.0); // yaw of first scan point
std::vector<float> yaw_last(N_SCANS, 0.0); // yaw of last scan point
std::vector<float> time_last(N_SCANS, 0.0); // last offset time
/*****************************************************************/
if (pl_orig.points[plsize - 1].time > 0) if (pl_orig.points[plsize - 1].time > 0)
{ {
@ -289,7 +289,6 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
else else
{ {
given_offset_time = false; given_offset_time = false;
memset(is_first, true, sizeof(is_first));
double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
double yaw_end = yaw_first; double yaw_end = yaw_first;
int layer_first = pl_orig.points[0].ring; int layer_first = pl_orig.points[0].ring;
@ -422,28 +421,19 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l; if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l;
// added_pt.curvature = pl_orig.points[i].t;
yaw_last[layer] = yaw_angle; yaw_last[layer] = yaw_angle;
time_last[layer]=added_pt.curvature; time_last[layer]=added_pt.curvature;
} }
// if(i==(plsize-1)) printf("index: %d layer: %d, yaw: %lf, offset-time: %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature, prints);
if (i % point_filter_num == 0) if (i % point_filter_num == 0)
{ {
if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > blind) if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > blind)
{ {
pl_surf.points.push_back(added_pt); pl_surf.points.push_back(added_pt);
// printf("time mode: %d time: %d \n", given_offset_time, pl_orig.points[i].t);
} }
} }
} }
} }
// pub_func(pl_surf, pub_full, msg->header.stamp);
// pub_func(pl_surf, pub_surf, msg->header.stamp);
// pub_func(pl_surf, pub_corn, msg->header.stamp);
} }
void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types) void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)