kxch928925819 commented 3 years ago

but pcd file has not issues!

XW-HKU commented 3 years ago

did you update your code?

kxch928925819 commented 3 years ago

yes！Brfore catkin-make，edit launch！

kxch928925819 commented 3 years ago

[ 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

XW-HKU commented 3 years ago

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.

kxch928925819 commented 3 years ago

[ 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

XW-HKU commented 3 years ago

`

<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.

kxch928925819 commented 3 years ago

链接：https://pan.baidu.com/s/1_gllUZ7-BG5eYF-AnzsyAA 提取码：evzr

kxch928925819 commented 3 years ago

小徐：拜托抽空看一下，其他人如果感兴趣，也希望能一起交流。我们的设备是 livox-horizon 以及manifold 2-c ，电池采用sony摄像机的V口电池，支架采用的斯坦尼康的支架，还有一个背心暂时未登场，这样支架直接受力在肩背，可以轻而易举的取下，进行像控点的采集。

XW-HKU commented 3 years ago

好，一会儿有空了我看下

kxch928925819 commented 3 years ago

谢谢！

XW-HKU commented 3 years ago

你发的数据包太大了，我这边下载速度很慢。

你说的scan_publish关掉后没有pcd的问题，我在最新代码里面fix了。现在在关掉点云publish的事后，也会有pcd save。但是如果点云太大，可能pcd save会出问题。我没试过很大的pcd保存，你可以试试。

主要是fastlio更多的是用于定位（给机器人无人机使用），你说的静止坐标提取这个不太好加入现有代码，会和其他模块冲突。

GPS的问题，我们正在添加，但是还需要一段时间。

kxch928925819 commented 3 years ago

”你说的静止坐标提取这个不太好加入现有代码“ 关于这句话，是否可以理解为如果在采集过程中，静置10秒，会对结果造成比较大的影响？

XW-HKU commented 3 years ago

”你说的静止坐标提取这个不太好加入现有代码“ 关于这句话，是否可以理解为如果在采集过程中，静置10秒，会对结果造成比较大的影响？

我个人觉得，这个功能最好是在拿到轨迹数据之后的后处理里来做，放在轨迹生成的地方比较难实现。不过你们自己可以修改代码试试。

kxch928925819 commented 3 years ago

非常感谢！把注意力放在轨迹数据和后处理！太谢谢啦！

XW-HKU commented 3 years ago

非常感谢！把注意力放在轨迹数据和后处理！太谢谢啦！

客气

kxch928925819 commented 3 years ago

/**** 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!

kxch928925819 commented 3 years ago

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 ?

XW-HKU commented 3 years ago

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.

kxch928925819 commented 3 years ago

我总是觉得，咱们的rviz显示，把坐标系统设置的不太舒服，譬如：我想要按照视场角进行采集时，显示的点云界面，很难进行旋转，设置，而如果是反向，就是把imu重力方向倒过来，会发现比较容易实现！不知道说的对不对，麻烦你看看！

XW-HKU commented 3 years ago

我总是觉得，咱们的rviz显示，把坐标系统设置的不太舒服，譬如：我想要按照视场角进行采集时，显示的点云界面，很难进行旋转，设置，而如果是反向，就是把imu重力方向倒过来，会发现比较容易实现！不知道说的对不对，麻烦你看看！

你可以设置一下rviz，Panels -> Views -> Invert Z Axis

kxch928925819 commented 3 years ago

为了能在扫描过程中，直接得到时间.pcd 文件，做了一点小修改，头文件加了

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 dev@livoxtech.com

// 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

include <ros/ros.h>

include <Eigen/Core>

include "IMU_Processing.hpp"

include <nav_msgs/Odometry.h>

include <nav_msgs/Path.h>

include <visualization_msgs/Marker.h>

include <pcl_conversions/pcl_conversions.h>

include <pcl/point_cloud.h>

include <pcl/point_types.h>

include <pcl/filters/voxel_grid.h>

include <pcl/io/pcd_io.h>

include <sensor_msgs/PointCloud2.h>

include <tf/transform_datatypes.h>

include <tf/transform_broadcaster.h>

include <geometry_msgs/Vector3.h>

include <livox_ros_driver/CustomMsg.h>

include "preprocess.h"

include <ikd-Tree/ikd_Tree.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> pointSearchInd_surf; vector cub_needrm; vector Nearest_Points; vector extrinT(3, 0.0); vector extrinR(9, 0.0); deque time_buffer; deque lidar_buffer; deque imu_buffer;

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 downSizeFilterSurf; pcl::VoxelGrid 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 p_pre(new Preprocess()); shared_ptr 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_Ip_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_Ip_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 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_Ip_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_Ip_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: "<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 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 &ekfom_data) { double match_start = omp_get_wtime(); laserCloudOri->clear(); corr_normvect->clear(); total_residual = 0.0;

/** 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(&currentTime));
    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;

}

XW-HKU commented 3 years ago

非常感谢！我之后看下如何合并进来。新疆好地方呀！

stale[bot] commented 1 year ago

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.

hku-mars / FAST_LIO

Horizon #32

include

include

include

include

include

include

include

include

include

include

include

include

include <ros/ros.h>

include <Eigen/Core>

include "IMU_Processing.hpp"

include <nav_msgs/Odometry.h>

include <nav_msgs/Path.h>

include <visualization_msgs/Marker.h>

include <pcl_conversions/pcl_conversions.h>

include <pcl/point_cloud.h>

include <pcl/point_types.h>

include <pcl/filters/voxel_grid.h>

include <pcl/io/pcd_io.h>

include <sensor_msgs/PointCloud2.h>

include <tf/transform_datatypes.h>

include <tf/transform_broadcaster.h>

include <geometry_msgs/Vector3.h>

include <livox_ros_driver/CustomMsg.h>

include "preprocess.h"

include <ikd-Tree/ikd_Tree.h>

include

include

include

include

define INIT_TIME (0.1)

define LASER_POINT_COV (0.001)

define MAXN (720000)

define PUBFRAME_PERIOD (20)