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
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 163
Max Intensity: 234
Min Color: 0; 0; 0
Min Intensity: 0
Name: surround
@ -326,25 +326,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/ThirdPersonFollower
Distance: 247.9972686767578
Distance: 89.18838500976562
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 165.75035095214844
Y: -44.61741256713867
Z: 1.1274762073298916e-5
X: -4.903029441833496
Y: -19.24248504638672
Z: -2.85572896245867e-5
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.5697963237762451
Pitch: 0.29979732632637024
Target Frame: global
Value: ThirdPersonFollower (rviz)
Yaw: 4.727225303649902
Yaw: 4.977229118347168
Saved: ~
Window Geometry:
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);
}
#define MAX_LINE_NUM 64
void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
pl_surf.clear();
@ -276,11 +274,13 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
int plsize = pl_orig.points.size();
pl_surf.reserve(plsize);
bool is_first[MAX_LINE_NUM];
double yaw_fp[MAX_LINE_NUM]={0}; // yaw of first scan point
/*** These variables only works when no point timestamps given ***/
double omega_l=3.61; // scan angular velocity
float yaw_last[MAX_LINE_NUM]={0.0}; // yaw of last scan point
float time_last[MAX_LINE_NUM]={0.0}; // last offset time
std::vector<bool> is_first(N_SCANS,true);
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)
{
@ -289,7 +289,6 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
else
{
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_end = yaw_first;
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;
// added_pt.curvature = pl_orig.points[i].t;
yaw_last[layer] = yaw_angle;
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(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);
// 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)