FAST_LIO
FAST_LIO copied to clipboard
hku-mars
Horizon
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_lio)/config/horizon.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="scan_publish_enable" type="bool" value="0" />
<param name="dense_publish_enable" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="1" />
<param name="pcd_save_enable" type="bool" value="1" />
<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>
[ WARN] [1625819309.222937980]: TF_OLD_DATA ignoring data from the past for frame aft_mapped at time 1.6258e+09 according to authority unknown_publisher Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained [ WARN] [1625819309.239976639]: TF_OLD_DATA ignoring data from the past for frame aft_mapped at time 1.6258e+09 according to authority unknown_publisher Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained [ mapping ]: time: IMU + Map + Input Downsample: 0.004825 ave match: 0.006561 ave solve: 0.001344 ave ICP: 0.012392 map incre: 0.000452 ave total: 0.012847 icp: 0.007939 construct H: 0.000437 [ mapping ]: time: IMU + Map + Input Downsample: 0.003644 ave match: 0.006562 ave solve: 0.001344 ave ICP: 0.011889 map incre: 0.000490 ave total: 0.012847 icp: 0.007940 construct H: 0.000437 [ WARN] [1625819309.424248839]: TF_OLD_DATA ignoring data from the past for frame aft_mapped at time 1.6258e+09 according to authority unknown_publisher Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained [ mapping ]: time: IMU + Map + Input Downsample: 0.003654 ave match: 0.006562 ave solve: 0.001344 ave ICP: 0.012661 map incre: 0.000419 ave total: 0.012848 icp: 0.007941 construct H: 0.000437
maybe you can set runtime_pos_log_enable to 0, that one may lead to problem.
If problem still exists, use avia.launch instead, these two has little difference.
[ WARN] [1625819309.424248839]: TF_OLD_DATA ignoring data from the past for frame aft_mapped at time 1.6258e+09 according to authority unknown_publisher Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
`
<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="scan_publish_enable" type="bool" value="1" />
<param name="dense_publish_enable" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="1" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
`
I test this, PCD got.
小徐：拜托抽空看一下，其他人如果感兴趣，也希望能一起交流。 我们的设备是 livox-horizon 以及manifold 2-c ，电池采用sony摄像机的V口电池，支架采用的斯坦尼康的支架，还有一个背心暂时未登场，这样支架直接受力在肩背，可以轻而易举的取下，进行像控点的采集。
你发的数据包太大了，我这边下载速度很慢。
你说的scan_publish关掉后没有pcd的问题，我在最新代码里面fix了。现在在关掉点云publish的事后，也会有pcd save。但是如果点云太大，可能pcd save会出问题。我没试过很大的pcd保存，你可以试试。
主要是fastlio更多的是用于定位（给机器人无人机使用），你说的静止坐标提取这个不太好加入现有代码，会和其他模块冲突。
GPS的问题，我们正在添加，但是还需要一段时间。
”你说的静止坐标提取这个不太好加入现有代码“ 关于这句话，是否可以理解为如果在采集过程中，静置10秒，会对结果造成比较大的影响？
我个人觉得，这个功能最好是在拿到轨迹数据之后的后处理里来做，放在轨迹生成的地方比较难实现。不过你们自己可以修改代码试试。
/**************** save map **************/ / 1. make sure you have enough memories / 2. pcd save will largely influence the real-time performences **/ if (pcl_wait_save->size() > 0 && pcd_save_en) { string file_name = string("scans.pcd"); string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); pcl::PCDWriter pcd_writer; cout << "current scan saved to /PCD/" << file_name<<endl; pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); } what save scans(time or cout).pcd ,please!
dji@dji-MANIFOLD-2-C:~/catkin_ws/src/FAST_LIO/Log$ python3 plot.py
Traceback (most recent call last):
File "plot.py", line 3, in
imu.txt is empty! wo hope you issues (evo_traj euroc utm kitti ) which is ?
dji@dji-MANIFOLD-2-C:~/catkin_ws/src/FAST_LIO/Log$ python3 plot.py Traceback (most recent call last): File "plot.py", line 3, in import numpy as np ModuleNotFoundError: No module named 'numpy' dji@dji-MANIFOLD-2-C:~/catkin_ws/src/FAST_LIO/Log$
imu.txt is empty! wo hope you issues (evo_traj euroc utm kitti ) which is ?
you may consider comment the line 36-51 in the plot.py.
我总是觉得，咱们的rviz显示，把坐标系统设置的不太舒服，譬如：我想要按照视场角进行采集时，显示的点云界面，很难进行旋转，设置，而如果是反向，就是把imu重力方向倒过来，会发现比较容易实现！ 不知道说的对不对，麻烦你看看！
我总是觉得，咱们的rviz显示，把坐标系统设置的不太舒服，譬如：我想要按照视场角进行采集时，显示的点云界面，很难进行旋转，设置，而如果是反向，就是把imu重力方向倒过来，会发现比较容易实现！ 不知道说的对不对，麻烦你看看！
你可以设置一下rviz，Panels -> Views -> Invert Z Axis
为了能在扫描过程中，直接得到 时间.pcd 文件，做了一点小修改，头文件加了
#include<time.h>
#include
在save pcd中做了一点小修改，如果有人和我一样，多次扫描，想直接可以得到pcd文件，可以参考一下。程序是我从csdn中截取修改的，编译完成，没有任何问题！ 分享出来，也算是回报小徐不厌其烦解答我问题回报吧！ 只是，回报太小，希望小徐来新疆，请你吃肉，喝酒！ lasermapping.cpp
// This is an advanced implementation of the algorithm described in the // following paper: // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
// Modifier: Livox [email protected]
// Copyright 2013, Ji Zhang, Carnegie Mellon University
// Further contributions copyright (c) 2016, Southwest Research Institute
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from this
// software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include <omp.h>
#include
#define INIT_TIME (0.1) #define LASER_POINT_COV (0.001) #define MAXN (720000) #define PUBFRAME_PERIOD (20)
/*** Time Log Variables / double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0; double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN]; double match_time = 0, solve_time = 0, solve_const_H_time = 0; int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0; bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false; /***********************/
float res_last[100000] = {0.0}; float DET_RANGE = 300.0f; const float MOV_THRESHOLD = 1.5f;
mutex mtx_buffer; condition_variable sig_buffer;
string root_dir = ROOT_DIR; string map_file_path, lid_topic, imu_topic;
double res_mean_last = 0.05, total_residual = 0.0; double last_timestamp_lidar = 0, last_timestamp_imu = -1.0; double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001; double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.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; bool point_selected_surf[100000] = {0}; bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited; bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
vector<vector
PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); PointCloudXYZI::Ptr _featsArray;
pcl::VoxelGrid<PointType> downSizeFilterSurf; pcl::VoxelGrid<PointType> downSizeFilterMap;
KD_TREE ikdtree;
V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); V3D euler_cur; V3D position_last(Zero3d); V3D Lidar_T_wrt_IMU(Zero3d); M3D Lidar_R_wrt_IMU(Eye3d);
/*** EKF inputs and output ***/ MeasureGroup Measures; esekfom::esekf<state_ikfom, 12, input_ikfom> kf; state_ikfom state_point; vect3 pos_lid;
nav_msgs::Path path; nav_msgs::Odometry odomAftMapped; geometry_msgs::Quaternion geoQuat; geometry_msgs::PoseStamped msg_body_pose;
shared_ptr<Preprocess> p_pre(new Preprocess()); shared_ptr<ImuProcess> p_imu(new ImuProcess());
void SigHandle(int sig) { flg_exit = true; ROS_WARN("catch sig %d", sig); sig_buffer.notify_all(); }
inline void dump_lio_state_to_log(FILE *fp)
{
V3D rot_ang(Log(state_point.rot.toRotationMatrix()));
fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time);
fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle
fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos
fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega
fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel
fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc
fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2)); // Bias_g
fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2)); // Bias_a
fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a
fprintf(fp, "\r\n");
fflush(fp);
}
void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s) { V3D p_body(pi->x, pi->y, pi->z); V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos);
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}
void pointBodyToWorld(PointType const * const pi, PointType * const po) { V3D p_body(pi->x, pi->y, pi->z); V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos);
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}
template<typename T> void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po) { V3D p_body(pi[0], pi[1], pi[2]); V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos);
po[0] = p_global(0);
po[1] = p_global(1);
po[2] = p_global(2);
}
void RGBpointBodyToWorld(PointType const * const pi, PointType * const po) { V3D p_body(pi->x, pi->y, pi->z); V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos);
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}
void RGBpointBodyLidarToIMU(PointType const * const pi, PointType * const po) { V3D p_body_lidar(pi->x, pi->y, pi->z); V3D p_body_imu(state_point.offset_R_L_I*p_body_lidar + state_point.offset_T_L_I);
po->x = p_body_imu(0);
po->y = p_body_imu(1);
po->z = p_body_imu(2);
po->intensity = pi->intensity;
}
void points_cache_collect() { PointVector points_history; ikdtree.acquire_removed_points(points_history); for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]); }
BoxPointType LocalMap_Points;
bool Localmap_Initialized = false;
void lasermap_fov_segment()
{
cub_needrm.clear();
kdtree_delete_counter = 0;
kdtree_delete_time = 0.0;
pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);
V3D pos_LiD = pos_lid;
if (!Localmap_Initialized){
for (int i = 0; i < 3; i++){
LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;
LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;
}
Localmap_Initialized = true;
return;
}
float dist_to_map_edge[3][2];
bool need_move = false;
for (int i = 0; i < 3; i++){
dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);
dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);
if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true;
}
if (!need_move) return;
BoxPointType New_LocalMap_Points, tmp_boxpoints;
New_LocalMap_Points = LocalMap_Points;
float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD -1)));
for (int i = 0; i < 3; i++){
tmp_boxpoints = LocalMap_Points;
if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE){
New_LocalMap_Points.vertex_max[i] -= mov_dist;
New_LocalMap_Points.vertex_min[i] -= mov_dist;
tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
cub_needrm.push_back(tmp_boxpoints);
} else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE){
New_LocalMap_Points.vertex_max[i] += mov_dist;
New_LocalMap_Points.vertex_min[i] += mov_dist;
tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
cub_needrm.push_back(tmp_boxpoints);
}
}
LocalMap_Points = New_LocalMap_Points;
points_cache_collect();
double delete_begin = omp_get_wtime();
if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);
kdtree_delete_time = omp_get_wtime() - delete_begin;
}
void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) { mtx_buffer.lock(); scan_count ++; double preprocess_start_time = omp_get_wtime(); if (msg->header.stamp.toSec() < last_timestamp_lidar) { ROS_ERROR("lidar loop back, clear buffer"); lidar_buffer.clear(); }
PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
p_pre->process(msg, ptr);
lidar_buffer.push_back(ptr);
time_buffer.push_back(msg->header.stamp.toSec());
last_timestamp_lidar = msg->header.stamp.toSec();
s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
mtx_buffer.unlock();
sig_buffer.notify_all();
}
double timediff_lidar_wrt_imu = 0.0; bool timediff_set_flg = false; void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) { mtx_buffer.lock(); double preprocess_start_time = omp_get_wtime(); scan_count ++; if (msg->header.stamp.toSec() < last_timestamp_lidar) { ROS_ERROR("lidar loop back, clear buffer"); lidar_buffer.clear(); } last_timestamp_lidar = msg->header.stamp.toSec();
if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty() )
{
printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n",last_timestamp_imu, last_timestamp_lidar);
}
if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty())
{
timediff_set_flg = true;
timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu;
printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu);
}
PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
p_pre->process(msg, ptr);
lidar_buffer.push_back(ptr);
time_buffer.push_back(last_timestamp_lidar);
s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
mtx_buffer.unlock();
sig_buffer.notify_all();
}
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) { publish_count ++; // cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl; sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en)
{
msg->header.stamp = \
ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec());
}
double timestamp = msg->header.stamp.toSec();
mtx_buffer.lock();
if (timestamp < last_timestamp_imu)
{
ROS_WARN("imu loop back, clear buffer");
imu_buffer.clear();
}
last_timestamp_imu = timestamp;
imu_buffer.push_back(msg);
mtx_buffer.unlock();
sig_buffer.notify_all();
}
bool sync_packages(MeasureGroup &meas) { if (lidar_buffer.empty() || imu_buffer.empty()) { return false; }
/*** push a lidar scan ***/
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);
lidar_pushed = true;
}
if (last_timestamp_imu < lidar_end_time)
{
return false;
}
/*** push imu data, and pop from imu buffer ***/
double imu_time = imu_buffer.front()->header.stamp.toSec();
meas.imu.clear();
while ((!imu_buffer.empty()) && (imu_time < lidar_end_time))
{
imu_time = imu_buffer.front()->header.stamp.toSec();
if(imu_time > lidar_end_time) break;
meas.imu.push_back(imu_buffer.front());
imu_buffer.pop_front();
}
lidar_buffer.pop_front();
time_buffer.pop_front();
lidar_pushed = false;
return true;
}
int process_increments = 0; void map_incremental() { PointVector PointToAdd; PointVector PointNoNeedDownsample; PointToAdd.reserve(feats_down_size); PointNoNeedDownsample.reserve(feats_down_size); for (int i = 0; i < feats_down_size; i++) { /* transform to world frame / pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); / decide if need add to map */ if (!Nearest_Points[i].empty() && flg_EKF_inited) { const PointVector &points_near = Nearest_Points[i]; bool need_add = true; BoxPointType Box_of_Point; PointType downsample_result, mid_point; mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; float dist = calc_dist(feats_down_world->points[i],mid_point); if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min){ PointNoNeedDownsample.push_back(feats_down_world->points[i]); continue; } for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i ++) { if (points_near.size() < NUM_MATCH_POINTS) break; if (calc_dist(points_near[readd_i], mid_point) < dist) { need_add = false; break; } } if (need_add) PointToAdd.push_back(feats_down_world->points[i]); } else { PointToAdd.push_back(feats_down_world->points[i]); } }
double st_time = omp_get_wtime();
add_point_size = ikdtree.Add_Points(PointToAdd, true);
ikdtree.Add_Points(PointNoNeedDownsample, false);
add_point_size = PointToAdd.size() + PointNoNeedDownsample.size();
kdtree_incremental_time = omp_get_wtime() - st_time;
}
PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
{
if(scan_pub_en)
{
PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
int size = laserCloudFullRes->points.size();
PointCloudXYZI::Ptr laserCloudWorld(
new PointCloudXYZI(size, 1));
for (int i = 0; i < size; i++)
{
RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
&laserCloudWorld->points[i]);
}
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "camera_init";
pubLaserCloudFull.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
}
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
if (pcd_save_en)
{
int size = feats_undistort->points.size();
PointCloudXYZI::Ptr laserCloudWorld( \
new PointCloudXYZI(size, 1));
for (int i = 0; i < size; i++)
{
RGBpointBodyToWorld(&feats_undistort->points[i], \
&laserCloudWorld->points[i]);
}
*pcl_wait_save += *laserCloudWorld;
}
}
void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body) { int size = feats_undistort->points.size(); PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
for (int i = 0; i < size; i++)
{
RGBpointBodyLidarToIMU(&feats_undistort->points[i], \
&laserCloudIMUBody->points[i]);
}
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "body";
pubLaserCloudFull_body.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
}
void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
{
PointCloudXYZI::Ptr laserCloudWorld(
new PointCloudXYZI(effct_feat_num, 1));
for (int i = 0; i < effct_feat_num; i++)
{
RGBpointBodyToWorld(&laserCloudOri->points[i],
&laserCloudWorld->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudFullRes3.header.frame_id = "camera_init";
pubLaserCloudEffect.publish(laserCloudFullRes3);
}
void publish_map(const ros::Publisher & pubLaserCloudMap) { sensor_msgs::PointCloud2 laserCloudMap; pcl::toROSMsg(*featsFromMap, laserCloudMap); laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudMap.header.frame_id = "camera_init"; pubLaserCloudMap.publish(laserCloudMap); }
template<typename T> void set_posestamp(T & out) { out.pose.position.x = state_point.pos(0); out.pose.position.y = state_point.pos(1); out.pose.position.z = state_point.pos(2); out.pose.orientation.x = geoQuat.x; out.pose.orientation.y = geoQuat.y; out.pose.orientation.z = geoQuat.z; out.pose.orientation.w = geoQuat.w;
}
void publish_odometry(const ros::Publisher & pubOdomAftMapped) { odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.child_frame_id = "body"; odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time); set_posestamp(odomAftMapped.pose); pubOdomAftMapped.publish(odomAftMapped); auto P = kf.get_P(); for (int i = 0; i < 6; i ++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i6 + 0] = P(k, 3); odomAftMapped.pose.covariance[i6 + 1] = P(k, 4); odomAftMapped.pose.covariance[i6 + 2] = P(k, 5); odomAftMapped.pose.covariance[i6 + 3] = P(k, 0); odomAftMapped.pose.covariance[i6 + 4] = P(k, 1); odomAftMapped.pose.covariance[i6 + 5] = P(k, 2); }
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \
odomAftMapped.pose.pose.position.y, \
odomAftMapped.pose.pose.position.z));
q.setW(odomAftMapped.pose.pose.orientation.w);
q.setX(odomAftMapped.pose.pose.orientation.x);
q.setY(odomAftMapped.pose.pose.orientation.y);
q.setZ(odomAftMapped.pose.pose.orientation.z);
transform.setRotation( q );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "body" ) );
}
void publish_path(const ros::Publisher pubPath) { set_posestamp(msg_body_pose); msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); msg_body_pose.header.frame_id = "camera_init";
/*** if path is too large, the rvis will crash ***/
static int jjj = 0;
jjj++;
if (jjj % 10 == 0)
{
path.poses.push_back(msg_body_pose);
pubPath.publish(path);
}
}
void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct
/** closest surface search and residual computation **/
#ifdef MP_EN
omp_set_num_threads(MP_PROC_NUM);
#pragma omp parallel for
#endif
for (int i = 0; i < feats_down_size; i++)
{
PointType &point_body = feats_down_body->points[i];
PointType &point_world = feats_down_world->points[i];
/* transform to world frame */
V3D p_body(point_body.x, point_body.y, point_body.z);
V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos);
point_world.x = p_global(0);
point_world.y = p_global(1);
point_world.z = p_global(2);
point_world.intensity = point_body.intensity;
vector<float> pointSearchSqDis(NUM_MATCH_POINTS);
auto &points_near = Nearest_Points[i];
if (ekfom_data.converge)
{
/** Find the closest surfaces in the map **/
ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis);
point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false : true;
}
if (!point_selected_surf[i]) continue;
VF(4) pabcd;
point_selected_surf[i] = false;
if (esti_plane(pabcd, points_near, 0.1f))
{
float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3);
float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());
if (s > 0.9)
{
point_selected_surf[i] = true;
normvec->points[i].x = pabcd(0);
normvec->points[i].y = pabcd(1);
normvec->points[i].z = pabcd(2);
normvec->points[i].intensity = pd2;
res_last[i] = abs(pd2);
}
}
}
effct_feat_num = 0;
for (int i = 0; i < feats_down_size; i++)
{
if (point_selected_surf[i])
{
laserCloudOri->points[effct_feat_num] = feats_down_body->points[i];
corr_normvect->points[effct_feat_num] = normvec->points[i];
total_residual += res_last[i];
effct_feat_num ++;
}
}
res_mean_last = total_residual / effct_feat_num;
match_time += omp_get_wtime() - match_start;
double solve_start_ = omp_get_wtime();
/*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/
ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //23
ekfom_data.h.resize(effct_feat_num);
for (int i = 0; i < effct_feat_num; i++)
{
const PointType &laser_p = laserCloudOri->points[i];
V3D point_this_be(laser_p.x, laser_p.y, laser_p.z);
M3D point_be_crossmat;
point_be_crossmat << SKEW_SYM_MATRX(point_this_be);
V3D point_this = s.offset_R_L_I * point_this_be + s.offset_T_L_I;
M3D point_crossmat;
point_crossmat<<SKEW_SYM_MATRX(point_this);
/*** get the normal vector of closest surface/corner ***/
const PointType &norm_p = corr_normvect->points[i];
V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);
/*** calculate the Measuremnt Jacobian matrix H ***/
V3D C(s.rot.conjugate() *norm_vec);
V3D A(point_crossmat * C);
V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec);
ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
/*** Measuremnt: distance to the closest surface/corner ***/
ekfom_data.h(i) = -norm_p.intensity;
}
solve_time += omp_get_wtime() - solve_start_;
}
int main(int argc, char** argv) { ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh;
nh.param<bool>("publish/scan_publish_en",scan_pub_en,1);
nh.param<bool>("publish/dense_publish_en",dense_pub_en,1);
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en,1);
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);
nh.param<string>("map_file_path",map_file_path,"");
nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");
nh.param<string>("common/imu_topic", imu_topic,"/livox/imu");
nh.param<bool>("common/time_sync_en", time_sync_en, false);
nh.param<double>("filter_size_corner",filter_size_corner_min,0.5);
nh.param<double>("filter_size_surf",filter_size_surf_min,0.5);
nh.param<double>("filter_size_map",filter_size_map_min,0.5);
nh.param<double>("cube_side_length",cube_len,200);
nh.param<float>("mapping/det_range",DET_RANGE,300.f);
nh.param<double>("mapping/fov_degree",fov_deg,180);
nh.param<double>("mapping/gyr_cov",gyr_cov,0.1);
nh.param<double>("mapping/acc_cov",acc_cov,0.1);
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/scan_line", p_pre->N_SCANS, 16);
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, 0);
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
nh.param<bool>("pcd_save_enable", pcd_save_en, 0);
nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());
nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());
cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
path.header.stamp = ros::Time::now();
path.header.frame_id ="camera_init";
/*** variables definition ***/
int effect_feat_num = 0, frame_num = 0;
double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0;
bool flg_EKF_converged, EKF_stop_flg = 0;
FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0);
HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0);
_featsArray.reset(new PointCloudXYZI());
memset(point_selected_surf, true, sizeof(point_selected_surf));
memset(res_last, -1000.0f, sizeof(res_last));
downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
memset(point_selected_surf, true, sizeof(point_selected_surf));
memset(res_last, -1000.0f, sizeof(res_last));
Lidar_T_wrt_IMU<<VEC_FROM_ARRAY(extrinT);
Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR);
p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU);
p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov));
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));
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);
/*** debug record ***/
FILE *fp;
string pos_log_dir = root_dir + "/Log/pos_log.txt";
fp = fopen(pos_log_dir.c_str(),"w");
ofstream fout_pre, fout_out, fout_dbg;
fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out);
fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out);
fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out);
if (fout_pre && fout_out)
cout << "~~~~"<<ROOT_DIR<<" file opened" << endl;
else
cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl;
/*** ROS subscribe initialization ***/
ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 100000);
ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered_body", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_effected", 100000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/Odometry", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000);
//------------------------------------------------------------------------------------------------------ signal(SIGINT, SigHandle); ros::Rate rate(5000); bool status = ros::ok(); while (status) { if (flg_exit) break; ros::spinOnce(); if(sync_packages(Measures)) { if (flg_reset) { ROS_WARN("reset when rosbag play back"); p_imu->Reset(); flg_reset = false; Measures.imu.clear(); continue; }
double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time;
match_time = 0;
kdtree_search_time = 0.0;
solve_time = 0;
solve_const_H_time = 0;
svd_time = 0;
t0 = omp_get_wtime();
p_imu->Process(Measures, kf, feats_undistort);
state_point = kf.get_x();
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
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;
continue;
}
flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \
false : true;
/*** Segment the map in lidar FOV ***/
lasermap_fov_segment();
/*** downsample the feature points in a scan ***/
downSizeFilterSurf.setInputCloud(feats_undistort);
downSizeFilterSurf.filter(*feats_down_body);
t1 = omp_get_wtime();
feats_down_size = feats_down_body->points.size();
/*** initialize the map kdtree ***/
if(ikdtree.Root_Node == nullptr)
{
if(feats_down_size > 5)
{
ikdtree.set_downsample_param(filter_size_map_min);
feats_down_world->resize(feats_down_size);
for(int i = 0; i < feats_down_size; i++)
{
pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
}
ikdtree.Build(feats_down_world->points);
}
continue;
}
int featsFromMapNum = ikdtree.validnum();
kdtree_size_st = ikdtree.size();
// 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 ***/
normvec->resize(feats_down_size);
feats_down_world->resize(feats_down_size);
V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);
fout_pre<<setw(20)<<Measures.lidar_beg_time - first_lidar_time<<" "<<euler_cur.transpose()<<" "<< state_point.pos.transpose()<<" "<<ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<< " " << state_point.vel.transpose() \
<<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<< endl;
if(0) // If you need to see map point, change to "if(1)"
{
PointVector ().swap(ikdtree.PCL_Storage);
ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
featsFromMap->clear();
featsFromMap->points = ikdtree.PCL_Storage;
}
pointSearchInd_surf.resize(feats_down_size);
Nearest_Points.resize(feats_down_size);
int rematch_num = 0;
bool nearest_search_en = true; //
t2 = omp_get_wtime();
/*** iterated state estimation ***/
double t_update_start = omp_get_wtime();
double solve_H_time = 0;
kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);
state_point = kf.get_x();
euler_cur = SO3ToEuler(state_point.rot);
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
geoQuat.x = state_point.rot.coeffs()[0];
geoQuat.y = state_point.rot.coeffs()[1];
geoQuat.z = state_point.rot.coeffs()[2];
geoQuat.w = state_point.rot.coeffs()[3];
double t_update_end = omp_get_wtime();
/******* Publish odometry *******/
publish_odometry(pubOdomAftMapped);
/*** add the feature points to map kdtree ***/
t3 = omp_get_wtime();
map_incremental();
t5 = omp_get_wtime();
/******* Publish points *******/
publish_path(pubPath);
if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull);
if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body);
// publish_effect_world(pubLaserCloudEffect);
// publish_map(pubLaserCloudMap);
/*** Debug variables ***/
if (runtime_pos_log)
{
frame_num ++;
kdtree_size_end = ikdtree.size();
aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + (t_update_end - t_update_start) / frame_num;
aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num;
aver_time_incre = aver_time_incre * (frame_num - 1)/frame_num + (kdtree_incremental_time)/frame_num;
aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + (solve_time + solve_H_time)/frame_num;
aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1)/frame_num + solve_time / frame_num;
T1[time_log_counter] = Measures.lidar_beg_time;
s_plot[time_log_counter] = t5 - t0;
s_plot2[time_log_counter] = feats_undistort->points.size();
s_plot3[time_log_counter] = kdtree_incremental_time;
s_plot4[time_log_counter] = kdtree_search_time;
s_plot5[time_log_counter] = kdtree_delete_counter;
s_plot6[time_log_counter] = kdtree_delete_time;
s_plot7[time_log_counter] = kdtree_size_st;
s_plot8[time_log_counter] = kdtree_size_end;
s_plot9[time_log_counter] = aver_time_consu;
s_plot10[time_log_counter] = add_point_size;
time_log_counter ++;
printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu,aver_time_icp, aver_time_const_H_time);
ext_euler = SO3ToEuler(state_point.offset_R_L_I);
fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<<" "<< state_point.vel.transpose() \
<<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<<" "<<feats_undistort->points.size()<<endl;
dump_lio_state_to_log(fp);
}
}
status = ros::ok();
rate.sleep();
}
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
if (pcl_wait_save->size() > 0 && pcd_save_en)
{
using namespace std;
time_t currentTime=time(NULL);
char chCurrentTime[256];
strftime(chCurrentTime,sizeof(chCurrentTime),"%Y%m%d %H%M%S",localtime(¤tTime));
string stCurrentTime=chCurrentTime;
string file_name=stCurrentTime+".pcd";
string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
pcl::PCDWriter pcd_writer;
cout << "current scan saved to /PCD/" << file_name<<endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
}
fout_out.close();
fout_pre.close();
if (runtime_pos_log)
{
vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
FILE *fp2;
string log_dir = root_dir + "/Log/fast_lio_time_log.csv";
fp2 = fopen(log_dir.c_str(),"w");
fprintf(fp2,"time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n");
for (int i = 0;i<time_log_counter; i++){
fprintf(fp2,"%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n",T1[i],s_plot[i],int(s_plot2[i]),s_plot3[i],s_plot4[i],int(s_plot5[i]),s_plot6[i],int(s_plot7[i]),int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]);
t.push_back(T1[i]);
s_vec.push_back(s_plot9[i]);
s_vec2.push_back(s_plot3[i] + s_plot6[i]);
s_vec3.push_back(s_plot4[i]);
s_vec5.push_back(s_plot[i]);
}
fclose(fp2);
}
return 0;
}
This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.
![stale[bot] avatar](https://avatars.githubusercontent.com/in/1724?v=4 "Published by a pub.dev verified publisher"){kind=small}