分离timerrecord.hpp

CMakeLists.txt

@@ -52,51 +52,6 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 ## make sure the livox_sdk_static library is installed
 find_library(LIVOX_SDK_LIBRARY liblivox_sdk_static.a /usr/local/lib)
 
-if((NOT LIVOX_SDK_LIBRARY) OR (NOT EXISTS ${LIVOX_SDK_LIBRARY}))
-	# couldn't find the livox sdk library
-	message("Coudn't find livox sdk library!")
-	message("Download Livox-SDK from github and build&install it please!")
-	message("Download Livox-SDK from github and build&install it please!")
-	message("Download Livox-SDK from github and build&install it please!")
-
-	message("git clone Livox-SDK from github temporarily, only for ROS distro jenkins build!")
-
-	# clone livox sdk source code from github
-	execute_process(COMMAND rm -rf ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK OUTPUT_VARIABLE cmd_res)
-	message("Try to pull the livox sdk source code from github")
-	FOREACH(res ${cmd_res})
-		MESSAGE(${res})
-	ENDFOREACH()
-
-	execute_process(COMMAND git clone https://github.com/Livox-SDK/Livox-SDK.git ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK OUTPUT_VARIABLE cmd_res)
-	FOREACH(res ${cmd_res})
-		MESSAGE(${res})
-	ENDFOREACH()
-
-	execute_process(COMMAND cmake .. WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build OUTPUT_VARIABLE cmd_res)
-	FOREACH(res ${cmd_res})
-		MESSAGE(${res})
-	ENDFOREACH()
-
-	execute_process(COMMAND make WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build OUTPUT_VARIABLE cmd_res)
-	FOREACH(res ${cmd_res})
-		MESSAGE(${res})
-	ENDFOREACH()
-
-	include_directories(
-		./
-		${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/sdk_core/include
-	)
-
-	link_directories(
-		./
-		${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build/sdk_core
-	)
-
-else()
-	message("find livox sdk library success")
-endif()
-
 include_directories(
 		define
 		lio/include
@@ -135,6 +90,8 @@ add_executable(${PROJECT_NAME}_node
 		lio/src/lio/IMUIntegrator.cpp
 		lio/src/lio/ceresfunc.cpp
 
+		lio/src/utils/TimerRecord.cpp
+
 		livox/livox_driver/lvx_file.cpp
 		livox/livox_driver/ldq.cpp
 		livox/livox_driver/lds.cpp
@@ -149,6 +106,8 @@ add_executable(${PROJECT_NAME}_node
 		livox/common/comm/gps_protocol.cpp
 
 
+
+
     )
 
 #---------------------------------------------------------------------------------------

lio/src/ScanRegistration.cpp

@@ -1,117 +0,0 @@
-#include "segment/LidarFeatureExtractor.h"
-#include <ros/ros.h>
-#include <livox_ros_driver/CustomMsg.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <pcl_conversions/pcl_conversions.h>
-
-ros::Publisher pubFullLaserCloud;
-ros::Publisher pubSharpCloud;
-ros::Publisher pubFlatCloud;
-ros::Publisher pubNonFeature;
-
-LidarFeatureExtractor* lidarFeatureExtractor;
-pcl::PointCloud<PointType>::Ptr laserCloud;
-pcl::PointCloud<PointType>::Ptr laserConerCloud;
-pcl::PointCloud<PointType>::Ptr laserSurfCloud;
-pcl::PointCloud<PointType>::Ptr laserNonFeatureCloud;
-int Lidar_Type = 0;
-int N_SCANS = 6;
-bool Feature_Mode = false;
-bool Use_seg = false;
-
-
-
-
-void lidarCallBackHorizon(const livox_ros_driver::CustomMsgConstPtr &msg)
-{
-
-    double timeSpan = ros::Time().fromNSec(msg->points.back().offset_time).toSec();
-    laserCloud.reset(new pcl::PointCloud<PointType>);
-    laserCloud->reserve(15000*N_SCANS);
-    PointType point;
-    for(const auto& p : msg->points)
-    {
-
-        int line_num = (int) p.line;
-        if (line_num > N_SCANS - 1) continue;
-        if (p.x < 0.01) continue;
-        if (!pcl_isfinite(p.x) ||
-                !pcl_isfinite(p.y) ||
-                !pcl_isfinite(p.z))
-        {
-            continue;
-        }
-        point.x = p.x;
-        point.y = p.y;
-        point.z = p.z;
-        point.intensity = p.reflectivity;
-        point.normal_x = ros::Time().fromNSec(p.offset_time).toSec() / timeSpan; //normao_x  time
-        point.normal_y = LidarFeatureExtractor::_int_as_float(line_num);                               //normal_y  line_num
-        laserCloud->push_back(point);
-    }
-
-
-  if(Use_seg){
-    lidarFeatureExtractor->FeatureExtract_with_segment( laserCloud, laserConerCloud, laserSurfCloud, laserNonFeatureCloud,N_SCANS);
-  }
-  else{
-    lidarFeatureExtractor->FeatureExtract(laserCloud, laserConerCloud, laserSurfCloud,N_SCANS);
-  } 
-
-  sensor_msgs::PointCloud2 laserCloudMsg;
-  pcl::toROSMsg(*laserCloud, laserCloudMsg);
-  laserCloudMsg.header = msg->header;
-  laserCloudMsg.header.stamp.fromNSec(msg->timebase+msg->points.back().offset_time);
-  pubFullLaserCloud.publish(laserCloudMsg);
-
-}
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "ScanRegistration");
-  ros::NodeHandle nodeHandler("~");
-
-  ros::Subscriber customCloud;;
-
-  std::string config_file;
-  nodeHandler.getParam("config_file", config_file);
-
-  cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
-  if (!fsSettings.isOpened()) {
-    std::cout << "config_file error: cannot open " << config_file << std::endl;
-    return false;
-  }
-  Lidar_Type = static_cast<int>(fsSettings["Lidar_Type"]);
-  N_SCANS = static_cast<int>(fsSettings["Used_Line"]);
-  Feature_Mode = static_cast<int>(fsSettings["Feature_Mode"]);
-  Use_seg = static_cast<int>(fsSettings["Use_seg"]);
-
-  int NumCurvSize = static_cast<int>(fsSettings["NumCurvSize"]);
-  float DistanceFaraway = static_cast<float>(fsSettings["DistanceFaraway"]);
-  int NumFlat = static_cast<int>(fsSettings["NumFlat"]);
-  int PartNum = static_cast<int>(fsSettings["PartNum"]);
-  float FlatThreshold = static_cast<float>(fsSettings["FlatThreshold"]);
-  float BreakCornerDis = static_cast<float>(fsSettings["BreakCornerDis"]);
-  float LidarNearestDis = static_cast<float>(fsSettings["LidarNearestDis"]);
-  float KdTreeCornerOutlierDis = static_cast<float>(fsSettings["KdTreeCornerOutlierDis"]);
-
-  laserCloud.reset(new pcl::PointCloud<PointType>);
-  laserConerCloud.reset(new pcl::PointCloud<PointType>);
-  laserSurfCloud.reset(new pcl::PointCloud<PointType>);
-  laserNonFeatureCloud.reset(new pcl::PointCloud<PointType>);
-
-  customCloud = nodeHandler.subscribe<livox_ros_driver::CustomMsg>("/livox/lidar", 100, &lidarCallBackHorizon);
-
-  pubFullLaserCloud = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_full_cloud", 10);
-  pubSharpCloud = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_less_sharp_cloud", 10);
-  pubFlatCloud = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_less_flat_cloud", 10);
-  pubNonFeature = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_nonfeature_cloud", 10);
-
-  lidarFeatureExtractor = new LidarFeatureExtractor(N_SCANS,NumCurvSize,DistanceFaraway,NumFlat,PartNum,
-                                                    FlatThreshold,BreakCornerDis,LidarNearestDis,KdTreeCornerOutlierDis);
-
-  ros::spin();
-
-  return 0;
-}
-

lio/src/lio/Estimator.cpp

@@ -1,5 +1,5 @@
 #include "Estimator.h"
-
+#include "utils/TimerRecord.h"
 
 Estimator::Estimator(const float& filter_corner, const float& filter_surf,
                      int ivox_nearby_type,int max_ivox_node,float resolution){
@@ -85,7 +85,8 @@ void Estimator::processPointToLine(std::vector<ceres::CostFunction *>& edges,
   int debug_num2 = 0;
   int debug_num12 = 0;
   int debug_num22 = 0;
-  for (int i = 0; i < laserCloudCornerStackNum; i++) {
+  for (int i = 0; i < laserCloudCornerStackNum; i++)
+  {
     _pointOri = laserCloudCorner->points[i];
       LocalMapIvox::pointAssociateToMap(&_pointOri, &_pointSel, m4d);
 
@@ -875,32 +876,35 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
       transformTobeMapped.topRightCorner(3, 1) = frame_curr->Q * exPbl + frame_curr->P;
 
 
-      threads[0] = std::thread(&Estimator::processPointToLine, this,
-                               std::ref(edgesLine[f]),
-                               std::ref(vLineFeatures[f]),
-                               std::ref(laserCloudCornerStack[f]),
-                               std::ref(exTlb),
-                               std::ref(transformTobeMapped));
+      TimerRecord::Execute(
+              [&,this]() {
+                threads[0] = std::thread(&Estimator::processPointToLine, this,
+                                         std::ref(edgesLine[f]),
+                                         std::ref(vLineFeatures[f]),
+                                         std::ref(laserCloudCornerStack[f]),
+                                         std::ref(exTlb),
+                                         std::ref(transformTobeMapped));
 
 
-      threads[1] = std::thread(&Estimator::processPointToPlanVec, this,
-                               std::ref(edgesPlan[f]),
-                               std::ref(vPlanFeatures[f]),
-                               std::ref(laserCloudSurfStack[f]),
-                               std::ref(exTlb),
-                               std::ref(transformTobeMapped));
+                threads[1] = std::thread(&Estimator::processPointToPlanVec, this,
+                                         std::ref(edgesPlan[f]),
+                                         std::ref(vPlanFeatures[f]),
+                                         std::ref(laserCloudSurfStack[f]),
+                                         std::ref(exTlb),
+                                         std::ref(transformTobeMapped));
 
 
-      threads[2] = std::thread(&Estimator::processNonFeatureICP, this,
-                               std::ref(edgesNon[f]),
-                               std::ref(vNonFeatures[f]),
-                               std::ref(laserCloudNonFeatureStack[f]),
-                               std::ref(exTlb),
-                               std::ref(transformTobeMapped));
+                threads[2] = std::thread(&Estimator::processNonFeatureICP, this,
+                                         std::ref(edgesNon[f]),
+                                         std::ref(vNonFeatures[f]),
+                                         std::ref(laserCloudNonFeatureStack[f]),
+                                         std::ref(exTlb),
+                                         std::ref(transformTobeMapped));
 
-      threads[0].join();
-      threads[1].join();
-      threads[2].join();
+                threads[0].join();
+                threads[1].join();
+                threads[2].join();
+              },"Estimator::FMatch");
 
     }
 
@@ -1069,8 +1073,12 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
     options.minimizer_progress_to_stdout = false;
     options.num_threads = 6;
     ceres::Solver::Summary summary;
-
-    ceres::Solve(options, &problem, &summary);
+  TimerRecord::Execute(
+          [&,this]()
+          {
+            ceres::Solve(options, &problem, &summary);
+          },"Estimator::solve"
+  );
     final_cost_ = summary.final_cost;
 
     //将优化的结果赋值到lidarFrameList的每一帧数据
@@ -1125,31 +1133,34 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
       edgesPlan[f].clear();
       edgesNon[f].clear();
 
-
-      threads[0] = std::thread(&Estimator::processPointToLine, this,
-                               std::ref(edgesLine[f]),
-                               std::ref(vLineFeatures[f]),
-                               std::ref(laserCloudCornerStack[f]),
-                               std::ref(exTlb),
-                               std::ref(transformTobeMapped));
-
-      threads[1] = std::thread(&Estimator::processPointToPlanVec, this,
-                               std::ref(edgesPlan[f]),
-                               std::ref(vPlanFeatures[f]),
-                               std::ref(laserCloudSurfStack[f]),
-                               std::ref(exTlb),
-                               std::ref(transformTobeMapped));
-
-      threads[2] = std::thread(&Estimator::processNonFeatureICP, this,
-                               std::ref(edgesNon[f]),
-                               std::ref(vNonFeatures[f]),
-                               std::ref(laserCloudNonFeatureStack[f]),
-                               std::ref(exTlb),
-                               std::ref(transformTobeMapped));
-
-      threads[0].join();
-      threads[1].join();
-      threads[2].join();
+      TimerRecord::Execute(
+              [&,this]() {
+
+                threads[0] = std::thread(&Estimator::processPointToLine, this,
+                                         std::ref(edgesLine[f]),
+                                         std::ref(vLineFeatures[f]),
+                                         std::ref(laserCloudCornerStack[f]),
+                                         std::ref(exTlb),
+                                         std::ref(transformTobeMapped));
+
+                threads[1] = std::thread(&Estimator::processPointToPlanVec, this,
+                                         std::ref(edgesPlan[f]),
+                                         std::ref(vPlanFeatures[f]),
+                                         std::ref(laserCloudSurfStack[f]),
+                                         std::ref(exTlb),
+                                         std::ref(transformTobeMapped));
+
+                threads[2] = std::thread(&Estimator::processNonFeatureICP, this,
+                                         std::ref(edgesNon[f]),
+                                         std::ref(vNonFeatures[f]),
+                                         std::ref(laserCloudNonFeatureStack[f]),
+                                         std::ref(exTlb),
+                                         std::ref(transformTobeMapped));
+
+                threads[0].join();
+                threads[1].join();
+                threads[2].join();
+              },"Estimator::FMatch");
       int cntFtu = 0;
       for (auto &e : edgesLine[f])
       {

lio/src/utils/TimerRecord.cpp

@@ -0,0 +1,31 @@
+//
+// Created by zx on 22-11-22.
+//
+
+#include "TimerRecord.h"
+#include <algorithm>
+//
+// Created by zx on 22-11-3.
+//
+
+
+void TimerRecord::PrintAll()
+{
+  std::cout <<">> ===== Printing run time ====="<<std::endl;
+  for (const auto& r : records_) {
+    auto v = std::minmax_element(r.second.timesqueue_.begin(), r.second.timesqueue_.end());
+
+    std::cout<< "> [ " << r.first << " ] average time usage: "
+             << std::accumulate(r.second.timesqueue_.begin(), r.second.timesqueue_.end(), 0.0) /
+                     double(r.second.timesqueue_.size())
+             << " ms , called times: " << r.second.timesqueue_.size()<<
+             "   max:"<<*v.second<<"ms , min:"<<*v.first<<"ms"<<std::endl;
+  }
+  std::cout << ">>> ===== Printing run time end ====="<<std::endl;
+}
+
+std::map<std::string,FuncRecord> TimerRecord::records_=std::map<std::string,FuncRecord>();
+std::mutex TimerRecord::mutex_=std::mutex();
+
+
+

lio/src/utils/TimerRecord.h

@@ -6,11 +6,13 @@
 #define SRC_LIO_LIVOX_SRC_UTILS_TIMERRECORD_H_
 
 #include <map>
+#include <mutex>
 #include <string>
 #include <vector>
 #include <chrono>
 #include <numeric>
 #include <iostream>
+#include <algorithm>
 
 struct FuncRecord
 {
@@ -32,40 +34,29 @@ class TimerRecord
 
  public:
     template <class F>
-    static void Execute(F func,const std::string& func_name)
+    inline static void Execute(F func,const std::string& func_name)
     {
         auto t1 = std::chrono::high_resolution_clock::now();
         func();
         auto t2 = std::chrono::high_resolution_clock::now();
         auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;
+      mutex_.lock();
         if (records_.find(func_name) != records_.end()) {
             records_[func_name].timesqueue_.emplace_back(time_used);
         } else {
             records_.insert({func_name, FuncRecord(func_name, time_used)});
         }
+        mutex_.unlock();
     }
 
-    static void PrintAll()
-    {
-        std::cout <<">> ===== Printing run time ====="<<std::endl;
-        for (const auto& r : records_) {
-            auto v = std::minmax_element(r.second.timesqueue_.begin(), r.second.timesqueue_.end());
-
-            std::cout<< "> [ " << r.first << " ] average time usage: "
-                     << std::accumulate(r.second.timesqueue_.begin(), r.second.timesqueue_.end(), 0.0) /
-                             double(r.second.timesqueue_.size())
-                     << " ms , called times: " << r.second.timesqueue_.size()<<
-                     "   max:"<<*v.second<<"ms , min:"<<*v.first<<"ms"<<std::endl;
-        }
-        std::cout << ">>> ===== Printing run time end ====="<<std::endl;
-    }
-
+    static void PrintAll();
 
 
  protected:
+    static std::mutex mutex_;
     static std::map<std::string,FuncRecord> records_;
 };
-std::map<std::string,FuncRecord> TimerRecord::records_=std::map<std::string,FuncRecord>();
+