mirrors/FAST_LIO_LOCALIZATION
1
0
Fork 0
mirror of https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git synced 2023-05-28 12:51:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

may better support 32 line or above

Browse Source
This commit is contained in:
xw
2021-07-19 10:08:36 -04:00
parent 5cd5be3201
commit f159d57da8
2 changed files with 13 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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)