mirrors/FAST_LIO
1
0
Fork 0
mirror of https://github.com/hku-mars/FAST_LIO.git synced 2023-04-06 16:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix the scan time calculation bug and change the definition of blind to the 'minimum range'.

Browse Source
This commit is contained in:
xw
2021-08-25 03:05:43 -04:00
parent dcc38885ac
commit 94973cb309
6 changed files with 82 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/IMU_Processing.hpp
View File

@@ -219,11 +219,11 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
const double &imu_end_time = v_imu.back()->header.stamp.toSec();
const double &pcl_beg_time = meas.lidar_beg_time;
const double &pcl_end_time = meas.lidar_end_time;
/*** sort point clouds by offset time ***/
pcl_out = *(meas.lidar);
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000);
// cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
// <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;

54
src/laserMapping.cpp
View File

@@ -91,7 +91,7 @@ double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_en
int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, pcd_save_interval = -1, pcd_index = 0;
bool point_selected_surf[100000] = {0};
bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited;
bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited;
bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
vector<vector<int>> pointSearchInd_surf;
@@ -360,6 +360,8 @@ void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
sig_buffer.notify_all();
}
double lidar_mean_scantime = 0.0;
int scan_num = 0;
bool sync_packages(MeasureGroup &meas)
{
if (lidar_buffer.empty() || imu_buffer.empty()) {
@@ -370,13 +372,25 @@ bool sync_packages(MeasureGroup &meas)
if(!lidar_pushed)
{
meas.lidar = lidar_buffer.front();
if(meas.lidar->points.size() <= 1)
{
lidar_buffer.pop_front();
return false;
}
meas.lidar_beg_time = time_buffer.front();
lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
if (meas.lidar->points.size() <= 1) // time too little
{
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
ROS_WARN("Too few input point cloud!\n");
}
else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime)
{
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
}
else
{
scan_num ++;
lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;
}
meas.lidar_end_time = lidar_end_time;
lidar_pushed = true;
}
@@ -684,6 +698,13 @@ void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_
}
}
if (effct_feat_num < 1)
{
ekfom_data.valid = false;
ROS_WARN("No Effective Points! \n");
return;
}
res_mean_last = total_residual / effct_feat_num;
match_time += omp_get_wtime() - match_start;
double solve_start_ = omp_get_wtime();
@@ -835,12 +856,11 @@ int main(int argc, char** argv)
ros::spinOnce();
if(sync_packages(Measures))
{
if (flg_reset)
if (flg_first_scan)
{
ROS_WARN("reset when rosbag play back");
p_imu->Reset();
flg_reset = false;
Measures.imu.clear();
first_lidar_time = Measures.lidar_beg_time;
p_imu->first_lidar_time = first_lidar_time;
flg_first_scan = false;
continue;
}
@@ -859,9 +879,7 @@ int main(int argc, char** argv)
if (feats_undistort->empty() || (feats_undistort == NULL))
{
first_lidar_time = Measures.lidar_beg_time;
p_imu->first_lidar_time = first_lidar_time;
// cout<<"FAST-LIO not ready"<<endl;
ROS_WARN("No point, skip this scan!\n");
continue;
}
@@ -896,6 +914,12 @@ int main(int argc, char** argv)
// cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;
/*** ICP and iterated Kalman filter update ***/
if (feats_down_size < 5)
{
ROS_WARN("No point, skip this scan!\n");
continue;
}
normvec->resize(feats_down_size);
feats_down_world->resize(feats_down_size);

26
src/preprocess.cpp
View File

@@ -121,13 +121,13 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
plsize--;
for(uint i=0; i<plsize; i++)
{
types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y;
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
types[i].dista = sqrt(vx * vx + vy * vy + vz * vz);
}
types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y);
give_feature(pl, types);
// pl_surf += pl;
}
@@ -147,12 +147,12 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
pl_full[i].y = msg->points[i].y;
pl_full[i].z = msg->points[i].z;
pl_full[i].intensity = msg->points[i].reflectivity;
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms
if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)
|| (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
|| (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)
&& (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y > blind))
&& (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind)))
{
pl_surf.push_back(pl_full[i]);
}
@@ -183,7 +183,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
for (uint i = 0; i < plsize; i++)
{
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
if (range < blind) continue;
if (range < (blind * blind)) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
added_pt.x = pl_orig.points[i].x;
@@ -237,7 +237,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
if (range < blind) continue;
if (range < (blind * blind)) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
@@ -248,13 +248,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;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
if (yaw_angle >= 180.0)
yaw_angle -= 360.0;
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 / 1e6; // curvature unit: ms
pl_surf.points.push_back(added_pt);
}
@@ -391,7 +385,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;
added_pt.curvature = pl_orig.points[i].time / 1000.0; // curvature unit: ms
if (!given_offset_time)
{
@@ -427,7 +421,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
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 * blind))
{
pl_surf.points.push_back(added_pt);
}