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

Support MARSIM simulator.

main
zfc-zfc 2024-07-23 19:18:04 +08:00
parent 5032d7c53f
commit 34b50d95da
7 changed files with 162 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
README.md
View File

@ -204,7 +204,22 @@ Step B: Run below
Step C: Run LiDAR's ros driver or play rosbag.
### 3.4 PCD file save
### 3.3 For MARSIM Simulator
Install MARSIM: https://github.com/hku-mars/MARSIM and run MARSIM as below
```
cd ~/$MARSIM_ROS_DIR$
roslaunch test_interface single_drone_avia.launch
```
Then Run FAST-LIO:
```
roslaunch fast_lio mapping_marsim.launch
```
### 3.5 PCD file save
Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds.

35
config/marsim.yaml Normal file
View File

@ -0,0 +1,35 @@
common:
lid_topic: "/quad0_pcl_render_node/sensor_cloud"
imu_topic: "/quad_0/imu"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
# This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
preprocess:
lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 4
blind: 0.5
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 90
det_range: 50.0
extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic
extrinsic_T: [ -0.0, -0.0, 0.0 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1]
publish:
path_en: false
scan_publish_en: true # false: close all the point cloud output
dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: true
interval: -1 # how many LiDAR frames saved in each pcd file;
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

21
launch/mapping_marsim.launch Normal file
View File

@ -0,0 +1,21 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_lio)/config/marsim.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.2" />
<param name="filter_size_map" type="double" value="0.3" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
</group>
</launch>

79
src/IMU_Processing.hpp
View File

@ -23,6 +23,7 @@
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Vector3.h>
#include "use-ikfom.hpp"
#include "preprocess.h"
/// *************Preconfiguration
@ -59,6 +60,7 @@ class ImuProcess
V3D cov_bias_gyr;
V3D cov_bias_acc;
double first_lidar_time;
int lidar_type;
private:
void IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
@ -218,10 +220,16 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
v_imu.push_front(last_imu_);
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 ***/
double pcl_beg_time = meas.lidar_beg_time;
double pcl_end_time = meas.lidar_end_time;
if (lidar_type == MARSIM) {
pcl_beg_time = last_lidar_end_time_;
pcl_end_time = meas.lidar_beg_time;
}
/*** sort point clouds by offset time ***/
pcl_out = *(meas.lidar);
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
// cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
@ -298,39 +306,42 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
/*** undistort each lidar point (backward propagation) ***/
if (pcl_out.points.begin() == pcl_out.points.end()) return;
auto it_pcl = pcl_out.points.end() - 1;
for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
{
auto head = it_kp - 1;
auto tail = it_kp;
R_imu<<MAT_FROM_ARRAY(head->rot);
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
vel_imu<<VEC_FROM_ARRAY(head->vel);
pos_imu<<VEC_FROM_ARRAY(head->pos);
acc_imu<<VEC_FROM_ARRAY(tail->acc);
angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
{
dt = it_pcl->curvature / double(1000) - head->offset_time;
if(lidar_type != MARSIM){
auto it_pcl = pcl_out.points.end() - 1;
for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
{
auto head = it_kp - 1;
auto tail = it_kp;
R_imu<<MAT_FROM_ARRAY(head->rot);
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
vel_imu<<VEC_FROM_ARRAY(head->vel);
pos_imu<<VEC_FROM_ARRAY(head->pos);
acc_imu<<VEC_FROM_ARRAY(tail->acc);
angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
/* Transform to the 'end' frame, using only the rotation
* Note: Compensation direction is INVERSE of Frame's moving direction
* So if we want to compensate a point at timestamp-i to the frame-e
* P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
M3D R_i(R_imu * Exp(angvel_avr, dt));
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
// save Undistorted points and their rotation
it_pcl->x = P_compensate(0);
it_pcl->y = P_compensate(1);
it_pcl->z = P_compensate(2);
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
{
dt = it_pcl->curvature / double(1000) - head->offset_time;
if (it_pcl == pcl_out.points.begin()) break;
}
/* Transform to the 'end' frame, using only the rotation
* Note: Compensation direction is INVERSE of Frame's moving direction
* So if we want to compensate a point at timestamp-i to the frame-e
* P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
M3D R_i(R_imu * Exp(angvel_avr, dt));
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
// save Undistorted points and their rotation
it_pcl->x = P_compensate(0);
it_pcl->y = P_compensate(1);
it_pcl->z = P_compensate(2);
if (it_pcl == pcl_out.points.begin()) break;
}
}
}
}

11
src/laserMapping.cpp
View File

@ -94,6 +94,7 @@ int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudVal
bool point_selected_surf[100000] = {0};
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;
int lidar_type;
vector<vector<int>> pointSearchInd_surf;
vector<BoxPointType> cub_needrm;
@ -375,6 +376,8 @@ bool sync_packages(MeasureGroup &meas)
{
meas.lidar = lidar_buffer.front();
meas.lidar_beg_time = time_buffer.front();
if (meas.lidar->points.size() <= 1) // time too little
{
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
@ -390,6 +393,8 @@ bool sync_packages(MeasureGroup &meas)
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;
}
if(lidar_type == MARSIM)
lidar_end_time = meas.lidar_beg_time;
meas.lidar_end_time = lidar_end_time;
@ -774,7 +779,7 @@ int main(int argc, char** argv)
nh.param<double>("mapping/b_gyr_cov",b_gyr_cov,0.0001);
nh.param<double>("mapping/b_acc_cov",b_acc_cov,0.0001);
nh.param<double>("preprocess/blind", p_pre->blind, 0.01);
nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA);
nh.param<int>("preprocess/lidar_type", lidar_type, AVIA);
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, US);
nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);
@ -786,6 +791,8 @@ int main(int argc, char** argv)
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());
nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());
p_pre->lidar_type = lidar_type;
cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
path.header.stamp = ros::Time::now();
@ -815,7 +822,7 @@ int main(int argc, char** argv)
p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov));
p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov));
p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov));
p_imu->lidar_type = lidar_type;
double epsi[23] = {0.001};
fill(epsi, epsi+23, 0.001);
kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);

29
src/preprocess.cpp
View File

@ -77,6 +77,10 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
case VELO16:
velodyne_handler(msg);
break;
case MARSIM:
sim_handler(msg);
break;
default:
printf("Error LiDAR Type");
@ -451,6 +455,31 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
}
}
void Preprocess::sim_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {
pl_surf.clear();
pl_full.clear();
pcl::PointCloud<pcl::PointXYZI> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.size();
pl_surf.reserve(plsize);
for (int i = 0; i < pl_orig.points.size(); 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 * blind) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
added_pt.x = pl_orig.points[i].x;
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.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.curvature = 0.0;
pl_surf.points.push_back(added_pt);
}
}
void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
{
int plsize = pl.size();

8
src/preprocess.h
View File

@ -1,3 +1,7 @@
#ifndef PREPROCESS_H
#define PREPROCESS_H
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
@ -10,7 +14,7 @@ using namespace std;
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
enum LID_TYPE{AVIA = 1, VELO16, OUST64, MARSIM}; //{1, 2, 3}
enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3};
enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
enum Surround{Prev, Next};
@ -106,6 +110,7 @@ class Preprocess
void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void sim_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
@ -122,3 +127,4 @@ class Preprocess
double smallp_intersect, smallp_ratio;
double vx, vy, vz;
};
#endif