分离timerrecord.hpp

CMakeLists.txt

@@ -89,6 +89,7 @@ add_executable(${PROJECT_NAME}_node
 		lio/src/lio/Estimator.cpp
 		lio/src/lio/IMUIntegrator.cpp
 		lio/src/lio/ceresfunc.cpp
+		lio/src/lio/ndtMap.cpp
 
 		lio/src/utils/TimerRecord.cpp
 

lio/src/lio/Estimator.cpp

@@ -822,7 +822,7 @@ void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
     //添加每帧数据的位姿优化变量
     for (int i = 0; i < windowSize; ++i)
     {
-      //三维P和朝向（旋转向量）
+      //三维P和朝向（rpy）
       problem.AddParameterBlock(para_PR[i], 6);
     }
     //添加每帧数据的速度优化变量

lio/src/lio/mapper.cpp

@@ -5,6 +5,7 @@
 #include "mapper.h"
 #include "utils/TimerRecord.h"
 
+
 Mapper::Mapper(int wndsize,LidarFeatureExtractor* feature_extractor,int Use_seg,float filter_coner,float filter_surf,
                int ivox_nearby_type,int max_ivox_node,float resolution)
 {
@@ -19,6 +20,7 @@ Mapper::Mapper(int wndsize,LidarFeatureExtractor* feature_extractor,int Use_seg,
   estimator = new Estimator(filter_parameter_corner, filter_parameter_surf,
                             ivox_nearby_type, max_ivox_node, resolution);
   lidarFrameList.reset(new std::list<Estimator::LidarFrame>);
+
   thread_ = std::thread(&Mapper::process, this);
 
 }
@@ -534,7 +536,6 @@ void Mapper::process()
     TimerRecord::Execute(
             [&,this](){
 
-
     if (Use_seg_)
     {
       lidarFeatureExtractor_->FeatureExtract_with_segment(cloudFull,

lio/src/lio/mapper.h

@@ -9,6 +9,7 @@
 
 #include "typedef.h"
 #include "segment/LidarFeatureExtractor.h"
+#include <pcl/registration/ndt.h>
 
 class Mapper
 {

lio/src/lio/ndtMap.cpp

@@ -0,0 +1,52 @@
+//
+// Created by zx on 22-11-23.
+//
+
+#include "ndtMap.h"
+#include <pcl/common/centroid.h>
+Ndt::Ndt()
+{
+  inited_=false;
+}
+Ndt::~Ndt()
+{
+
+}
+
+void Ndt::Init(PointCloud::Ptr cloud)
+{
+  Eigen::Vector4f centroid;
+  pcl::compute3DCentroid(*cloud, centroid);
+
+  center_[0] = centroid[0];
+  center_[1] = centroid[0];
+  center_[2] = centroid[0];
+
+  Eigen::MatrixXd M;
+  M.resize(3,cloud->size());
+  for(int i=0;i<cloud->size();++i)
+  {
+    Eigen::Vector3d pt(cloud->points[i].x,cloud->points[i].y,cloud->points[i].z);
+    Eigen::Vector3d diff=pt-center_;
+    M.topLeftCorner(3,1)=diff;
+  }
+  sigma_=M*(M.inverse());
+}
+
+template <class T>
+T Ndt::GetProb(Eigen::Matrix<T,3,1> pt)
+{
+  Eigen::Matrix<T,3,1> center;
+  center<<T(center_[0]),T(center_[1]),T(center_[2]);
+  Eigen::Matrix<T,3,1> point(pt[0],pt[1],pt[2]);
+  Eigen::Matrix<T,3,1> diff=point-center;
+
+  Eigen::Matrix<T,3,3> sigma;
+  sigma<<T(sigma_(0,0)),T(sigma_(0,1)),T(sigma_(0,2)),
+          T(sigma_(1,0)),T(sigma_(1,1)),T(sigma_(1,2)),
+          T(sigma_(2,0)),T(sigma_(2,1)),T(sigma_(2,2));
+
+  Eigen::Matrix<T,1,1> P=diff.transpose()*sigma.inverse()*diff;
+  return std::exp(P(0,0));
+}
+

lio/src/lio/ndtMap.h

@@ -0,0 +1,38 @@
+//
+// Created by zx on 22-11-23.
+//
+
+#ifndef LIO_LIVOX_CPP_LIO_SRC_LIO_NDTMAP_H_
+#define LIO_LIVOX_CPP_LIO_SRC_LIO_NDTMAP_H_
+#include "typedef.h"
+
+class Ndt
+{
+ public:
+    Ndt();
+    ~Ndt();
+    void Init(PointCloud::Ptr cloud);
+    template <class T>
+    T GetProb(Eigen::Matrix<T,3,1> pt);
+
+ protected:
+    bool inited_;
+    Eigen::Matrix3d sigma_;
+    Eigen::Vector3d center_;
+};
+
+class NdtMap
+{
+ public:
+    NdtMap();
+    ~NdtMap();
+    void SetTargetCloud(PointCloud::Ptr target);
+
+ private:
+    double resolution_;
+    PointType target_center_;
+
+};
+
+
+#endif //LIO_LIVOX_CPP_LIO_SRC_LIO_NDTMAP_H_

lio/src/utils/TimerRecord.h

@@ -33,20 +33,23 @@ class TimerRecord
  public:
 
  public:
-    template <class F>
-    inline static void Execute(F func,const std::string& func_name)
+    template<class F>
+    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;
+      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();
+      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();
@@ -54,7 +57,7 @@ class TimerRecord
 
  protected:
     static std::mutex mutex_;
-    static std::map<std::string,FuncRecord> records_;
+    static std::map<std::string, FuncRecord> records_;
 };