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

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

main
xw 2021-08-25 03:05:43 -04:00
parent dcc38885ac
commit 94973cb309
6 changed files with 82 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
include/IKFoM_toolkit/esekfom/esekfom.hpp
View File

@ -1634,22 +1634,25 @@ public:
{ {
dyn_share.valid = true; dyn_share.valid = true;
h_dyn_share(x_, dyn_share); h_dyn_share(x_, dyn_share);
if(! dyn_share.valid)
{
continue;
}
//Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share); //Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);
#ifdef USE_sparse #ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView(); spMt h_x_ = dyn_share.h_x.sparseView();
#else #else
Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x; Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
#endif #endif
double solve_start = omp_get_wtime(); double solve_start = omp_get_wtime();
dof_Measurement = h_x_.rows(); dof_Measurement = h_x_.rows();
vectorized_state dx; vectorized_state dx;
x_.boxminus(dx, x_propagated); x_.boxminus(dx, x_propagated);
dx_new = dx; dx_new = dx;
if(! dyn_share.valid)
{
continue;
}
P_ = P_propagated; P_ = P_propagated;

1
include/common_lib.h
View File

@ -60,6 +60,7 @@ struct MeasureGroup // Lidar data and imu dates for the curent process
this->lidar.reset(new PointCloudXYZI()); this->lidar.reset(new PointCloudXYZI());
}; };
double lidar_beg_time; double lidar_beg_time;
double lidar_end_time;
PointCloudXYZI::Ptr lidar; PointCloudXYZI::Ptr lidar;
deque<sensor_msgs::Imu::ConstPtr> imu; deque<sensor_msgs::Imu::ConstPtr> imu;
}; };

38
rviz_cfg/loam_livox.rviz
View File

@ -16,7 +16,7 @@ Panels:
- /Odometry1/Odometry1/Covariance1/Orientation1 - /Odometry1/Odometry1/Covariance1/Orientation1
- /MarkerArray1/Namespaces1 - /MarkerArray1/Namespaces1
Splitter Ratio: 0.6432291865348816 Splitter Ratio: 0.6432291865348816
Tree Height: 1146 Tree Height: 1144
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Tool Properties - Class: rviz/Tool Properties
@ -85,8 +85,8 @@ Visualization Manager:
Enabled: true Enabled: true
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 234 Max Intensity: 150
Min Color: 0; 0; 0 Min Color: 238; 238; 236
Min Intensity: 0 Min Intensity: 0
Name: surround Name: surround
Position Transformer: XYZ Position Transformer: XYZ
@ -325,34 +325,34 @@ Visualization Manager:
Value: true Value: true
Views: Views:
Current: Current:
Class: rviz/ThirdPersonFollower Class: rviz/Orbit
Distance: 89.18838500976562 Distance: 135.68701171875
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: -4.903029441833496 X: 11.471270561218262
Y: -19.24248504638672 Y: 33.732704162597656
Z: -2.85572896245867e-5 Z: -18.399494171142578
Focal Shape Fixed Size: false Focal Shape Fixed Size: true
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: 0.29979732632637024 Pitch: 0.04979678615927696
Target Frame: global Target Frame: global
Value: ThirdPersonFollower (rviz) Value: Orbit (rviz)
Yaw: 4.977229118347168 Yaw: 4.707205772399902
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
collapsed: false collapsed: true
Height: 1385 Height: 1383
Hide Left Dock: false Hide Left Dock: true
Hide Right Dock: false Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001c8000004b7fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004b7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bd00000052fc0100000002fb0000000800540069006d00650100000000000009bd000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000697000004b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd0000000400000000000001c8000004b5fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000004b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000061f00000052fc0100000002fb0000000800540069006d006501000000000000061f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000061f000004b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection: Selection:
collapsed: false collapsed: false
Time: Time:
@ -360,7 +360,7 @@ Window Geometry:
Tool Properties: Tool Properties:
collapsed: false collapsed: false
Views: Views:
collapsed: false collapsed: true
Width: 2493 Width: 1567
X: 67 X: 67
Y: 27 Y: 27

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_beg_time = v_imu.front()->header.stamp.toSec();
const double &imu_end_time = v_imu.back()->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_beg_time = meas.lidar_beg_time;
const double &pcl_end_time = meas.lidar_end_time;
/*** sort point clouds by offset time ***/ /*** sort point clouds by offset time ***/
pcl_out = *(meas.lidar); pcl_out = *(meas.lidar);
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); 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<<", " \ // 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; // <<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 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; 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 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; bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
vector<vector<int>> pointSearchInd_surf; vector<vector<int>> pointSearchInd_surf;
@ -360,6 +360,8 @@ void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
sig_buffer.notify_all(); sig_buffer.notify_all();
} }
double lidar_mean_scantime = 0.0;
int scan_num = 0;
bool sync_packages(MeasureGroup &meas) bool sync_packages(MeasureGroup &meas)
{ {
if (lidar_buffer.empty() || imu_buffer.empty()) { if (lidar_buffer.empty() || imu_buffer.empty()) {
@ -370,13 +372,25 @@ bool sync_packages(MeasureGroup &meas)
if(!lidar_pushed) if(!lidar_pushed)
{ {
meas.lidar = lidar_buffer.front(); meas.lidar = lidar_buffer.front();
if(meas.lidar->points.size() <= 1)
{
lidar_buffer.pop_front();
return false;
}
meas.lidar_beg_time = time_buffer.front(); 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; 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; res_mean_last = total_residual / effct_feat_num;
match_time += omp_get_wtime() - match_start; match_time += omp_get_wtime() - match_start;
double solve_start_ = omp_get_wtime(); double solve_start_ = omp_get_wtime();
@ -835,12 +856,11 @@ int main(int argc, char** argv)
ros::spinOnce(); ros::spinOnce();
if(sync_packages(Measures)) if(sync_packages(Measures))
{ {
if (flg_reset) if (flg_first_scan)
{ {
ROS_WARN("reset when rosbag play back"); first_lidar_time = Measures.lidar_beg_time;
p_imu->Reset(); p_imu->first_lidar_time = first_lidar_time;
flg_reset = false; flg_first_scan = false;
Measures.imu.clear();
continue; continue;
} }
@ -859,9 +879,7 @@ int main(int argc, char** argv)
if (feats_undistort->empty() || (feats_undistort == NULL)) if (feats_undistort->empty() || (feats_undistort == NULL))
{ {
first_lidar_time = Measures.lidar_beg_time; ROS_WARN("No point, skip this scan!\n");
p_imu->first_lidar_time = first_lidar_time;
// cout<<"FAST-LIO not ready"<<endl;
continue; 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; // 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 ***/ /*** 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); normvec->resize(feats_down_size);
feats_down_world->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--; plsize--;
for(uint i=0; i<plsize; i++) 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; vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y; vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z; 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); give_feature(pl, types);
// pl_surf += pl; // 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].y = msg->points[i].y;
pl_full[i].z = msg->points[i].z; pl_full[i].z = msg->points[i].z;
pl_full[i].intensity = msg->points[i].reflectivity; 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) 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].y - pl_full[i-1].y) > 1e-7)
|| (abs(pl_full[i].z - pl_full[i-1].z) > 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]); 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++) 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; 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; Eigen::Vector3d pt_vec;
PointType added_pt; PointType added_pt;
added_pt.x = pl_orig.points[i].x; 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; 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; Eigen::Vector3d pt_vec;
PointType added_pt; 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_x = 0;
added_pt.normal_y = 0; added_pt.normal_y = 0;
added_pt.normal_z = 0; added_pt.normal_z = 0;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; added_pt.curvature = pl_orig.points[i].t / 1e6; // curvature unit: ms
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;
pl_surf.points.push_back(added_pt); 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.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z; added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity; 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) 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 (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); pl_surf.points.push_back(added_pt);
} }