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;
h_dyn_share(x_, dyn_share);
if(! dyn_share.valid)
{
continue;
}
//Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);
#ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView();
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
#endif
#ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView();
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
#endif
double solve_start = omp_get_wtime();
dof_Measurement = h_x_.rows();
vectorized_state dx;
x_.boxminus(dx, x_propagated);
dx_new = dx;
if(! dyn_share.valid)
{
continue;
}
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());
};
double lidar_beg_time;
double lidar_end_time;
PointCloudXYZI::Ptr lidar;
deque<sensor_msgs::Imu::ConstPtr> imu;
};

38
rviz_cfg/loam_livox.rviz
View File

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