|
|
@@ -23,6 +23,7 @@
|
|
|
#include <tf/transform_datatypes.h>
|
|
|
#include <tf/transform_broadcaster.h>
|
|
|
#include <geometry_msgs/Vector3.h>
|
|
|
+#include <geometry_msgs/PoseWithCovarianceStamped.h> //PoseWithCovarianceStamped
|
|
|
#include <livox_ros_driver/CustomMsg.h>
|
|
|
#include "preprocess.h"
|
|
|
#include <ikd-Tree/ikd_Tree.h>
|
|
|
@@ -43,6 +44,7 @@
|
|
|
#include <pcl/common/common.h>
|
|
|
#include <pcl/common/transforms.h>
|
|
|
#include <pcl/registration/icp.h>
|
|
|
+#include <pcl/registration/ndt.h>
|
|
|
#include <pcl/io/pcd_io.h>
|
|
|
#include <pcl/filters/filter.h>
|
|
|
#include <pcl/filters/crop_box.h>
|
|
|
@@ -63,6 +65,7 @@
|
|
|
#include <gtsam/inference/Symbol.h>
|
|
|
#include <gtsam/nonlinear/ISAM2.h>
|
|
|
|
|
|
+#include "keyFramesManager.h"
|
|
|
/*
|
|
|
// gnss
|
|
|
#include "GNSS_Processing.hpp"
|
|
|
@@ -147,9 +150,11 @@ V3D Lidar_T_wrt_IMU(Zero3d); // T lidar to imu (imu = r * lidar + t)
|
|
|
M3D Lidar_R_wrt_IMU(Eye3d); // R lidar to imu (imu = r * lidar + t)
|
|
|
|
|
|
/*** EKF inputs and output ***/
|
|
|
+std::mutex ikf_lock_;
|
|
|
MeasureGroup Measures;
|
|
|
esekfom::esekf<state_ikfom, 12, input_ikfom> ikfom_; // 状态，噪声维度，输入
|
|
|
state_ikfom state_point;
|
|
|
+state_ikfom last_state_point;
|
|
|
vect3 pos_lid; // world系下lidar坐标
|
|
|
|
|
|
nav_msgs::Path path;
|
|
|
@@ -254,12 +259,10 @@ float globalMapVisualizationSearchRadius;
|
|
|
float globalMapVisualizationPoseDensity;
|
|
|
float globalMapVisualizationLeafSize;
|
|
|
|
|
|
-// saveMap
|
|
|
-ros::ServiceServer srvSaveMap;
|
|
|
-ros::ServiceServer srvSavePose;
|
|
|
+
|
|
|
bool savePCD; // 是否保存地图
|
|
|
string savePCDDirectory; // 保存路径
|
|
|
-
|
|
|
+bool ndt_loop=false;
|
|
|
/*
|
|
|
* scancontext 回环检测
|
|
|
*/
|
|
|
@@ -402,29 +405,6 @@ float pointDistance(PointType p1, PointType p2)
|
|
|
return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
|
|
|
}
|
|
|
|
|
|
-/**
|
|
|
- * 初始化
|
|
|
- */
|
|
|
-void allocateMemory()
|
|
|
-{
|
|
|
- cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
|
|
|
- cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
|
|
|
- copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
|
|
|
- copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
|
|
|
-
|
|
|
- kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
|
|
|
- kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
|
|
|
-
|
|
|
- laserCloudOri.reset(new pcl::PointCloud<PointType>());
|
|
|
-
|
|
|
- kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
|
|
|
- kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
|
|
|
-
|
|
|
- for (int i = 0; i < 6; ++i)
|
|
|
- {
|
|
|
- transformTobeMapped[i] = 0;
|
|
|
- }
|
|
|
-}
|
|
|
|
|
|
// eulerAngle 2 Quaterniond
|
|
|
Eigen::Quaterniond EulerToQuat(float roll_, float pitch_, float yaw_)
|
|
|
@@ -673,7 +653,7 @@ void performLoopClosure() {
|
|
|
|
|
|
loopKeyPre = SCclosestHistoryFrameID;
|
|
|
loopKeyCur = cloudKeyPoses3D->size() - 1; // because cpp starts 0 and ends n-1
|
|
|
- cout << "Loop detected! - between " << loopKeyPre << " and " << loopKeyCur << "" << endl;
|
|
|
+ //cout << "Loop detected! - between " << loopKeyPre << " and " << loopKeyCur << "" << endl;
|
|
|
|
|
|
}else{
|
|
|
return;
|
|
|
@@ -696,7 +676,18 @@ void performLoopClosure() {
|
|
|
publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
|
|
|
}
|
|
|
|
|
|
+ double icp_t1 = omp_get_wtime();
|
|
|
+ float noiseScore;
|
|
|
+ Eigen::Affine3f correctionLidarFrame;
|
|
|
+ float x, y, z, roll, pitch, yaw;
|
|
|
+ /*PointTypePose current = copy_cloudKeyPoses6D->points[loopKeyCur];
|
|
|
+ PointTypePose target = copy_cloudKeyPoses6D->points[loopKeyPre];
|
|
|
+ gtsam::Pose3 poseSrc(gtsam::Rot3::RzRyRx(double(current.roll), double(current.pitch), double(current.yaw)),
|
|
|
+ gtsam::Point3(double(current.x), double(current.y), double(current.z)));
|
|
|
+ gtsam::Pose3 poseTar(gtsam::Rot3::RzRyRx(double(target.roll), double(target.pitch), double(target.yaw)),
|
|
|
+ gtsam::Point3(double(target.x), double(target.y), double(target.z)));*/
|
|
|
// ICP Settings
|
|
|
+if(ndt_loop==false) {
|
|
|
pcl::IterativeClosestPoint<PointType, PointType> icp;
|
|
|
icp.setMaxCorrespondenceDistance(150); // giseop , use a value can cover 2*historyKeyframeSearchNum range in meter
|
|
|
icp.setMaximumIterations(100);
|
|
|
@@ -709,35 +700,65 @@ void performLoopClosure() {
|
|
|
icp.setInputTarget(prevKeyframeCloud);
|
|
|
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
|
|
|
icp.align(*unused_result);
|
|
|
-
|
|
|
- // 未收敛，或者匹配不够好
|
|
|
- if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
|
|
|
+ noiseScore =icp.getFitnessScore(); // loop_clousre noise from ndt
|
|
|
+ correctionLidarFrame = icp.getFinalTransformation();
|
|
|
+ double icp_t2 = omp_get_wtime();
|
|
|
+ pcl::getTranslationAndEulerAngles(correctionLidarFrame, x, y, z, roll, pitch, yaw);
|
|
|
+ if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) {
|
|
|
+ printf("icp failed,score:%f from %d to %d ,(%f %f %f) (%f %f %f) time:%f\n",
|
|
|
+ icp.getFitnessScore(),loopKeyCur, loopKeyPre,x, y, z, roll, pitch, yaw,icp_t2 - icp_t1);
|
|
|
return;
|
|
|
-
|
|
|
- std::cout << "icp success " << std::endl;
|
|
|
+ }
|
|
|
+ printf("icp success,score:%f from %d to %d ,(%f %f %f) (%f %f %f) time:%f\n",
|
|
|
+ icp.getFitnessScore(),loopKeyCur, loopKeyPre,x, y, z, roll, pitch, yaw,icp_t2 - icp_t1);
|
|
|
+
|
|
|
+}else {
|
|
|
+ pcl::NormalDistributionsTransform<PointType, PointType> ndt;
|
|
|
+ ndt.setTransformationEpsilon(0.001);
|
|
|
+ // Setting maximum step size for More-Thuente line search.
|
|
|
+ ndt.setStepSize(0.1);
|
|
|
+ //Setting Resolution of NDT grid structure (VoxelGridCovariance).
|
|
|
+ ndt.setResolution(0.5);
|
|
|
+ // Setting max number of registration iterations.
|
|
|
+ ndt.setMaximumIterations(50);
|
|
|
+ // Setting point cloud to be aligned.
|
|
|
+ ndt.setInputSource(cureKeyframeCloud);
|
|
|
+ // Setting point cloud to be aligned to.
|
|
|
+ ndt.setInputTarget(prevKeyframeCloud);
|
|
|
+ pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
|
|
|
+ ndt.align(*unused_result, Eigen::Matrix4f::Identity());
|
|
|
+
|
|
|
+ double icp_t2 = omp_get_wtime();
|
|
|
+ noiseScore =ndt.getFitnessScore(); // loop_clousre noise from ndt
|
|
|
+ correctionLidarFrame = ndt.getFinalTransformation();
|
|
|
+ pcl::getTranslationAndEulerAngles(correctionLidarFrame, x, y, z, roll, pitch, yaw);
|
|
|
+ if (ndt.hasConverged() == false || ndt.getFitnessScore() > historyKeyframeFitnessScore) {
|
|
|
+ printf("ndt failed,score:%f from %d to %d ,(%f %f %f) (%f %f %f) time:%f\n",
|
|
|
+ ndt.getFitnessScore(),loopKeyCur, loopKeyPre,x, y, z, roll, pitch, yaw,icp_t2 - icp_t1);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ printf("ndt success,score:%f from %d to %d ,(%f %f %f) (%f %f %f) time:%f\n",
|
|
|
+ ndt.getFitnessScore(),loopKeyCur, loopKeyPre,x, y, z, roll, pitch, yaw,icp_t2 - icp_t1);
|
|
|
+}
|
|
|
|
|
|
// 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云
|
|
|
if (pubIcpKeyFrames.getNumSubscribers() != 0) {
|
|
|
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
|
|
|
- pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
|
|
|
+ pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, correctionLidarFrame);
|
|
|
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
|
|
|
}
|
|
|
|
|
|
// 闭环优化得到的当前关键帧与闭环关键帧之间的位姿变换
|
|
|
- float x, y, z, roll, pitch, yaw;
|
|
|
- Eigen::Affine3f correctionLidarFrame;
|
|
|
- correctionLidarFrame = icp.getFinalTransformation();
|
|
|
-
|
|
|
// 闭环优化前当前帧位姿
|
|
|
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
|
|
|
// 闭环优化后当前帧位姿
|
|
|
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
|
|
|
- pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw); // 获取上一帧 相对 当前帧的 位姿
|
|
|
+ pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw); // 获取上一帧匹配后的位姿
|
|
|
gtsam::Pose3 poseFrom = gtsam::Pose3(gtsam::Rot3::RzRyRx(roll, pitch, yaw), gtsam::Point3(x, y, z));
|
|
|
// 闭环匹配帧的位姿
|
|
|
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
|
|
|
gtsam::Vector Vector6(6);
|
|
|
- float noiseScore = icp.getFitnessScore(); // loop_clousre noise from icp
|
|
|
+
|
|
|
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
|
|
|
gtsam::noiseModel::Diagonal::shared_ptr constraintNoise = gtsam::noiseModel::Diagonal::Variances(Vector6);
|
|
|
std::cout << "loopNoiseQueue = " << noiseScore << std::endl;
|
|
|
@@ -762,6 +783,7 @@ void saveKeyFramesAndFactor()
|
|
|
addOdomFactor();
|
|
|
scManager.makeAndSaveScancontextAndKeys(*feats_undistort);
|
|
|
performLoopClosure(); //闭环检测
|
|
|
+ visualizeLoopClosure();//展示闭环边
|
|
|
// 闭环因子 (rs-loop-detect) 基于欧氏距离的检测
|
|
|
addLoopFactor();
|
|
|
// 执行优化
|
|
|
@@ -1906,7 +1928,34 @@ void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_
|
|
|
solve_time += omp_get_wtime() - solve_start_;
|
|
|
}
|
|
|
|
|
|
+void init_pose_cbk(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg){
|
|
|
+ double x=msg->pose.pose.position.x;
|
|
|
+ double y=msg->pose.pose.position.y;
|
|
|
+ printf(" received init pose:%f %f \n",x,y);
|
|
|
+ state_ikfom state_inited;
|
|
|
+ Eigen::Vector3d pos(x,y, 0);
|
|
|
+ Eigen::Quaterniond q(msg->pose.pose.orientation.w,msg->pose.pose.orientation.x,
|
|
|
+ msg->pose.pose.orientation.y,msg->pose.pose.orientation.z);
|
|
|
+ // 更新状态量
|
|
|
+ state_inited.pos = pos;
|
|
|
+ state_inited.rot = q;
|
|
|
+
|
|
|
+ double epsi[23] = {0.005};
|
|
|
+ fill(epsi, epsi + 23, 0.005);
|
|
|
+ ///初始化，其中h_share_model定义了·平面搜索和残差计算
|
|
|
+ esekfom::esekf<state_ikfom, 12, input_ikfom> new_ikfom; // 状态，噪声维度，输入
|
|
|
+ new_ikfom.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);
|
|
|
+ new_ikfom.change_x(state_inited);
|
|
|
+
|
|
|
+ ikf_lock_.lock();
|
|
|
+ ikfom_=new_ikfom;
|
|
|
+ state_point=state_inited;
|
|
|
+ last_state_point=state_inited;
|
|
|
+ p_imu->Reset();
|
|
|
+ flg_first_scan=true;
|
|
|
+ ikf_lock_.unlock();
|
|
|
|
|
|
+}
|
|
|
|
|
|
int main(int argc, char **argv) {
|
|
|
// allocateMemory();
|
|
|
@@ -1921,7 +1970,7 @@ int main(int argc, char **argv) {
|
|
|
nh.param<bool>("publish/scan_publish_en", scan_pub_en, true);
|
|
|
nh.param<bool>("publish/dense_publish_en", dense_pub_en, true);
|
|
|
nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, true);
|
|
|
- nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 4);
|
|
|
+ nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 10);
|
|
|
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");
|
|
|
@@ -1940,7 +1989,7 @@ int main(int argc, char **argv) {
|
|
|
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>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);
|
|
|
- nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
|
|
|
+ nh.param<int>("point_filter_num", p_pre->point_filter_num, 1);
|
|
|
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false);
|
|
|
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
|
|
|
nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true);
|
|
|
@@ -1983,7 +2032,12 @@ int main(int argc, char **argv) {
|
|
|
|
|
|
// visual ikdtree map
|
|
|
nh.param<bool>("locate_mode", locate_mode, false);
|
|
|
+ vector<double> init_pos(3, 0.0);
|
|
|
+ vector<double> init_rpy(3, 0.0);
|
|
|
+ nh.param<vector<double>>("locate/init_pos", init_pos, vector<double>());
|
|
|
+ nh.param<vector<double>>("locate/init_rpy", init_rpy, vector<double>());
|
|
|
nh.param<bool>("visulize_IkdtreeMap", visulize_IkdtreeMap, false);
|
|
|
+ nh.param<bool>("ndtLoopMatch", ndt_loop, false);
|
|
|
|
|
|
// reconstruct ikdtree
|
|
|
nh.param<bool>("recontructKdTree", recontructKdTree, false);
|
|
|
@@ -2091,270 +2145,311 @@ int main(int argc, char **argv) {
|
|
|
"/fast_lio_sam/mapping/loop_closure_constraints", 1);
|
|
|
|
|
|
|
|
|
- // saveMap 发布地图保存服务
|
|
|
- srvSaveMap = nh.advertiseService("/save_map", &saveMapService);
|
|
|
-
|
|
|
- // savePose 发布轨迹保存服务
|
|
|
- srvSavePose = nh.advertiseService("/save_pose", &savePoseService);
|
|
|
-
|
|
|
-
|
|
|
+ ros::Subscriber sub_init_pose;
|
|
|
// 回环检测线程
|
|
|
- std::thread* loopthread= nullptr;
|
|
|
- if (locate_mode == false)
|
|
|
- loopthread=new std::thread(&loopClosureThread);
|
|
|
- else{
|
|
|
+ std::thread *loopthread = nullptr;
|
|
|
+ if (locate_mode == false) {
|
|
|
+ ros::ServiceServer srvSaveMap;
|
|
|
+ ros::ServiceServer srvSavePose;
|
|
|
+ // saveMap 发布地图保存服务
|
|
|
+ srvSaveMap = nh.advertiseService("/save_map", &saveMapService);
|
|
|
+ // savePose 发布轨迹保存服务
|
|
|
+ srvSavePose = nh.advertiseService("/save_pose", &savePoseService);
|
|
|
+ loopthread = new std::thread(&loopClosureThread);
|
|
|
+ } else {
|
|
|
PointCloudXYZI map_cloud;
|
|
|
- std::string map_file=std::getenv("HOME")+savePCDDirectory+"/filterGlobalMap.pcd";
|
|
|
- if (pcl::io::loadPCDFile<PointType> (map_file, map_cloud) == -1) //* 读入PCD格式的文件，如果文件不存在，返回-1
|
|
|
+ std::string map_file = std::getenv("HOME") + savePCDDirectory + "/filterGlobalMap.pcd";
|
|
|
+ if (pcl::io::loadPCDFile<PointType>(map_file, map_cloud) == -1) //* 读入PCD格式的文件，如果文件不存在，返回-1
|
|
|
{
|
|
|
PCL_ERROR ("Couldn't read file test_pcd.pcd \n"); //文件不存在时，返回错误，终止程序。
|
|
|
return (-1);
|
|
|
}
|
|
|
- printf(" Read map point num:%d\n",map_cloud.size());
|
|
|
+ printf(" Read map point num:%d\n", map_cloud.size());
|
|
|
ikdtree.Build(map_cloud.points);
|
|
|
- usleep(1000*1000);
|
|
|
- }
|
|
|
|
|
|
- //------------------------------------------------------------------------------------------------------
|
|
|
- if (visulize_IkdtreeMap) // If you need to see map point, change to "if(1)"
|
|
|
- {
|
|
|
+ sub_init_pose = nh.subscribe("/initialpose", 1000, init_pose_cbk);
|
|
|
+
|
|
|
+ state_ikfom state_inited;
|
|
|
+ Eigen::Vector3d pos(init_pos[0], init_pos[1], init_pos[2]);
|
|
|
+ Eigen::Quaterniond q = EulerToQuat(init_rpy[0], init_rpy[1], init_rpy[2]);
|
|
|
+ // 更新状态量
|
|
|
+ state_inited.pos = pos;
|
|
|
+ state_inited.rot = q;
|
|
|
+ ikfom_.change_x(state_inited);
|
|
|
+ last_state_point = state_inited;
|
|
|
+
|
|
|
+ usleep(1000 * 1000);
|
|
|
PointVector().swap(ikdtree.PCL_Storage);
|
|
|
ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
|
|
|
featsFromMap->clear();
|
|
|
featsFromMap->points = ikdtree.PCL_Storage;
|
|
|
publish_map(pubLaserCloudMap);
|
|
|
}
|
|
|
- signal(SIGINT, SigHandle);
|
|
|
- ros::Rate rate(5000);
|
|
|
- bool status = ros::ok();
|
|
|
- while (status)
|
|
|
- {
|
|
|
- if (flg_exit)
|
|
|
- break;
|
|
|
- ros::spinOnce();
|
|
|
|
|
|
- /// 在Measure内，储存当前lidar数据及lidar扫描时间内对应的imu数据序列
|
|
|
- if (sync_packages(Measures))
|
|
|
- {
|
|
|
- //第一帧lidar数据
|
|
|
- if (flg_first_scan)
|
|
|
- {
|
|
|
- first_lidar_time = Measures.lidar_beg_time; //记录第一帧绝对时间
|
|
|
- p_imu->first_lidar_time = first_lidar_time; //记录第一帧绝对时间
|
|
|
- flg_first_scan = false;
|
|
|
- continue;
|
|
|
- }
|
|
|
+ signal(SIGINT, SigHandle);
|
|
|
+ ros::Rate rate(5000);
|
|
|
+ bool status = ros::ok();
|
|
|
+ while (status) {
|
|
|
+ if (flg_exit)
|
|
|
+ break;
|
|
|
+ ros::spinOnce();
|
|
|
+
|
|
|
+ /// 在Measure内，储存当前lidar数据及lidar扫描时间内对应的imu数据序列
|
|
|
+ if (sync_packages(Measures)) {
|
|
|
+ ikf_lock_.lock();
|
|
|
+ //第一帧lidar数据
|
|
|
+ if (flg_first_scan) {
|
|
|
+ first_lidar_time = Measures.lidar_beg_time; //记录第一帧绝对时间
|
|
|
+ p_imu->first_lidar_time = first_lidar_time; //记录第一帧绝对时间
|
|
|
+ flg_first_scan = false;
|
|
|
+ ikf_lock_.unlock();
|
|
|
+ continue;
|
|
|
+ }
|
|
|
|
|
|
- double t0, t1, t2, t3, t4, t5, match_start, solve_start, svd_time;
|
|
|
+ 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();
|
|
|
+ match_time = 0;
|
|
|
+ kdtree_search_time = 0.0;
|
|
|
+ solve_time = 0;
|
|
|
+ solve_const_H_time = 0;
|
|
|
+ svd_time = 0;
|
|
|
+ t0 = omp_get_wtime();
|
|
|
|
|
|
- //根据imu数据序列和lidar数据，向前传播纠正点云的畸变, 此前已经完成间隔采样或特征提取
|
|
|
- // feats_undistort 为畸变纠正之后的点云,lidar系
|
|
|
- p_imu->Process(Measures, ikfom_, feats_undistort);
|
|
|
- state_point = ikfom_.get_x(); // 前向传播后body的状态预测值
|
|
|
- pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // global系 lidar位置
|
|
|
+ //根据imu数据序列和lidar数据，向前传播纠正点云的畸变, 此前已经完成间隔采样或特征提取
|
|
|
+ // feats_undistort 为畸变纠正之后的点云,lidar系
|
|
|
|
|
|
- if (feats_undistort->empty() || (feats_undistort == NULL))
|
|
|
- {
|
|
|
- ROS_WARN("No point, skip this scan!\n");
|
|
|
- continue;
|
|
|
- }
|
|
|
+ p_imu->Process(Measures, ikfom_, feats_undistort);
|
|
|
+ state_point = ikfom_.get_x(); // 前向传播后body的状态预测值
|
|
|
+ pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // global系 lidar位置
|
|
|
|
|
|
- // 检查当前lidar数据时间，与最早lidar数据时间是否足够,两帧间隔0.1s
|
|
|
- flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true;
|
|
|
-
|
|
|
- /*** Segment the map in lidar FOV ***/
|
|
|
- if(locate_mode==false)
|
|
|
- lasermap_fov_segment(); // 根据lidar在W系下的位置，重新确定局部地图的包围盒角点，移除远端的点
|
|
|
-
|
|
|
- /*** 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(locate_mode==false) {
|
|
|
- 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])); // point转到world系下
|
|
|
- }
|
|
|
- // world系下对当前帧降采样后的点云，初始化lkd-tree
|
|
|
- ikdtree.Build(feats_down_world->points);
|
|
|
- }
|
|
|
- continue;
|
|
|
- }
|
|
|
- }
|
|
|
- int featsFromMapNum = ikdtree.validnum();
|
|
|
- kdtree_size_st = ikdtree.size();
|
|
|
+ if (feats_undistort->empty() || (feats_undistort == NULL)) {
|
|
|
+ ROS_WARN("No point, skip this scan!\n");
|
|
|
+ ikf_lock_.unlock();
|
|
|
+ continue;
|
|
|
+ }
|
|
|
|
|
|
- // cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;
|
|
|
+ // 检查当前lidar数据时间，与最早lidar数据时间是否足够,两帧间隔0.1s
|
|
|
+ flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true;
|
|
|
+
|
|
|
+ /*** Segment the map in lidar FOV ***/
|
|
|
+ if (locate_mode == false)
|
|
|
+ lasermap_fov_segment(); // 根据lidar在W系下的位置，重新确定局部地图的包围盒角点，移除远端的点
|
|
|
+ else {
|
|
|
+ /*** downsample the feature points in a scan ***/
|
|
|
+ double ndt_beg = omp_get_wtime();
|
|
|
+ downSizeFilterSurf.setInputCloud(feats_undistort);
|
|
|
+ downSizeFilterSurf.filter(*feats_down_body);
|
|
|
+
|
|
|
+ pcl::NormalDistributionsTransform<PointType, PointType> ndt;
|
|
|
+ ndt.setTransformationEpsilon(0.001);
|
|
|
+ // Setting maximum step size for More-Thuente line search.
|
|
|
+ ndt.setStepSize(0.1);
|
|
|
+ //Setting Resolution of NDT grid structure (VoxelGridCovariance).
|
|
|
+ ndt.setResolution(0.5);
|
|
|
+ // Setting max number of registration iterations.
|
|
|
+ ndt.setMaximumIterations(50);
|
|
|
+ // Setting point cloud to be aligned.
|
|
|
+ ndt.setInputSource(feats_down_body);
|
|
|
+ // Setting point cloud to be aligned to.
|
|
|
+ ndt.setInputTarget(featsFromMap);
|
|
|
+ pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
|
|
|
+ ndt.align(*unused_result, Eigen::Matrix4f::Identity());
|
|
|
+ double ndt_end = omp_get_wtime();
|
|
|
+
|
|
|
+ printf(" ndt time :%f , size(%d %d)\n", ndt_end - ndt_beg, feats_down_body->size(), featsFromMap->size());
|
|
|
|
|
|
- /*** ICP and iterated Kalman filter update ***/
|
|
|
- if (feats_down_size < 5)
|
|
|
- {
|
|
|
- ROS_WARN("No point, skip this scan!\n");
|
|
|
- continue;
|
|
|
- }
|
|
|
+ }
|
|
|
|
|
|
- normvec->resize(feats_down_size);
|
|
|
- feats_down_world->resize(feats_down_size);
|
|
|
+ t1 = omp_get_wtime();
|
|
|
+ feats_down_size = feats_down_body->points.size(); //当前帧降采样后点数
|
|
|
|
|
|
- // lidar --> imu
|
|
|
- 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;
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- 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;
|
|
|
- ikfom_.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); //预测、更新
|
|
|
- state_point = ikfom_.get_x();
|
|
|
- euler_cur = SO3ToEuler(state_point.rot);
|
|
|
- pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // world系下lidar坐标
|
|
|
- geoQuat.x = state_point.rot.coeffs()[0]; // world系下当前imu的姿态四元数
|
|
|
- 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();
|
|
|
-
|
|
|
- getCurPose(state_point); // 更新transformTobeMapped
|
|
|
- /*back end*/
|
|
|
- // 1.计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
|
|
|
- // 2.添加激光里程计因子、GPS因子、闭环因子
|
|
|
- // 3.执行因子图优化
|
|
|
- // 4.得到当前帧优化后的位姿，位姿协方差
|
|
|
- // 5.添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
|
|
|
- if(locate_mode==false) {
|
|
|
- saveKeyFramesAndFactor();
|
|
|
- // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹， 重构ikdtree
|
|
|
- correctPoses();
|
|
|
+ /*** initialize the map kdtree ***/
|
|
|
+ if (locate_mode == false) {
|
|
|
+ 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])); // point转到world系下
|
|
|
}
|
|
|
- double tloop = omp_get_wtime();
|
|
|
- printf(" match time:%fs,kdtree points:%d save KeyFrame loop time:%fs\n",t_update_end-t_update_start,
|
|
|
- ikdtree.validnum(),tloop-t_update_end);
|
|
|
- /******* Publish odometry *******/
|
|
|
- publish_odometry(pubOdomAftMapped);
|
|
|
- /*** add the feature points to map kdtree ***/
|
|
|
-
|
|
|
-
|
|
|
- if(locate_mode==false) {
|
|
|
- t3 = omp_get_wtime();
|
|
|
- map_incremental();
|
|
|
- t5 = omp_get_wtime();
|
|
|
+ // world系下对当前帧降采样后的点云，初始化lkd-tree
|
|
|
+ ikdtree.Build(feats_down_world->points);
|
|
|
}
|
|
|
- // Publish points
|
|
|
- if (scan_pub_en)
|
|
|
- publish_frame_world(pubLaserCloudFull); // 发布world系下的点云
|
|
|
-
|
|
|
- if (path_en){
|
|
|
- publish_path(pubPath);
|
|
|
- publish_path_update(pubPathUpdate); // 发布经过isam2优化后的路径
|
|
|
- static int jjj = 0;
|
|
|
- jjj++;
|
|
|
- if (jjj % 100 == 0)
|
|
|
- {
|
|
|
- publishGlobalMap(); // 发布局部点云特征地图
|
|
|
- }
|
|
|
- }
|
|
|
+ ikf_lock_.unlock();
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ int featsFromMapNum = ikdtree.validnum();
|
|
|
+ kdtree_size_st = ikdtree.size();
|
|
|
|
|
|
- if (scan_pub_en && scan_body_pub_en)
|
|
|
- publish_frame_body(pubLaserCloudFull_body); // 发布imu系下的点云
|
|
|
+ // cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;
|
|
|
|
|
|
- /*
|
|
|
- //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);
|
|
|
- }*/
|
|
|
+ /*** ICP and iterated Kalman filter update ***/
|
|
|
+ if (feats_down_size < 5) {
|
|
|
+ ROS_WARN("No point, skip this scan!\n");
|
|
|
+ ikf_lock_.unlock();
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ normvec->resize(feats_down_size);
|
|
|
+ feats_down_world->resize(feats_down_size);
|
|
|
+
|
|
|
+ // lidar --> imu
|
|
|
+ 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;
|
|
|
+
|
|
|
+ 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;
|
|
|
+ bool matched = ikfom_.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);//预测、更新
|
|
|
+ state_point = ikfom_.get_x();
|
|
|
+ last_state_point = state_point;
|
|
|
+ /*if (matched==false && locate_mode==true) {
|
|
|
+ printf(" ikfom state update failed ...continue...\n");
|
|
|
+ ikfom_.change_x(last_state_point);//恢复到前一帧的数据
|
|
|
+ state_point = last_state_point;
|
|
|
+ }*/
|
|
|
+
|
|
|
+ euler_cur = SO3ToEuler(state_point.rot);
|
|
|
+ pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // world系下lidar坐标
|
|
|
+ geoQuat.x = state_point.rot.coeffs()[0]; // world系下当前imu的姿态四元数
|
|
|
+ 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();
|
|
|
+ getCurPose(state_point); // 更新transformTobeMapped
|
|
|
+ // Publish points
|
|
|
+ if (scan_pub_en) {
|
|
|
+ publish_frame_world(pubLaserCloudFull); // 发布world系下的点云
|
|
|
+ }
|
|
|
+ /******* Publish odometry *******/
|
|
|
+ publish_odometry(pubOdomAftMapped);
|
|
|
+ ikf_lock_.unlock();
|
|
|
+ /*if (matched == false && locate_mode==true) {
|
|
|
+ continue;
|
|
|
+ }*/
|
|
|
+
|
|
|
+ /*back end*/
|
|
|
+ // 1.计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
|
|
|
+ // 2.添加激光里程计因子、GPS因子、闭环因子
|
|
|
+ // 3.执行因子图优化
|
|
|
+ // 4.得到当前帧优化后的位姿，位姿协方差
|
|
|
+ // 5.添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
|
|
|
+ if (locate_mode == false) {
|
|
|
+ saveKeyFramesAndFactor();
|
|
|
+ // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹， 重构ikdtree
|
|
|
+ correctPoses();
|
|
|
+ }
|
|
|
+ double tloop = omp_get_wtime();
|
|
|
+ /*printf(" match time:%fs,kdtree points:%d save KeyFrame loop time:%fs\n", t_update_end - t_update_start,
|
|
|
+ ikdtree.validnum(), tloop - t_update_end);*/
|
|
|
+
|
|
|
+ /*** add the feature points to map kdtree ***/
|
|
|
+ if (locate_mode == false) {
|
|
|
+ t3 = omp_get_wtime();
|
|
|
+ map_incremental();
|
|
|
+ t5 = omp_get_wtime();
|
|
|
+ }
|
|
|
+
|
|
|
+ if (path_en) {
|
|
|
+ publish_path(pubPath);
|
|
|
+ publish_path_update(pubPathUpdate); // 发布经过isam2优化后的路径
|
|
|
+ static int jjj = 0;
|
|
|
+ jjj++;
|
|
|
+ if (jjj % 100 == 0) {
|
|
|
+ publishGlobalMap(); // 发布局部点云特征地图
|
|
|
}
|
|
|
+ }
|
|
|
|
|
|
- status = ros::ok();
|
|
|
- rate.sleep();
|
|
|
- }
|
|
|
+ if (scan_pub_en && scan_body_pub_en)
|
|
|
+ publish_frame_body(pubLaserCloudFull_body); // 发布imu系下的点云
|
|
|
|
|
|
- /**************** 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);
|
|
|
+ /*
|
|
|
+ //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);
|
|
|
+ }*/
|
|
|
}
|
|
|
|
|
|
- fout_out.close();
|
|
|
- fout_pre.close();
|
|
|
+ status = ros::ok();
|
|
|
+ rate.sleep();
|
|
|
+ }
|
|
|
|
|
|
- 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);
|
|
|
- }
|
|
|
+ /**************** 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);
|
|
|
+ }
|
|
|
|
|
|
- startFlag = false;
|
|
|
- if(locate_mode==false) {
|
|
|
- loopthread->join(); // 分离线程
|
|
|
+ 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]);
|
|
|
}
|
|
|
- delete loopthread;
|
|
|
+ fclose(fp2);
|
|
|
+ }
|
|
|
+
|
|
|
+ startFlag = false;
|
|
|
+ if (locate_mode == false) {
|
|
|
+ loopthread->join(); // 分离线程
|
|
|
+ }
|
|
|
+ delete loopthread;
|
|
|
|
|
|
- return 0;
|
|
|
+ return 0;
|
|
|
}
|
