local_gpu_cluster_box

test_project/linefit_ground_segmentation_local_gpu_test/CMakeLists.txt

+cmake_minimum_required(VERSION 3.10)
+project(linefit_ground_segmentation_local_gpu_test)
+set(TRT_MAIN_PATH "/host/home/juefx/liuji_workspace/nvidia_lib/TensorRT-8.5.1.7") 
+
+
+#enable_language(CUDA)
+add_definitions(-std=c++17)
+option(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
+
+
+
+find_package(PCL REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(Boost 1.65.1 REQUIRED system)
+find_package(CUDA REQUIRED)
+find_package(Eigen3 REQUIRED)
+include_directories(${Eigen_INCLUDE_DIRS})
+include_directories(${PCL_INCLUDE_DIRS})
+include_directories(${catkin_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+set(PCL_PTH "pcl-1.8")
+
+set(CUDA_NVCC_PLAGS ${CUDA_NVCC_PLAGS};-std=c++17;-g;-G;-gencode;arch=compute_72;code=sm_72)
+
+                                              # use jf preprocess
+#include_directories("/usr/include/${PCL_PTH}" /usr/include/eigen3)
+include_directories(include)
+# include_directories(./cuda/include)
+
+
+if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+        message("xavier on")
+        # add_definitions(-DWERXAVIER)
+        include_directories(/usr/local/cuda/targets/aarch64-linux/include)
+        link_directories(/usr/local/cuda/targets/aarch64-linux/lib)
+        # find_package (OpenCV 4.0.0 REQUIRED)
+        # include_directories ("/usr/include/opencv4/")
+else()
+#       add_compile_definitions(THRUST_IGNORE_CUB_VERSION_CHECK)  # for cuda version >= 11
+        # message( SEND_ERROR "only valid in xavier!" )
+        # add_definitions(-DBOOST_ERROR_CODE_HEADER_ONLY)
+        # add_definitions(-DWERGPU)
+        message("=> xavier off")
+        include_directories("${TRT_MAIN_PATH}/include")
+        include_directories(/usr/local/cuda/include)
+        link_directories("${TRT_MAIN_PATH}/lib" )
+        link_directories(/usr/local/cuda/lib64)
+        link_directories(/usr/local/lib)
+        link_directories(/usr/lib/x86_64-linux-gnu)
+endif()
+
+
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Ofast -Wfatal-errors -D_MWAITXINTRIN_H_INCLUDED")
+
+
+# add_subdirectory(cuda)
+
+file(GLOB gpu_source_files "src/*.cu")
+cuda_add_library(dbscan_cuda SHARED ${gpu_source_files})
+
+# target_include_directories(dbscan_cuda
+#   SYSTEM PUBLIC
+#     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#     $<INSTALL_INTERFACE:include>
+#     ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}
+#   PRIVATE ${PCL_INCLUDE_DIRS}
+# )
+
+target_link_libraries(dbscan_cuda nvinfer cudart)
+target_link_libraries(dbscan_cuda  ${PCL_LIBRARIES})
+
+target_compile_options(dbscan_cuda
+  PRIVATE
+     $<$<COMPILE_LANGUAGE:CUDA>:-expt-extended-lambda -Xcompiler -fPIC -Xcudafe --diag_suppress=esa_on_defaulted_function_ignored>
+)
+
+
+#target_include_directories(myplugins PRIVATE)
+add_executable(linefit_ground_segmentation_test_jf_single ${PROJECT_SOURCE_DIR}/src/ground_segmentation_node_single_test.cc 
+              ${PROJECT_SOURCE_DIR}/src/ground_segmentation.cc ${PROJECT_SOURCE_DIR}/src/segment.cc ${PROJECT_SOURCE_DIR}/src/bin.cc)
+
+add_executable(linefit_ground_segmentation_test_jf_mult ${PROJECT_SOURCE_DIR}/src/ground_segmentation_node_mult_test.cc 
+              ${PROJECT_SOURCE_DIR}/src/ground_segmentation.cc ${PROJECT_SOURCE_DIR}/src/segment.cc ${PROJECT_SOURCE_DIR}/src/bin.cc)
+
+# ground_segmentation_node.cc原始代码 
+add_executable(linefit_ground_segmentation_test_jf ${PROJECT_SOURCE_DIR}/src/ground_segmentation_node.cc 
+              ${PROJECT_SOURCE_DIR}/src/ground_segmentation.cc ${PROJECT_SOURCE_DIR}/src/segment.cc ${PROJECT_SOURCE_DIR}/src/bin.cc)              
+
+
+set(LIBRARIES_TO_LINK dbscan_cuda nvinfer cudart yaml-cpp -lm -pthread -lboost_system ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
+# target_link_libraries(linefit_ground_segmentation_test_jf dbscan_cuda)
+# target_link_libraries(linefit_ground_segmentation_test_jf nvinfer)
+# target_link_libraries(linefit_ground_segmentation_test_jf cudart)
+# target_link_libraries(linefit_ground_segmentation_test_jf -lm -pthread -lboost_system)
+# target_link_libraries(linefit_ground_segmentation_test_jf ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
+# target_link_libraries(linefit_ground_segmentation_test_jf yaml-cpp)
+#target_link_libraries(linefit_ground_segmentation_test_jf -lpcl_filters yaml-cpp)
+
+target_link_libraries(linefit_ground_segmentation_test_jf ${LIBRARIES_TO_LINK})
+target_link_libraries(linefit_ground_segmentation_test_jf_single ${LIBRARIES_TO_LINK})
+target_link_libraries(linefit_ground_segmentation_test_jf_mult ${LIBRARIES_TO_LINK})
+
+add_definitions(-O2 -pthread)

test_project/linefit_ground_segmentation_local_gpu_test/cuda/include/dbscan/CudaUtils.cuh

+/**
+ * @file    CudaUtils.cuh
+ *
+ * @author  btran
+ *
+ */
+
+#pragma once
+
+#include <cstdio>
+#include <cstdlib>
+
+#include <cuda_runtime.h>
+
+namespace cuda
+{
+namespace utils
+{
+cudaError_t warmUpGPU();
+}  // namespace utils
+}  // namespace cuda
+
+inline void HandleError(cudaError_t err, const char* file, int line)
+{
+    if (err != cudaSuccess) {
+        printf("%s in %s at line %d\n", cudaGetErrorString(err), file, line);
+        exit(EXIT_FAILURE);
+    }
+}
+
+#define HANDLE_ERROR(err) (HandleError(err, __FILE__, __LINE__))

test_project/linefit_ground_segmentation_local_gpu_test/cuda/include/dbscan/DBSCAN.cuh

+/**
+ * @file    DBSCAN.cuh
+ *
+ * @author  btran
+ *
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace clustering
+{
+namespace cuda
+{
+template <typename PointType> class DBSCAN
+{
+ public:
+    struct Param {
+        int pointDimension;
+        double eps;
+        int minPoints;
+    };
+
+    explicit DBSCAN(const Param& param);
+    ~DBSCAN();
+
+    std::vector<std::vector<int>> run(const PointType*, int numPoints) const;
+
+ private:
+    Param m_param;
+
+    mutable PointType* m_dPoints;
+    mutable int m_allocatedSize;
+};
+}  // namespace cuda
+}  // namespace clustering

test_project/linefit_ground_segmentation_local_gpu_test/cuda/src/CudaUtils.cu

+/**
+ * @file    CudaUtils.cu
+ *
+ * @author  btran
+ *
+ */
+
+#include <dbscan/CudaUtils.cuh>
+
+namespace cuda
+{
+namespace utils
+{
+namespace
+{
+__global__ void warmUpGPUKernel()
+{
+    int idx = blockIdx.x * blockDim.x + threadIdx.x;
+    ++idx;
+}
+}  // namespace
+
+cudaError_t warmUpGPU()
+{
+    warmUpGPUKernel<<<1, 1>>>();
+    cudaDeviceSynchronize();
+    return cudaGetLastError();
+}
+}  // namespace utils
+}  // namespace cuda

test_project/linefit_ground_segmentation_local_gpu_test/cuda/src/DBSCAN.cu

+/**
+ * @file    DBSCAN.cu
+ *
+ * @author  btran
+ *
+ */
+
+#include <dbscan/CudaUtils.cuh>
+#include <dbscan/DBSCAN.cuh>
+
+#include <pcl/point_types.h>
+
+#include <thrust/device_vector.h>
+#include <thrust/host_vector.h>
+
+#include "Types.cuh"
+
+namespace clustering
+{
+namespace cuda
+{
+namespace
+{
+cudaDeviceProp dProperties;
+
+template <typename PointType>
+__global__ void makeGraphStep1Kernel(const PointType* __restrict__ points, Node* nodes, int* nodeDegs, int numPoints,
+                                     float eps, int minPoints)
+{
+    int tid = threadIdx.x + blockIdx.x * blockDim.x;
+
+    while (tid < numPoints) {
+        Node* node = &nodes[tid];
+        const PointType* point = &points[tid];
+
+        for (int i = 0; i < numPoints; ++i) {
+            if (i == tid) {
+                continue;
+            }
+
+            const PointType* curPoint = &points[i];
+            float diffx = point->x - curPoint->x;
+            float diffy = point->y - curPoint->y;
+            float diffz = point->z - curPoint->z;
+
+            float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
+
+            if (sqrDist < eps * eps) {
+                node->numNeighbors++;
+            }
+        }
+
+        if (node->numNeighbors >= minPoints) {
+            node->type = NodeType::CORE;
+        }
+
+        nodeDegs[tid] = node->numNeighbors;
+
+        tid += blockDim.x * gridDim.x;
+    }
+}
+
+template <typename PointType>
+__global__ void makeGraphStep2Kernel(const PointType* __restrict__ points, const int* __restrict__ neighborStartIndices,
+                                     int* adjList, int numPoints, float eps)
+{
+    int tid = threadIdx.x + blockIdx.x * blockDim.x;
+
+    while (tid < numPoints) {
+        const PointType* point = &points[tid];
+
+        int startIdx = neighborStartIndices[tid];
+
+        int countNeighbors = 0;
+        for (int i = 0; i < numPoints; ++i) {
+            if (i == tid) {
+                continue;
+            }
+
+            const PointType* curPoint = &points[i];
+            float diffx = point->x - curPoint->x;
+            float diffy = point->y - curPoint->y;
+            float diffz = point->z - curPoint->z;
+
+            float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
+
+            if (sqrDist < eps * eps) {
+                adjList[startIdx + countNeighbors++] = i;
+            }
+        }
+
+        tid += blockDim.x * gridDim.x;
+    }
+}
+
+__global__ void BFSKernel(const Node* __restrict__ nodes, const int* __restrict__ adjList,
+                          const int* __restrict__ neighborStartIndices, char* Fa, char* Xa, int numPoints)
+{
+    int tid = threadIdx.x + blockIdx.x * blockDim.x;
+
+    while (tid < numPoints) {
+        if (Fa[tid]) {
+            Fa[tid] = false;
+            Xa[tid] = true;
+
+            int startIdx = neighborStartIndices[tid];
+
+            for (int i = 0; i < nodes[tid].numNeighbors; ++i) {
+                int nIdx = adjList[startIdx + i];
+                Fa[nIdx] = 1 - Xa[nIdx];
+            }
+        }
+
+        tid += blockDim.x * gridDim.x;
+    }
+}
+
+void BFS(Node* hNodes, Node* dNodes, const int* adjList, const int* neighborStartIndices, int v, int numPoints,
+         std::vector<int>& curCluster)
+{
+    thrust::device_vector<char> dXa(numPoints, false);
+    thrust::device_vector<char> dFa(numPoints, false);
+    dFa[v] = true;
+
+    int numThreads = 256;
+    int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
+
+    int countFa = 1;
+    while (countFa > 0) {
+        BFSKernel<<<numBlocks, numThreads>>>(dNodes, adjList, neighborStartIndices,
+                                             thrust::raw_pointer_cast(dFa.data()), thrust::raw_pointer_cast(dXa.data()),
+                                             numPoints);
+        countFa = thrust::count(thrust::device, dFa.begin(), dFa.end(), true);
+    }
+
+    thrust::host_vector<char> hXa = dXa;
+
+    for (int i = 0; i < numPoints; ++i) {
+        if (hXa[i]) {
+            hNodes[i].visited = true;
+            curCluster.emplace_back(i);
+        }
+    }
+}
+
+template <typename PointType> Graph makeGraph(const PointType* points, int numPoints, float eps, int minPoints)
+{
+    Graph graph;
+
+    graph.nodes.resize(numPoints);
+    graph.neighborStartIndices.resize(numPoints);
+
+    thrust::device_vector<int> dNodeDegs(numPoints);
+
+    int numThreads = 256;
+    int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
+
+    makeGraphStep1Kernel<PointType><<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.nodes.data()),
+                                                               thrust::raw_pointer_cast(dNodeDegs.data()), numPoints,
+                                                               eps, minPoints);
+
+    thrust::exclusive_scan(dNodeDegs.begin(), dNodeDegs.end(), graph.neighborStartIndices.begin());
+
+    int totalEdges = dNodeDegs.back() + graph.neighborStartIndices.back();
+    graph.adjList.resize(totalEdges);
+
+    makeGraphStep2Kernel<PointType>
+        <<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.neighborStartIndices.data()),
+                                    thrust::raw_pointer_cast(graph.adjList.data()), numPoints, eps);
+
+    return graph;
+}
+}  // namespace
+
+template <typename PointType>
+DBSCAN<PointType>::DBSCAN(const Param& param)
+    : m_param(param)
+    , m_dPoints(nullptr)
+    , m_allocatedSize(-1)
+{
+    HANDLE_ERROR(cudaGetDeviceProperties(&dProperties, 0));
+}
+
+template <typename PointType> DBSCAN<PointType>::~DBSCAN()
+{
+    if (m_dPoints) {
+        HANDLE_ERROR(cudaFree(m_dPoints));
+        m_dPoints = nullptr;
+    }
+}
+
+template <typename PointType>
+std::vector<std::vector<int>> DBSCAN<PointType>::run(const PointType* points, int numPoints) const
+{
+    if (numPoints <= 0) {
+        throw std::runtime_error("number of points must be more than 0");
+    }
+
+    if (m_allocatedSize < numPoints) {
+        m_allocatedSize = numPoints;
+        if (m_dPoints) {
+            HANDLE_ERROR(cudaFree(m_dPoints));
+        }
+        HANDLE_ERROR(cudaMalloc((void**)&m_dPoints, numPoints * sizeof(PointType)));
+    }
+    HANDLE_ERROR(cudaMemcpy(m_dPoints, points, numPoints * sizeof(PointType), cudaMemcpyHostToDevice));
+
+    auto graph = makeGraph<PointType>(m_dPoints, numPoints, m_param.eps, m_param.minPoints);
+
+    thrust::host_vector<Node> hNodes = graph.nodes;
+
+    std::vector<std::vector<int>> clusterIndices;
+    for (int i = 0; i < numPoints; ++i) {
+        auto& curHNode = hNodes[i];
+        if (curHNode.visited || curHNode.type != NodeType::CORE) {
+            continue;
+        }
+
+        std::vector<int> curCluster;
+        curCluster.emplace_back(i);
+        curHNode.visited = true;
+
+        BFS(hNodes.data(), thrust::raw_pointer_cast(graph.nodes.data()), thrust::raw_pointer_cast(graph.adjList.data()),
+            thrust::raw_pointer_cast(graph.neighborStartIndices.data()), i, numPoints, curCluster);
+
+        clusterIndices.emplace_back(std::move(curCluster));
+    }
+
+    return clusterIndices;
+}
+
+#undef INSTANTIATE_TEMPLATE
+#define INSTANTIATE_TEMPLATE(DATA_TYPE) template class DBSCAN<DATA_TYPE>;
+
+INSTANTIATE_TEMPLATE(pcl::PointXYZ);
+INSTANTIATE_TEMPLATE(pcl::PointXYZI);
+INSTANTIATE_TEMPLATE(pcl::PointXYZRGB);
+INSTANTIATE_TEMPLATE(pcl::PointNormal);
+INSTANTIATE_TEMPLATE(pcl::PointXYZRGBNormal);
+INSTANTIATE_TEMPLATE(pcl::PointXYZINormal);
+
+#undef INSTANTIATE_TEMPLATE
+}  // namespace cuda
+}  // namespace clustering

test_project/linefit_ground_segmentation_local_gpu_test/cuda/src/Types.cuh

+/**
+ * @file    Types.cuh
+ *
+ * @author  btran
+ *
+ */
+
+#pragma once
+
+#include <thrust/device_vector.h>
+
+namespace clustering
+{
+namespace cuda
+{
+enum class NodeType : int { CORE, NOISE };
+
+struct Node {
+    __host__ __device__ Node()
+        : type(NodeType::NOISE)
+        , numNeighbors(0)
+        , visited(false)
+    {
+    }
+
+    NodeType type;
+    int numNeighbors;
+    char visited;
+};
+
+struct Graph {
+    thrust::device_vector<Node> nodes;
+    thrust::device_vector<int> neighborStartIndices;
+    thrust::device_vector<int> adjList;
+};
+}  // namespace cuda
+}  // namespace clustering

test_project/linefit_ground_segmentation_local_gpu_test/include/dbscan/CudaUtils.cuh

+/**
+ * @file    CudaUtils.cuh
+ *
+ * @author  btran
+ *
+ */
+
+#pragma once
+
+#include <cstdio>
+#include <cstdlib>
+
+#include <cuda_runtime.h>
+
+namespace cuda
+{
+namespace utils
+{
+cudaError_t warmUpGPU();
+}  // namespace utils
+}  // namespace cuda
+
+inline void HandleError(cudaError_t err, const char* file, int line)
+{
+    if (err != cudaSuccess) {
+        printf("%s in %s at line %d\n", cudaGetErrorString(err), file, line);
+        exit(EXIT_FAILURE);
+    }
+}
+
+#define HANDLE_ERROR(err) (HandleError(err, __FILE__, __LINE__))

test_project/linefit_ground_segmentation_local_gpu_test/include/dbscan/DBSCAN.cuh

+/**
+ * @file    DBSCAN.cuh
+ *
+ * @author  btran
+ *
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace clustering
+{
+namespace cuda
+{
+template <typename PointType> class DBSCAN
+{
+ public:
+    struct Param {
+        int pointDimension;
+        double eps;
+        int minPoints;
+    };
+
+    explicit DBSCAN(const Param& param);
+    ~DBSCAN();
+
+    std::vector<std::vector<int>> run(const PointType*, int numPoints) const;
+
+ private:
+    Param m_param;
+
+    mutable PointType* m_dPoints;
+    mutable int m_allocatedSize;
+};
+}  // namespace cuda
+}  // namespace clustering

test_project/linefit_ground_segmentation_local_gpu_test/include/dbscan/Types.cuh

+/**
+ * @file    Types.cuh
+ *
+ * @author  btran
+ *
+ */
+
+#pragma once
+
+#include <thrust/device_vector.h>
+
+namespace clustering
+{
+namespace cuda
+{
+enum class NodeType : int { CORE, NOISE };
+
+struct Node {
+    __host__ __device__ Node()
+        : type(NodeType::NOISE)
+        , numNeighbors(0)
+        , visited(false)
+    {
+    }
+
+    NodeType type;
+    int numNeighbors;
+    char visited;
+};
+
+struct Graph {
+    thrust::device_vector<Node> nodes;
+    thrust::device_vector<int> neighborStartIndices;
+    thrust::device_vector<int> adjList;
+};
+}  // namespace cuda
+}  // namespace clustering

test_project/linefit_ground_segmentation_local_gpu_test/include/ground_segmentation/bin.h

+#ifndef GROUND_SEGMENTATION_BIN_H_
+#define GROUND_SEGMENTATION_BIN_H_
+
+#include <atomic>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+class Bin {
+public:
+  /*设定了一个最小Z点的结构体，其中是一个二维结构，在x-y平面的线段长度，以及z轴上的数值*/
+  struct MinZPoint {
+    MinZPoint() : z(0), d(0) {}
+    MinZPoint(const double& d, const double& z) : z(z), d(d) {}
+    bool operator==(const MinZPoint& comp) {return z == comp.z && d == comp.d;}
+
+    double z;
+    double d;
+  };
+
+private:
+
+  std::atomic<bool> has_point_;
+  std::atomic<double> min_z;
+  std::atomic<double> min_z_range;
+
+public:
+
+  Bin();
+
+  /// \brief Fake copy constructor to allow vector<vector<Bin> > initialization.
+  Bin(const Bin& segment);
+
+  void addPoint(const pcl::PointXYZ& point);
+
+  void addPoint(const double& d, const double& z);
+
+  MinZPoint getMinZPoint();
+
+  inline bool hasPoint() {return has_point_;}
+
+};
+
+#endif /* GROUND_SEGMENTATION_BIN_H_ */

test_project/linefit_ground_segmentation_local_gpu_test/include/ground_segmentation/ground_segmentation.h

+#ifndef GROUND_SEGMENTATION_H_
+#define GROUND_SEGMENTATION_H_
+
+#include <mutex>
+
+#include <glog/logging.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include<pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include "ground_segmentation/segment.h"
+
+struct GroundSegmentationParams {
+  GroundSegmentationParams() :
+      visualize(false),
+      r_min_square(0.3 * 0.3),
+      r_max_square(20*20),
+      n_bins(30),
+      n_segments(180),
+      max_dist_to_line(0.15),
+      max_slope(1),
+      n_threads(4),
+      max_error_square(0.01),
+      long_threshold(2.0),
+      max_long_height(0.1),
+      max_start_height(0.2),
+      sensor_height(0.2),
+      line_search_angle(0.2) {}
+
+  // Visualize estimated ground.
+  bool visualize;
+  // Minimum range of segmentation.
+  double r_min_square;
+  // Maximum range of segmentation.
+  double r_max_square;
+  // Number of radial bins.
+  int n_bins;
+  // Number of angular segments.
+  int n_segments;
+  // Maximum distance to a ground line to be classified as ground.
+  double max_dist_to_line;
+  // Max slope to be considered ground line.
+  double max_slope;
+  // Max error for line fit.
+  double max_error_square;
+  // Distance at which points are considered far from each other.
+  double long_threshold;
+  // Maximum slope for
+  double max_long_height;
+  // Maximum heigh of starting line to be labelled ground.
+  double max_start_height;
+  // Height of sensor above ground.
+  double sensor_height;
+  // How far to search for a line in angular direction [rad].
+  double line_search_angle;
+  // Number of threads.
+  int n_threads;
+};
+
+typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+
+typedef std::pair<pcl::PointXYZ, pcl::PointXYZ> PointLine;
+
+class GroundSegmentation {
+
+  const GroundSegmentationParams params_;
+
+  // Access with segments_[segment][bin].
+  std::vector<Segment> segments_;
+
+  // Bin index of every point.
+  std::vector<std::pair<int, int> > bin_index_;
+
+  // 2D coordinates (d, z) of every point in its respective segment.
+  std::vector<Bin::MinZPoint> segment_coordinates_;
+
+  // Visualizer.
+  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
+
+  void assignCluster(std::vector<int>* segmentation);
+
+  void assignClusterThread(const unsigned int& start_index,
+                           const unsigned int& end_index,
+                           std::vector<int>* segmentation);
+
+  void insertPoints(const PointCloud& cloud);
+
+  void insertionThread(const PointCloud& cloud,
+                       const size_t start_index,
+                       const size_t end_index);
+
+  void getMinZPoints(PointCloud* out_cloud);
+
+  void getLines(std::list<PointLine>* lines);
+
+  void lineFitThread(const unsigned int start_index, const unsigned int end_index,
+                     std::list<PointLine> *lines, std::mutex* lines_mutex);
+
+  pcl::PointXYZ minZPointTo3d(const Bin::MinZPoint& min_z_point, const double& angle);
+
+  void getMinZPointCloud(PointCloud* cloud);
+
+  void visualizePointCloud(const PointCloud::ConstPtr& cloud,
+                           const std::string& id = "point_cloud");
+
+  void visualizeLines(const std::list<PointLine>& lines);
+
+  void visualize(const std::list<PointLine>& lines, const PointCloud::ConstPtr& cloud, const PointCloud::ConstPtr& ground_cloud, const PointCloud::ConstPtr& obstacle_cloud);
+
+public:
+
+  GroundSegmentation(const GroundSegmentationParams& params = GroundSegmentationParams());
+
+  void segment(const PointCloud& cloud, std::vector<int>* segmentation);
+
+};
+
+#endif // GROUND_SEGMENTATION_H_
\ No newline at end of file

test_project/linefit_ground_segmentation_local_gpu_test/include/ground_segmentation/segment.h

+#ifndef GROUND_SEGMENTATION_SEGMENT_H_
+#define GROUND_SEGMENTATION_SEGMENT_H_
+
+#include <list>
+#include <map>
+
+#include "ground_segmentation/bin.h"
+
+class Segment {
+public:
+  typedef std::pair<Bin::MinZPoint, Bin::MinZPoint> Line;
+
+  typedef std::pair<double, double> LocalLine;
+
+private:
+  // Parameters. Description in GroundSegmentation.
+  const double max_slope_;
+  const double max_error_;
+  const double long_threshold_;
+  const double max_long_height_;
+  const double max_start_height_;
+  const double sensor_height_;
+
+  std::vector<Bin> bins_;
+
+  std::list<Line> lines_;
+
+  LocalLine fitLocalLine(const std::list<Bin::MinZPoint>& points);
+
+  double getMeanError(const std::list<Bin::MinZPoint>& points, const LocalLine& line);
+
+  double getMaxError(const std::list<Bin::MinZPoint>& points, const LocalLine& line);
+
+  Line localLineToLine(const LocalLine& local_line, const std::list<Bin::MinZPoint>& line_points);
+
+
+public:
+
+  Segment(const unsigned int& n_bins,
+          const double& max_slope,
+          const double& max_error,
+          const double& long_threshold,
+          const double& max_long_height,
+          const double& max_start_height,
+          const double& sensor_height);
+
+  double verticalDistanceToLine(const double& d, const double &z);
+
+  void fitSegmentLines();
+
+  inline Bin& operator[](const size_t& index) {
+    return bins_[index];
+  }
+
+  inline std::vector<Bin>::iterator begin() {
+    return bins_.begin();
+  }
+
+  inline std::vector<Bin>::iterator end() {
+    return bins_.end();
+  }
+
+  bool getLines(std::list<Line>* lines);
+
+};
+
+#endif /* GROUND_SEGMENTATION_SEGMENT_H_ */

test_project/linefit_ground_segmentation_local_gpu_test/src/AppUtility.hpp

+/**
+ * @file    AppUtility.hpp
+ *
+ * @author  btran
+ *
+ */
+
+#pragma once
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <utility>
+#include <vector>
+
+namespace
+{
+inline pcl::PointIndices::Ptr toPclPointIndices(const std::vector<int>& indices)
+{
+    pcl::PointIndices::Ptr pclIndices(new pcl::PointIndices);
+    pclIndices->indices = indices;
+
+    return pclIndices;
+}
+
+template <typename POINT_CLOUD_TYPE>
+inline std::vector<typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr>
+toClusters(const std::vector<std::vector<int>>& clusterIndices,
+           const typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr& inPcl)
+{
+    std::vector<typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr> clusters;
+    clusters.reserve(clusterIndices.size());
+
+    for (const auto& curIndices : clusterIndices) {
+        typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr curPcl(new pcl::PointCloud<POINT_CLOUD_TYPE>);
+        pcl::copyPointCloud(*inPcl, *toPclPointIndices(curIndices), *curPcl);
+        clusters.emplace_back(curPcl);
+    }
+
+    return clusters;
+}
+
+inline std::vector<std::array<std::uint8_t, 3>> generateColorCharts(const uint16_t numSources,
+                                                                    const std::uint16_t seed = 2020)
+{
+    std::srand(seed);
+
+    std::vector<std::array<std::uint8_t, 3>> colors;
+    colors.reserve(numSources);
+
+    for (std::uint16_t i = 0; i < numSources; ++i) {
+        colors.emplace_back(std::array<uint8_t, 3>{static_cast<uint8_t>(std::rand() % 256),
+                                                   static_cast<uint8_t>(std::rand() % 256),
+                                                   static_cast<uint8_t>(std::rand() % 256)});
+    }
+
+    return colors;
+}
+
+template <typename POINT_CLOUD_TYPE>
+inline std::pair<std::vector<std::array<std::uint8_t, 3>>,
+                 std::vector<pcl::visualization::PointCloudColorHandlerCustom<POINT_CLOUD_TYPE>>>
+initPclColorHandlers(const std::vector<typename pcl::PointCloud<POINT_CLOUD_TYPE>::Ptr>& inPcls)
+{
+    using PointCloudType = POINT_CLOUD_TYPE;
+    using PointCloud = pcl::PointCloud<PointCloudType>;
+
+    std::vector<pcl::visualization::PointCloudColorHandlerCustom<PointCloudType>> pclColorHandlers;
+    pclColorHandlers.reserve(inPcls.size());
+    auto colorCharts = ::generateColorCharts(inPcls.size());
+
+    for (int i = 0; i < inPcls.size(); ++i) {
+        const auto& curColor = colorCharts[i];
+        pclColorHandlers.emplace_back(pcl::visualization::PointCloudColorHandlerCustom<PointCloudType>(
+            inPcls[i], curColor[0], curColor[1], curColor[2]));
+    }
+
+    return std::make_pair(colorCharts, pclColorHandlers);
+}
+
+pcl::visualization::PCLVisualizer::Ptr initializeViewer()
+{
+    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
+    pcl::PointXYZ o(0.1, 0, 0);
+    viewer->addSphere(o, 0.1, "sphere", 0);
+    viewer->setBackgroundColor(0.05, 0.05, 0.05, 0);
+    viewer->addCoordinateSystem(0.5);
+    viewer->setCameraPosition(-26, 0, 3, 10, -1, 0.5, 0, 0, 1);
+
+    return viewer;
+}
+}  // namespace

test_project/linefit_ground_segmentation_local_gpu_test/src/CudaUtils.cu

+/**
+ * @file    CudaUtils.cu
+ *
+ * @author  btran
+ *
+ */
+
+#include <dbscan/CudaUtils.cuh>
+
+namespace cuda
+{
+namespace utils
+{
+namespace
+{
+__global__ void warmUpGPUKernel()
+{
+    int idx = blockIdx.x * blockDim.x + threadIdx.x;
+    ++idx;
+}
+}  // namespace
+
+cudaError_t warmUpGPU()
+{
+    warmUpGPUKernel<<<1, 1>>>();
+    cudaDeviceSynchronize();
+    return cudaGetLastError();
+}
+}  // namespace utils
+}  // namespace cuda

test_project/linefit_ground_segmentation_local_gpu_test/src/DBSCAN.cu

+/**
+ * @file    DBSCAN.cu
+ *
+ * @author  btran
+ *
+ */
+
+// #include <dbscan/CudaUtils.cuh>
+#include <dbscan/DBSCAN.cuh>
+
+#include <pcl/point_types.h>
+
+#include <thrust/device_vector.h>
+#include <thrust/host_vector.h>
+
+#include "dbscan/Types.cuh"
+#include "dbscan/CudaUtils.cuh"
+
+namespace clustering
+{
+namespace cuda
+{
+namespace
+{
+cudaDeviceProp dProperties;
+
+template <typename PointType>
+__global__ void makeGraphStep1Kernel(const PointType* __restrict__ points, Node* nodes, int* nodeDegs, int numPoints,
+                                     float eps, int minPoints)
+{
+    int tid = threadIdx.x + blockIdx.x * blockDim.x;
+
+    while (tid < numPoints) {
+        Node* node = &nodes[tid];
+        const PointType* point = &points[tid];
+
+        for (int i = 0; i < numPoints; ++i) {
+            if (i == tid) {
+                continue;
+            }
+
+            const PointType* curPoint = &points[i];
+            float diffx = point->x - curPoint->x;
+            float diffy = point->y - curPoint->y;
+            float diffz = point->z - curPoint->z;
+
+            float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
+
+            if (sqrDist < eps * eps) {
+                node->numNeighbors++;
+            }
+        }
+
+        if (node->numNeighbors >= minPoints) {
+            node->type = NodeType::CORE;
+        }
+
+        nodeDegs[tid] = node->numNeighbors;
+
+        tid += blockDim.x * gridDim.x;
+    }
+}
+
+template <typename PointType>
+__global__ void makeGraphStep2Kernel(const PointType* __restrict__ points, const int* __restrict__ neighborStartIndices,
+                                     int* adjList, int numPoints, float eps)
+{
+    int tid = threadIdx.x + blockIdx.x * blockDim.x;
+
+    while (tid < numPoints) {
+        const PointType* point = &points[tid];
+
+        int startIdx = neighborStartIndices[tid];
+
+        int countNeighbors = 0;
+        for (int i = 0; i < numPoints; ++i) {
+            if (i == tid) {
+                continue;
+            }
+
+            const PointType* curPoint = &points[i];
+            float diffx = point->x - curPoint->x;
+            float diffy = point->y - curPoint->y;
+            float diffz = point->z - curPoint->z;
+
+            float sqrDist = diffx * diffx + diffy * diffy + diffz * diffz;
+
+            if (sqrDist < eps * eps) {
+                adjList[startIdx + countNeighbors++] = i;
+            }
+        }
+
+        tid += blockDim.x * gridDim.x;
+    }
+}
+
+__global__ void BFSKernel(const Node* __restrict__ nodes, const int* __restrict__ adjList,
+                          const int* __restrict__ neighborStartIndices, char* Fa, char* Xa, int numPoints)
+{
+    int tid = threadIdx.x + blockIdx.x * blockDim.x;
+
+    while (tid < numPoints) {
+        if (Fa[tid]) {
+            Fa[tid] = false;
+            Xa[tid] = true;
+
+            int startIdx = neighborStartIndices[tid];
+
+            for (int i = 0; i < nodes[tid].numNeighbors; ++i) {
+                int nIdx = adjList[startIdx + i];
+                Fa[nIdx] = 1 - Xa[nIdx];
+            }
+        }
+
+        tid += blockDim.x * gridDim.x;
+    }
+}
+
+void BFS(Node* hNodes, Node* dNodes, const int* adjList, const int* neighborStartIndices, int v, int numPoints,
+         std::vector<int>& curCluster)
+{
+    thrust::device_vector<char> dXa(numPoints, false);
+    thrust::device_vector<char> dFa(numPoints, false);
+    dFa[v] = true;
+
+    int numThreads = 256;
+    int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
+
+    int countFa = 1;
+    while (countFa > 0) {
+        BFSKernel<<<numBlocks, numThreads>>>(dNodes, adjList, neighborStartIndices,
+                                             thrust::raw_pointer_cast(dFa.data()), thrust::raw_pointer_cast(dXa.data()),
+                                             numPoints);
+        countFa = thrust::count(thrust::device, dFa.begin(), dFa.end(), true);
+    }
+
+    thrust::host_vector<char> hXa = dXa;
+
+    for (int i = 0; i < numPoints; ++i) {
+        if (hXa[i]) {
+            hNodes[i].visited = true;
+            curCluster.emplace_back(i);
+        }
+    }
+}
+
+template <typename PointType> Graph makeGraph(const PointType* points, int numPoints, float eps, int minPoints)
+{
+    Graph graph;
+
+    graph.nodes.resize(numPoints);
+    graph.neighborStartIndices.resize(numPoints);
+
+    thrust::device_vector<int> dNodeDegs(numPoints);
+
+    int numThreads = 256;
+    int numBlocks = std::min(dProperties.maxGridSize[0], (numPoints + numThreads - 1) / numThreads);
+
+    makeGraphStep1Kernel<PointType><<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.nodes.data()),
+                                                               thrust::raw_pointer_cast(dNodeDegs.data()), numPoints,
+                                                               eps, minPoints);
+
+    thrust::exclusive_scan(dNodeDegs.begin(), dNodeDegs.end(), graph.neighborStartIndices.begin());
+
+    int totalEdges = dNodeDegs.back() + graph.neighborStartIndices.back();
+    graph.adjList.resize(totalEdges);
+
+    makeGraphStep2Kernel<PointType>
+        <<<numBlocks, numThreads>>>(points, thrust::raw_pointer_cast(graph.neighborStartIndices.data()),
+                                    thrust::raw_pointer_cast(graph.adjList.data()), numPoints, eps);
+
+    return graph;
+}
+}  // namespace
+
+template <typename PointType>
+DBSCAN<PointType>::DBSCAN(const Param& param)
+    : m_param(param)
+    , m_dPoints(nullptr)
+    , m_allocatedSize(-1)
+{
+    HANDLE_ERROR(cudaGetDeviceProperties(&dProperties, 0));
+    
+}
+
+template <typename PointType> DBSCAN<PointType>::~DBSCAN()
+{
+    if (m_dPoints) {
+        HANDLE_ERROR(cudaFree(m_dPoints));
+        m_dPoints = nullptr;
+    }
+}
+
+template <typename PointType>
+std::vector<std::vector<int>> DBSCAN<PointType>::run(const PointType* points, int numPoints) const
+{
+    if (numPoints <= 0) {
+        throw std::runtime_error("number of points must be more than 0");
+    }
+
+    if (m_allocatedSize < numPoints) {
+        m_allocatedSize = numPoints;
+        if (m_dPoints) {
+            HANDLE_ERROR(cudaFree(m_dPoints));
+        }
+        HANDLE_ERROR(cudaMalloc((void**)&m_dPoints, numPoints * sizeof(PointType)));
+    }
+
+    HANDLE_ERROR(cudaMemcpy(m_dPoints, points, numPoints * sizeof(PointType), cudaMemcpyHostToDevice));
+
+    auto graph = makeGraph<PointType>(m_dPoints, numPoints, m_param.eps, m_param.minPoints);
+
+    thrust::host_vector<Node> hNodes = graph.nodes;
+
+    std::vector<std::vector<int>> clusterIndices;
+    for (int i = 0; i < numPoints; ++i) {
+        auto& curHNode = hNodes[i];
+        if (curHNode.visited || curHNode.type != NodeType::CORE) {
+            continue;
+        }
+
+        std::vector<int> curCluster;
+        curCluster.emplace_back(i);
+        curHNode.visited = true;
+
+        BFS(hNodes.data(), thrust::raw_pointer_cast(graph.nodes.data()), thrust::raw_pointer_cast(graph.adjList.data()),
+            thrust::raw_pointer_cast(graph.neighborStartIndices.data()), i, numPoints, curCluster);
+
+        clusterIndices.emplace_back(std::move(curCluster));
+    }
+
+    return clusterIndices;
+}
+
+#undef INSTANTIATE_TEMPLATE
+#define INSTANTIATE_TEMPLATE(DATA_TYPE) template class DBSCAN<DATA_TYPE>;
+
+INSTANTIATE_TEMPLATE(pcl::PointXYZ);
+INSTANTIATE_TEMPLATE(pcl::PointXYZI);
+INSTANTIATE_TEMPLATE(pcl::PointXYZRGB);
+INSTANTIATE_TEMPLATE(pcl::PointNormal);
+INSTANTIATE_TEMPLATE(pcl::PointXYZRGBNormal);
+INSTANTIATE_TEMPLATE(pcl::PointXYZINormal);
+
+#undef INSTANTIATE_TEMPLATE
+}  // namespace cuda
+}  // namespace clustering

test_project/linefit_ground_segmentation_local_gpu_test/src/bin.cc

+#include "ground_segmentation/bin.h"
+
+#include <limits>
+
+Bin::Bin() : min_z(std::numeric_limits<double>::max()), has_point_(false) {}
+
+Bin::Bin(const Bin& bin) : min_z(std::numeric_limits<double>::max()),
+                                           has_point_(false) {}
+
+void Bin::addPoint(const pcl::PointXYZ& point) {
+  const double d = sqrt(point.x * point.x + point.y * point.y);
+  addPoint(d, point.z);
+}
+
+/*添加点*/
+void Bin::addPoint(const double& d, const double& z) {
+  has_point_ = true;
+  if (z < min_z) {
+    min_z = z;
+    min_z_range = d;
+  }
+}
+
+/*判断是否有点，如果有点存在的话，将我们之前算好的min_z和min_z_range传入到point之中*/
+Bin::MinZPoint Bin::getMinZPoint() {
+  MinZPoint point;
+
+  if (has_point_) {
+    point.z = min_z;
+    point.d = min_z_range;
+  }
+
+  return point;
+}

test_project/linefit_ground_segmentation_local_gpu_test/src/ground_segmentation.cc

+#include "ground_segmentation/ground_segmentation.h"
+
+#include <chrono>
+#include <cmath>
+#include <list>
+#include <memory>
+#include <thread>
+
+/*可视化点云*/
+void GroundSegmentation::visualizePointCloud(const PointCloud::ConstPtr& cloud,
+                                             const std::string& id) {
+  viewer_->addPointCloud(cloud, id, 0);  //   /*将点云和id添加到可视化显示器之中*/
+}
+/*显示地面线*/
+void GroundSegmentation::visualizeLines(const std::list<PointLine>& lines) {
+  size_t counter = 0;
+  /*遍历显示地面线*/
+  for (auto it = lines.begin(); it != lines.end(); ++it) {
+    viewer_->addLine<pcl::PointXYZ>(it->first, it->second, std::to_string(counter++));
+  }
+}
+
+/*
+  可视化:
+  在这里，我们将pcl_viewer中需要设定的内容都设定好
+*/
+void GroundSegmentation::visualize(const std::list<PointLine>& lines,
+                                   const PointCloud::ConstPtr& min_cloud,
+                                   const PointCloud::ConstPtr& ground_cloud,
+                                   const PointCloud::ConstPtr& obstacle_cloud) {
+  viewer_->setBackgroundColor (0, 0, 0);
+  viewer_->addCoordinateSystem (1.0);
+  viewer_->initCameraParameters ();
+  viewer_->setCameraPosition(-2.0, 0, 2.0, 1.0, 0, 0);
+  visualizePointCloud(min_cloud, "min_cloud");
+  viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,
+                                             0.0f, 1.0f, 0.0f,
+                                             "min_cloud");
+  viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
+                                             2.0f,
+                                             "min_cloud");
+  visualizePointCloud(ground_cloud, "ground_cloud");
+  viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,
+                                             1.0f, 0.0f, 0.0f,
+                                             "ground_cloud");
+  visualizePointCloud(obstacle_cloud, "obstacle_cloud");
+  visualizeLines(lines);
+  while (!viewer_->wasStopped ()){
+      viewer_->spinOnce (100);
+      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+  }
+}
+
+/*地面分割的构造函数*/
+GroundSegmentation::GroundSegmentation(const GroundSegmentationParams& params) :
+    params_(params),
+    segments_(params.n_segments, Segment(params.n_bins,
+                                         params.max_slope,
+                                         params.max_error_square,
+                                         params.long_threshold,
+                                         params.max_long_height,
+                                         params.max_start_height,
+                                         params.sensor_height)) {
+  if (params.visualize) viewer_ = std::make_shared<pcl::visualization::PCLVisualizer>("3D Viewer");
+}
+
+/*地面分割的分割函数*/
+void GroundSegmentation::segment(const PointCloud& cloud, std::vector<int>* segmentation) {
+  /*初始化一些比较基础的东西*/
+  // std::cout << "Segmenting cloud with " << cloud.size() << " points...\n";
+  std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();
+  segmentation->clear();
+  segmentation->resize(cloud.size(), 0);
+  bin_index_.resize(cloud.size());
+  segment_coordinates_.resize(cloud.size());
+
+  /*插入点云数据*/
+  insertPoints(cloud);
+  /*定义地面点的线的参数*/
+  std::list<PointLine> lines;
+  /*根据我们的设定来决定是否进行可视化的展示*/
+  if (params_.visualize) {
+    getLines(&lines);
+  }
+  else {
+    getLines(NULL);
+  }
+  /*对于传入的分割进行细分*/
+  /*从这里可以看到对于点云属于障碍物还是属于地面点进行了标签的划分*/
+  assignCluster(segmentation);
+  std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
+  // /*如果是进行了可视化的操作，则进行一下的操作*/
+  // if (params_.visualize) {
+  //   // Visualize.
+  //   PointCloud::Ptr obstacle_cloud(new PointCloud());
+  //   // Get cloud of ground points.
+  //   PointCloud::Ptr ground_cloud(new PointCloud());
+  //   for (size_t i = 0; i < cloud.size(); ++i) {
+  //     if (segmentation->at(i) == 1) ground_cloud->push_back(cloud[i]);
+  //     else obstacle_cloud->push_back(cloud[i]);
+  //   }
+  //   PointCloud::Ptr min_cloud(new PointCloud());
+  //   getMinZPointCloud(min_cloud.get());
+  //   pcl::io::savePCDFile("src/linefit_ground_segmentation_ros/data/m1_ground_cloud.pcd", *ground_cloud);
+  //   pcl::io::savePCDFile("src/linefit_ground_segmentation_ros/data/m1_obstacle_cloud.pcd", *obstacle_cloud);
+  //   // visualize(lines, min_cloud, ground_cloud, obstacle_cloud);
+
+  // }
+  /*如果是进行了可视化的操作，则进行一下的操作*/
+  if (0) {
+    // Visualize.
+    PointCloud::Ptr obstacle_cloud(new PointCloud());
+    // Get cloud of ground points.
+    PointCloud::Ptr ground_cloud(new PointCloud());
+    for (size_t i = 0; i < cloud.size(); ++i) {
+      if (segmentation->at(i) == 1) ground_cloud->push_back(cloud[i]);
+      else obstacle_cloud->push_back(cloud[i]);
+    }
+    // PointCloud::Ptr min_cloud(new PointCloud());
+    // getMinZPointCloud(min_cloud.get());
+    std::cout << "ground_cloud point size = " << ground_cloud->points.size() << std::endl;
+    pcl::io::savePCDFileASCII("/home/nvidia/lw/catkin_ws_road_seg/m1_ground_cloud.pcd", *ground_cloud);
+    pcl::io::savePCDFileASCII("/home/nvidia/lw/catkin_ws_road_seg/m1_obstacle_cloud.pcd", *obstacle_cloud);
+    // visualize(lines, min_cloud, ground_cloud, obstacle_cloud);
+
+  }
+  // std::chrono::high_resolution_clock::time_point vis_end = std::chrono::high_resolution_clock::now();
+  // std::chrono::duration<double, std::milli> fp_ms = end - start;
+  // std::chrono::duration<double, std::milli> vis_ms = vis_end - start;
+  // std::cout << "Done! Took " << fp_ms.count() << "ms\n";
+  // std::cout << "Plus Vis Took " << vis_ms.count() << "ms\n";
+}
+
+/*获取到线*/
+void GroundSegmentation::getLines(std::list<PointLine> *lines) {
+  std::mutex line_mutex;
+  std::vector<std::thread> thread_vec(params_.n_threads);
+  unsigned int i;
+  for (i = 0; i < params_.n_threads; ++i) {
+    const unsigned int start_index = params_.n_segments / params_.n_threads * i;
+    const unsigned int end_index = params_.n_segments / params_.n_threads * (i+1);
+    thread_vec[i] = std::thread(&GroundSegmentation::lineFitThread, this,
+                                start_index, end_index, lines, &line_mutex);
+  }
+  for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) {
+    it->join();
+  }
+}
+
+/*这里是获取线的操作*/
+void GroundSegmentation::lineFitThread(const unsigned int start_index,
+                                       const unsigned int end_index,
+                                       std::list<PointLine> *lines, std::mutex* lines_mutex) {
+  const bool visualize = lines;
+  const double seg_step = 2*M_PI / params_.n_segments;
+  double angle = -M_PI + seg_step/2 + seg_step * start_index;
+  for (unsigned int i = start_index; i < end_index; ++i) {
+    segments_[i].fitSegmentLines();
+    // Convert lines to 3d if we want to.
+    /*这里也是可视化的一些操作*/
+    if (visualize) {
+      std::list<Segment::Line> segment_lines;
+      segments_[i].getLines(&segment_lines);
+      for (auto line_iter = segment_lines.begin(); line_iter != segment_lines.end(); ++line_iter) {
+        const pcl::PointXYZ start = minZPointTo3d(line_iter->first, angle);
+        const pcl::PointXYZ end = minZPointTo3d(line_iter->second, angle);
+        lines_mutex->lock();
+        lines->emplace_back(start, end);
+        lines_mutex->unlock();
+      }
+
+      angle += seg_step;
+    }
+  }
+}
+
+void GroundSegmentation::getMinZPointCloud(PointCloud* cloud) {
+  const double seg_step = 2*M_PI / params_.n_segments;
+  double angle = -M_PI + seg_step/2;
+  for (auto seg_iter = segments_.begin(); seg_iter != segments_.end(); ++seg_iter) {
+    for (auto bin_iter = seg_iter->begin(); bin_iter != seg_iter->end(); ++bin_iter) {
+      const pcl::PointXYZ min = minZPointTo3d(bin_iter->getMinZPoint(), angle);
+      cloud->push_back(min);
+    }
+
+    angle += seg_step;
+  }
+}
+
+/*根据传入的二维点，也可以通过算出的angle将二维点转化为三维点，主要是x-y平面内的变换*/
+pcl::PointXYZ GroundSegmentation::minZPointTo3d(const Bin::MinZPoint &min_z_point,
+                                                const double &angle) {
+  pcl::PointXYZ point;
+  point.x = cos(angle) * min_z_point.d;
+  point.y = sin(angle) * min_z_point.d;
+  point.z = min_z_point.z;
+  return point;
+}
+
+/*分配集群，将传入的分割进行簇的划分*/
+void GroundSegmentation::assignCluster(std::vector<int>* segmentation) {
+  std::vector<std::thread> thread_vec(params_.n_threads);
+  const size_t cloud_size = segmentation->size();
+  for (unsigned int i = 0; i < params_.n_threads; ++i) {
+    const unsigned int start_index = cloud_size / params_.n_threads * i;
+    const unsigned int end_index = cloud_size / params_.n_threads * (i+1);
+    thread_vec[i] = std::thread(&GroundSegmentation::assignClusterThread, this,
+                                start_index, end_index, segmentation);
+  }
+  for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) {
+    it->join();
+  }
+}
+
+/*执行分配集群的线程操作*/
+void GroundSegmentation::assignClusterThread(const unsigned int &start_index,
+                                             const unsigned int &end_index,
+                                             std::vector<int> *segmentation) {
+  /*通过传入的segments的个数，得到划分segment的步长，也就是δ值*/
+  const double segment_step = 2*M_PI/params_.n_segments;
+  /*进行遍历操作*/
+  /*对于每一个点进行处理，根据距离判断是否属于远离地面的点*/
+  for (unsigned int i = start_index; i < end_index; ++i) {
+    /*首先，得到每一个三维点的二维映射*/
+    Bin::MinZPoint point_2d = segment_coordinates_[i];
+    /*得到第i个bin的第一个值，作为分割的索引*/
+    const int segment_index = bin_index_[i].first;
+    /*判定分割的index是需要大于0的*/
+    if (segment_index >= 0) {
+      /*计算处两个点到线的垂直距离*/
+      /*将点投影到直线上*/
+      double dist = segments_[segment_index].verticalDistanceToLine(point_2d.d, point_2d.z);
+      // Search neighboring segments.
+      /*搜索相邻的分割*/
+      int steps = 1;
+      /*根据划分好的segment来不断的进行数据的处理*/
+      /*在设定的步长的情况下在搜索框内能有几个步长*/
+      while (dist == -1 && steps * segment_step < params_.line_search_angle) {
+        // Fix indices that are out of bounds.
+        /*修复超出范围的索引*/
+        int index_1 = segment_index + steps;//进行正向步长的搜索
+        while (index_1 >= params_.n_segments) index_1 -= params_.n_segments;
+        int index_2 = segment_index - steps;//进行反向步长的索引
+        while (index_2 < 0) index_2 += params_.n_segments;
+        // Get distance to neighboring lines.
+        /*算出根据正反双向最近搜索的点到线段投影的距离*/
+        const double dist_1 = segments_[index_1].verticalDistanceToLine(point_2d.d, point_2d.z);
+        const double dist_2 = segments_[index_2].verticalDistanceToLine(point_2d.d, point_2d.z);
+        // Select larger distance if both segments return a valid distance.
+        /*如果两个分割都返回有效距离，则选择更大的距离*/
+        if (dist_1 > dist) {
+          dist = dist_1;
+        }
+        if (dist_2 > dist) {
+          dist = dist_2;
+        }
+        /*不断增加步长，一直持续下去，直到跳出循环，这样可以保证比较公平的遍历到里面的点*/
+        ++steps;
+      }
+      /*这里是进行标签的设定*/
+      if (dist < params_.max_dist_to_line && dist != -1) {
+        /*这里是对于是否属于地面点的判定*/
+        segmentation->at(i) = 1;
+      }
+    }
+  }
+}
+
+/*获取最小z点的点云数据*/
+void GroundSegmentation::getMinZPoints(PointCloud* out_cloud) {
+  /*得到分割的步长，以及bins的步长*/
+  const double seg_step = 2*M_PI / params_.n_segments;
+  const double bin_step = (sqrt(params_.r_max_square) - sqrt(params_.r_min_square))
+      / params_.n_bins;
+  /*得到最小的r*/
+  const double r_min = sqrt(params_.r_min_square);
+  double angle = -M_PI + seg_step/2;
+  for (auto seg_iter = segments_.begin(); seg_iter != segments_.end(); ++seg_iter) {
+    double dist = r_min + bin_step/2;
+    for (auto bin_iter = seg_iter->begin(); bin_iter != seg_iter->end(); ++bin_iter) {
+      pcl::PointXYZ point;
+      if (bin_iter->hasPoint()) {
+        /*对于bin_iter进行最小z点的提取*/
+        Bin::MinZPoint min_z_point(bin_iter->getMinZPoint());
+        point.x = cos(angle) * min_z_point.d;
+        point.y = sin(angle) * min_z_point.d;
+        point.z = min_z_point.z;
+        /*将point放入到out_cloud之中*/
+        out_cloud->push_back(point);
+      }
+      /*按照步长增加dist*/
+      dist += bin_step;
+    }
+    /*按照划分的步长进行角度的增加*/
+    angle += seg_step;
+  }
+}
+
+
+/*插入点云*/
+void GroundSegmentation::insertPoints(const PointCloud& cloud) {
+  std::vector<std::thread> threads(params_.n_threads);
+  const size_t points_per_thread = cloud.size() / params_.n_threads;
+  // Launch threads.
+  /*根据我们设定的数目来将整个的点云分为几个部分开始处理，利用多线程来处理*/
+  for (unsigned int i = 0; i < params_.n_threads - 1; ++i) {
+    const size_t start_index = i * points_per_thread;
+    const size_t end_index = (i+1) * points_per_thread - 1;
+    threads[i] = std::thread(&GroundSegmentation::insertionThread, this,
+                             cloud, start_index, end_index);
+  }
+  // Launch last thread which might have more points than others.
+  /*启动最后一个可能含有更多点云的线程*/
+  const size_t start_index = (params_.n_threads - 1) * points_per_thread;
+  const size_t end_index = cloud.size() - 1;
+  threads[params_.n_threads - 1] =
+      std::thread(&GroundSegmentation::insertionThread, this, cloud, start_index, end_index);
+  // Wait for threads to finish.
+  for (auto it = threads.begin(); it != threads.end(); ++it) {
+    it->join();
+  }
+}
+
+/*线程启动中会执行的函数*/
+void GroundSegmentation::insertionThread(const PointCloud& cloud,
+                                         const size_t start_index,
+                                         const size_t end_index) {
+  /*同样的先算步长，然后再进行其他的操作*/
+  const double segment_step = 2*M_PI / params_.n_segments;
+  const double bin_step = (sqrt(params_.r_max_square) - sqrt(params_.r_min_square))
+      / params_.n_bins;
+  const double r_min = sqrt(params_.r_min_square);
+  /*对于起始索引和终止索引进行遍历*/
+  for (unsigned int i = start_index; i < end_index; ++i) {
+    pcl::PointXYZ point(cloud[i]);
+    /*这里是算模长*/
+    const double range_square = point.x * point.x + point.y * point.y;
+    const double range = sqrt(range_square);
+    /*判断模场是否属于最小值和最大值之间*/
+    if (range_square < params_.r_max_square && range_square > params_.r_min_square) {
+       /*得到角度*/
+      const double angle = std::atan2(point.y, point.x);
+      /*根据模场和角度来算出bin的索引以及划分的索引*/
+      const unsigned int bin_index = (range - r_min) / bin_step;
+      const unsigned int segment_index = (angle + M_PI) / segment_step;
+      const unsigned int segment_index_clamped = segment_index == params_.n_segments ? 0 : segment_index;
+      /*对于设定的属于bin和划分中的集合进行数据的添加*/
+      segments_[segment_index_clamped][bin_index].addPoint(range, point.z);
+      /*将后面的数据作为一个元组全部传递到bin_index之中*/
+      bin_index_[i] = std::make_pair(segment_index_clamped, bin_index);
+    }
+    else {
+      bin_index_[i] = std::make_pair<int, int>(-1, -1);
+    }
+    /*获取到划分坐标为最小z点的坐标和range*/
+    segment_coordinates_[i] = Bin::MinZPoint(range, point.z);
+  }
+}

test_project/linefit_ground_segmentation_local_gpu_test/src/ground_segmentation_node.cc

+#include <pcl/io/ply_io.h>
+// #include <pcl_ros/point_cloud.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/segmentation/progressive_morphological_filter.h>
+#include <pcl/common/common.h>
+#include <pcl/common/time.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl/common/transforms.h>
+#include <yaml-cpp/yaml.h>
+#include <cuda_runtime.h>
+#include "ground_segmentation/ground_segmentation.h"
+#include <dbscan/CudaUtils.cuh>
+#include <dbscan/DBSCAN.cuh>
+#include "AppUtility.hpp"
+
+
+#define POINTS_TO_FILE  // 定义宏
+
+namespace
+{
+  using PointCloudType = pcl::PointXYZ;
+  using PointCloud = pcl::PointCloud<PointCloudType>;
+  using PointCloudPtr = PointCloud::Ptr;
+  using DBSCAN = clustering::cuda::DBSCAN<PointCloudType>;
+
+  auto timer = pcl::StopWatch();
+  int NUM_TEST = 20;
+}  // namespace
+
+
+
+template <typename T = double> struct CudaTimer {
+  CudaTimer() {
+    //cudaDeviceSynchronize();
+    mCurTime = std::chrono::steady_clock::now();
+  }
+
+  T ticking() {
+    //cudaDeviceSynchronize();
+    auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now() - mCurTime);
+    T res = duration.count();
+    mCurTime = std::chrono::steady_clock::now();
+    return res * 1e-6;
+  }
+
+  private:
+    std::chrono::time_point<std::chrono::steady_clock> mCurTime;
+};
+
+// 定义包围盒信息
+struct BoxInfo {
+  Eigen::Vector3f center;
+  Eigen::Vector3f size;
+  Eigen::Quaternionf orientation;
+};
+
+void writeClusterToFile(const std::vector<PointCloudPtr>& clusters, const std::string& filename)
+{
+    //std::ofstream file(filename);
+    std::ofstream file(filename, std::ios::app);
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+    int index = 1;
+
+    for (auto& cluster : clusters) {
+        file << "cluster " << index << std::endl;
+        for (auto& point : cluster->points) {
+            file << point.x << ", " << point.y << ", " << point.z << std::endl;
+        }
+        ++index;
+    }
+    file.close();
+}
+
+void writeGroundPointToFile(pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_ground_cloud, const std::string& filename)
+{   
+    int index = 0;
+
+    std::ofstream file(filename);
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+    std::cout << "File created" << std::endl;
+    file << "cluster " << index << std::endl;
+   
+
+
+    // 遍历点云中的每个点
+    for (const auto& point : filter_ground_cloud->points)
+    {
+      // 以指定格式将点的坐标写入到文件中
+      file << point.x << ", " << point.y << ", " << point.z << endl;
+    }
+    file.close();
+}
+
+
+// for robosence solid lidar m1
+namespace robosence_ros {
+    struct EIGEN_ALIGN16 Point {
+        double timestamp;
+        PCL_ADD_POINT4D;
+        uint8_t intensity;
+        uint16_t ring;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+}  // namespace robosence_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(robosence_ros::Point,
+    (double, timestamp, timestamp)
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (uint8_t, intensity, intensity)
+    (uint16_t, ring, ring)
+)
+typedef pcl::PointXYZ PointType;
+typedef pcl::PointCloud<PointType> PointCloudXYZI;
+
+
+
+/*我们定义的分割节点的类*/
+class SegmentationNode {
+
+  GroundSegmentationParams params_;//地面分割的参数
+
+public:
+  SegmentationNode(const GroundSegmentationParams& params) : params_(params) {
+  
+  }
+
+  void getBoundingBoxInfo(PointCloudPtr cloud_cluster, Eigen::Vector3f& boxCenter, Eigen::Vector3f& size,Eigen::Quaternionf& boxOrientation)
+{   
+    //PCA 本质就是基于坐标系的转化
+
+    //5. 计算点云的几何中心和重心，注意这两个概念不一样，重心是所有点的平均值，中心是最大最小点的平均值
+    Eigen::Vector4f centroid;     // 重心
+    pcl::compute3DCentroid(*cloud_cluster, centroid);
+    PointType min_pt, max_pt;
+    Eigen::Vector3f center;       // 中心
+    pcl::getMinMax3D(*cloud_cluster, min_pt, max_pt); 
+    center = (max_pt.getVector3fMap() + min_pt.getVector3fMap())/2;
+    // 6. 计算协方差矩阵的特征向量
+    Eigen::Matrix3f covariance;
+    pcl::computeCovarianceMatrixNormalized(*cloud_cluster, centroid, covariance); // 这里必须用重心 重心作为PCA新坐标系的原点
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
+    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
+    Eigen::Vector3f eigenValuesPCA  = eigen_solver.eigenvalues();
+    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //正交化
+    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
+    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
+    // 按照特征值从大到小排列（eigenvalues函数返回的特征值是从小到大排列的）
+    Eigen::Matrix3f eigenVectorsPCA1;
+    eigenVectorsPCA1.col(0) = eigenVectorsPCA.col(2);
+    eigenVectorsPCA1.col(1) = eigenVectorsPCA.col(1);
+    eigenVectorsPCA1.col(2) = eigenVectorsPCA.col(0);
+    eigenVectorsPCA = eigenVectorsPCA1;
+
+    // 7. 计算变换矩阵，只考虑绕全局坐标系Z轴的转动
+    Eigen::Vector3f ea = (eigenVectorsPCA).eulerAngles(2, 1, 0); // 分别对应 yaw pitch roll
+    Eigen::AngleAxisf keep_Z_Rot(ea[0], Eigen::Vector3f::UnitZ());
+    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+    transform.translate(center);  // translate与rotate书写的顺序不可搞反
+    transform.rotate(keep_Z_Rot); // radians
+    
+    // 8. 计算包围盒的尺寸
+    pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
+    pcl::transformPointCloud(*cloud_cluster, *transformedCloud, transform.inverse());
+    // 重新计算变换后的点云的中心，因为变换前的点云的中心不是其包围盒的几何中心
+    PointType min_pt_T, max_pt_T;
+    pcl::getMinMax3D(*transformedCloud, min_pt_T, max_pt_T);
+    Eigen::Vector3f center_new = (max_pt_T.getVector3fMap() + min_pt_T.getVector3fMap()) / 2;
+    Eigen::Vector3f box_dim;
+    box_dim = max_pt_T.getVector3fMap() - min_pt_T.getVector3fMap();
+    Eigen::Affine3f transform2 = Eigen::Affine3f::Identity();
+    transform2.translate(center_new);
+    Eigen::Affine3f transform3 = transform * transform2;
+
+    size = box_dim;
+    boxCenter = transform3.translation(); // 这里用新的"中心"，因为要显示的包围盒采用几何中心作为参考点
+    boxOrientation = Eigen::Quaternionf(keep_Z_Rot);
+  
+}
+
+  void detectFod(pcl::PointCloud<pcl::PointXYZ>::Ptr& ground_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& obstacle_cloud,  pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_clustered, const std::string& filename,const std::string& number_str)
+  {
+
+    cloud_clustered->header = ground_cloud->header;
+
+    //auto t = CudaTimer<>();
+ 
+    // 2: 获取ground_cloud外接凸包
+    // pcl::ConvexHull<pcl::PointXYZ> hull;                  //创建凸包对象
+    // hull.setInputCloud(ground_cloud);                  //设置输入点云
+    // hull.setDimension(2);                                 //设置凸包维度
+    // std::vector<pcl::Vertices> polygons;                  //设置向量，用于保存凸包定点
+    // pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);//该点运用于描述凸包形状
+    // hull.reconstruct(*surface_hull, polygons);            //计算2D凸包结果
+
+    // int mid = surface_hull->points.size() / 2;
+    // if (mid-3 >= 0 && mid+3 < surface_hull->points.size())
+    // {
+    //   surface_hull->points[mid-3].x = 150;
+    //   surface_hull->points[mid-2].x = 150;
+    //   surface_hull->points[mid-1].x = 150;
+    //   surface_hull->points[mid].x = 150;
+    //   surface_hull->points[mid+3].x = 150;
+    //   surface_hull->points[mid+2].x = 150;
+    //   surface_hull->points[mid+1].x = 150;
+    // }
+
+    /*
+    for(int i = 0; i < surface_hull->points.size(); i++)
+    {
+      pcl::PointXYZ tmp;
+      tmp.x = surface_hull->points[i].x;
+      tmp.y = surface_hull->points[i].y;
+      tmp.z = surface_hull->points[i].z;
+      std::cout << "x = " << tmp.x << " y = " << tmp.y << " z = " << tmp.z << std::endl;
+
+    }
+    */
+
+    //std::cout << " " << std::endl;
+    // 3: 根据凸包对obstacle_cloud进行过滤，保留路面上的点云
+    printf("=> obstacle_cloud points size : %d  \n", obstacle_cloud->points.size());
+    
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
+
+    objects = obstacle_cloud ;
+    /* //liuji -  凸包
+    pcl::CropHull<pcl::PointXYZ> bb_filter;               //创建crophull对象
+    bb_filter.setDim(2);                                  //设置维度：该维度需要与凸包维度一致
+    bb_filter.setInputCloud(obstacle_cloud);              //设置需要滤波的点云
+    bb_filter.setHullIndices(polygons);                   //输入封闭多边形的顶点
+    bb_filter.setHullCloud(surface_hull);                 //输入封闭多边形的形状
+    bb_filter.filter(*objects);                           //执行CropHull滤波，存出结果在objects
+    */
+   
+
+
+    // std::cout << objects->size() << std::endl;            //
+    //auto k1= t.ticking();
+    //printf("=> get convex hull : %.2f ms \n", k1);
+    // 基于距离将点云分成两个，分别进行滤波
+    printf("=>obstacle_cloud crophull points size : %d  \n", objects->points.size());
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr far (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close (new pcl::PointCloud<pcl::PointXYZ>);
+
+    for(int i = 0; i < objects->points.size(); i++)
+    {
+      if (objects->points[i].x < 1000)
+      {
+        if (objects->points[i].z > -1.0)
+          close->points.push_back(objects->points[i]);
+      }
+      else
+        far->points.push_back(objects->points[i]);
+    }
+
+    printf("=>obstacle_cloud voxel before close points size : %d  \n", close->points.size());
+    //auto k2= t.ticking();
+    //printf("=> get close cloud : %.2f ms \n", k2);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::VoxelGrid<pcl::PointXYZ> vor;  //滤波处理对象
+    vor.setInputCloud(close);
+    vor.setLeafSize(0.10f, 0.10f, 0.10f);  //设置滤波器处理时采用的体素大小的参数
+    vor.filter(*close_voxel_filtered);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    vor.filter(*close_objects_filtered);
+    //auto k3= t.ticking();
+    //printf("=> Voxel close cloud : %.2f ms \n", k3);
+    // 4: 获取点云目标检测结果，对路面点云进行目标过滤
+    /* //liuji  注释
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr far_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> ssor;//创建滤波器对象
+    ssor.setInputCloud(close_voxel_filtered);                       //设置待滤波的点云
+    ssor.setMeanK(50);                                 //设置在进行统计时考虑查询点临近点数
+    ssor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值
+    ssor.filter(*close_objects_filtered);                    //存储
+    */
+
+    //auto k4= t.ticking();
+    //printf("=> OutlierRemoval close cloud : %.2f ms \n", k4);
+    //pcl::StatisticalOutlierRemoval<pcl::PointXYZ> fsor;//创建滤波器对象
+    //fsor.setInputCloud(far);                       //设置待滤波的点云
+    //fsor.setMeanK(5);                                 //设置在进行统计时考虑查询点临近点数
+    //fsor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值
+    //fsor.filter(*far_objects_filtered);                    //存储
+    printf("=>close_voxel_filtered  points  size : %d  \n", close_voxel_filtered->points.size());
+
+    if (close_objects_filtered->points.size() < 10)
+      return;
+    
+    
+    // 5: 对obstacle_cloud进行dbscan聚类，得到每个障碍物
+
+    auto t = CudaTimer<>();
+
+    int numPoints = close_objects_filtered->points.size();
+    printf("=>close_objects_filtered voxel(dbscan before) after points  size : %d  \n", numPoints);
+   
+    PointType* points = close_objects_filtered->points.data();
+    DBSCAN::Param param{.pointDimension = 3, .eps = 0.8, .minPoints = 100};
+    DBSCAN dbscanHandler(param);
+    std::vector<std::vector<int>> clusterIndices;
+
+    clusterIndices = dbscanHandler.run(points, numPoints);
+    std::vector<PointCloudPtr> clusters = ::toClusters<PointCloudType>(clusterIndices, close_objects_filtered);
+    auto k = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k);
+/* 原始cpu DBSCAN算法使用的是 KdTree
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(close_objects_filtered);
+    // Segmentation, [Euclidean Cluster Extraction](https://pcl.readthedocs.io/projects/tutorials/en/latest/cluster_extraction.html#cluster-extraction)
+    std::vector<pcl::PointIndices> cluster_indices;
+    auto k5= t.ticking();
+    //printf("=> KdTree close cloud : %.2f ms \n", k5);
+    DBSCANKdtreeCluster ec;
+    ec.setCorePointMinPts(20);//一个核心点邻域最少点数
+    ec.setClusterTolerance(0.8);  //算法半径
+    ec.setMinClusterSize(100);   //一个簇最少点数
+	//ec.setMinClusterSize(10);
+    ec.setMaxClusterSize(99999);  //一个簇最大点数
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(close_objects_filtered);
+	//ec.setInputCloud(close_voxel_filtered);
+    ec.extract(cluster_indices);
+    auto k7= t.ticking();
+    //printf("=> extract close cloud : %.2f ms \n", k7);
+    auto k6 = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k6);
+    printf("=> cluster_indices size : %.d \n", cluster_indices.size());
+*/
+
+
+/* 对每个簇进行处理，将point的值获取到
+    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++) {
+      for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
+            pcl::PointXYZ tmp;
+            tmp.x = close_objects_filtered->points[*pit].x;
+            tmp.y = close_objects_filtered->points[*pit].y;
+            tmp.z = close_objects_filtered->points[*pit].z;
+            // tmp.intensity = j%8;
+            // tmp.ring = objects->points[*pit].ring;
+            if (tmp.z < -1.4 || tmp.z > 4.2)
+              continue;
+            cloud_clustered->points.push_back(tmp);
+      }
+    }
+
+*/
+    //该循坏只是把聚类后的点云获取到，并没有去做分类处理
+    printf("=> clusters size : %.d \n", clusters.size());
+    auto k1 = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k);
+    std::string bbox_txt = "../result/clusters_bbox_" + number_str + ".txt" ;
+    ofstream outfile(bbox_txt);
+    for (int i  = 0; i < clusters.size(); i++) {
+      #ifdef POINTS_TO_FILE
+      Eigen::Vector3f boxCenter, boxSize;
+      Eigen::Quaternionf boxOrientation;
+      getBoundingBoxInfo(clusters[i], boxCenter, boxSize, boxOrientation);
+      std::cout << "cluster: " << i << "\n";
+      std::cout  << "box center: " << boxCenter.transpose() << "\n";
+      std::cout  << "box size: " << boxSize.transpose() << "\n";
+      std::cout  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n";
+      outfile << "cluster: " << i << "\n";
+      outfile  << "box center: " << boxCenter.transpose() << "\n";
+      outfile  << "box size: " << boxSize.transpose() << "\n";
+      outfile  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n";
+      #endif
+      for (auto it = clusters[i]->begin(); it !=clusters[i]->end(); ++it){
+        pcl::PointXYZ tmp;
+        tmp.x = it->x;
+        tmp.y = it->y;
+        tmp.z = it->z;
+        if (tmp.z < -1.4 || tmp.z > 4.2)
+                  continue;
+        cloud_clustered->points.push_back(tmp);
+      }
+    }
+    outfile.close();
+
+    printf("=> final obstacle_cloud clustered points size : %.d \n", cloud_clustered->points.size());
+    #ifdef POINTS_TO_FILE
+    writeClusterToFile(clusters,filename);
+    #endif
+
+    for(int i = 0; i < far->points.size(); i++)
+    {
+      if (far->points[i].z < -1.4 || far->points[i].z > 4.2)
+        continue;
+      cloud_clustered->points.push_back(far->points[i]);
+    }
+    //printf("=> processs result : %.2f ms \n", k1);
+    // std::cout<<"cluster size = "<<cloud_clustered->points.size()<<endl;
+    
+  }
+
+  void processGround(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_ground_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_ground_cloud,const std::string& filename)
+  {
+
+    // // 1. 过滤离群点
+    // pcl::PointCloud<pcl::PointXYZ>::Ptr outlier_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    // pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;//创建滤波器对象
+    // sor.setInputCloud(source_ground_cloud); //设置待滤波的点云
+    // sor.setMeanK(50);     //设置在进行统计时考虑查询点临近点数
+    // sor.setStddevMulThresh(1.0);      //设置判断是否为离群点的阀值
+    // sor.filter(*outlier_filtered);      //存储
+ 
+    // 体素滤波
+    pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::VoxelGrid<pcl::PointXYZ> ssor;  //滤波处理对象
+    ssor.setInputCloud(source_ground_cloud);
+    ssor.setLeafSize(0.15f, 0.15f, 0.15f);  //设置滤波器处理时采用的体素大小的参数
+    // ssor.filter(*voxel_filtered);
+	  ssor.filter(*filter_ground_cloud);
+    #ifdef POINTS_TO_FILE
+    writeGroundPointToFile(filter_ground_cloud,filename);
+    #endif
+	/*
+    std::chrono::high_resolution_clock::time_point s2 = std::chrono::high_resolution_clock::now();
+    // 欧式聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(voxel_filtered);
+    // Segmentation, [Euclidean Cluster Extraction](https://pcl.readthedocs.io/projects/tutorials/en/latest/cluster_extraction.html#cluster-extraction)
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+
+    ec.setClusterTolerance(0.8);  //算法半径
+    ec.setMinClusterSize(100);   //一个簇最少点数
+    ec.setMaxClusterSize(99999);  //一个簇最大点数
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(voxel_filtered);
+    ec.extract(cluster_indices);
+    std::chrono::high_resolution_clock::time_point s3 = std::chrono::high_resolution_clock::now();
+    // 提取聚类中最大的簇
+    std::vector<int> max_cluster;
+    int max_size = 0;
+
+    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+    {
+        //std::cout << it->indices.size() << std::endl;
+        if (it->indices.size() > max_size)
+        {
+            // std::cout << it->indices.size() << std::endl;
+            max_size = it->indices.size();
+            max_cluster = it->indices;
+        }
+    }
+    //std::cout << "max_cluster size = " << max_cluster.size() << std::endl;
+    //std::cout << "max_size = " << max_size << std::endl;
+    for (std::vector<int>::const_iterator pit = max_cluster.begin(); pit != max_cluster.end(); ++pit)
+        filter_ground_cloud->points.push_back(voxel_filtered->points[*pit]); 
+    filter_ground_cloud->header = source_ground_cloud->header;
+    std::chrono::high_resolution_clock::time_point s4 = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double, std::milli> s1_ms = s1 - start;
+    std::chrono::duration<double, std::milli> s2_ms = s2 - start;
+    std::chrono::duration<double, std::milli> s3_ms = s3 - start;
+    std::chrono::duration<double, std::milli> s4_ms = s4 - start;
+	*/
+    // std::cout << "s1_ms = " << s1_ms.count() << " ms" << " s2_ms = " << s2_ms.count() << " s3_ms = " << s3_ms.count() << " ms" << " s4_ms = " << s4_ms.count() << " ms\n";
+  }
+
+
+
+  /*回调函数*/
+
+  void scanCallback(pcl::PointCloud<pcl::PointXYZ>& pl_full,std::vector<float>& timing, const std::string& filename,std::string& number_str) {
+
+   
+    // m1点云数据格式转换
+    //pcl::PointCloud<robosence_ros::Point> pl_orig; //pcl::PointCloud是一个模板类pcl::PointCloud<pcl::PointXYZ>表示由三维点组成的点云
+    
+
+    /*将我们设定的参数传递进去，这里就直接跳转到linefit_ground_segmentation之中*/
+    auto s = CudaTimer<>(); 
+    GroundSegmentation segmenter(params_);
+    /*定义一个是否属于地面点的标签，来进行区分*/
+    std::vector<int> labels;
+
+    /*这个是地面分割类中的主要函数，分割函数9*/
+    segmenter.segment(pl_full, &labels);
+
+    auto k1 = s.ticking();
+    //printf("=> segment ground and obstacle: %.2f ms \n", k1);
+
+    /*定义了两种点云，一个是地面点，一个是障碍物点*/
+    // pcl::PointCloud<pcl::PointXYZ> ground_cloud, obstacle_cloud;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, obstacle_cloud;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_cloud, cloud_clustered;
+    ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    obstacle_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    filter_ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    cloud_clustered.reset(new pcl::PointCloud<pcl::PointXYZ>());
+  
+    /*将整个的无序点云按照标签分配到不同的点云之中*/
+    /*通过这里的标签对于是否属于地面点进行了划分*/
+    // for (size_t i = 0; i < pl_full.size(); ++i) {
+    //   if (labels[i] == 1) ground_cloud.push_back(pl_full[i]);
+    //   else obstacle_cloud.push_back(pl_full[i]);
+    // }
+
+    for (size_t i = 0; i < pl_full.size(); ++i) {
+      if (labels[i] == 1) ground_cloud->points.push_back(pl_full[i]);
+      else obstacle_cloud->points.push_back(pl_full[i]);
+    }
+    
+    std::cout << " process ground_cloud before size = " << ground_cloud->points.size() << std::endl;
+    processGround(ground_cloud, filter_ground_cloud, filename);
+    std::cout << " process ground_cloud after size  = " << filter_ground_cloud->points.size() << std::endl;
+
+    auto k2 = s.ticking();
+    //printf("=> process ground cloud: %.2f ms \n", k2);
+    //std::cout << " filter_ground_cloud.size() = " << filter_ground_cloud->points.size() << std::endl;
+    detectFod(filter_ground_cloud, obstacle_cloud, cloud_clustered, filename, number_str);
+
+    auto k3 = s.ticking();
+    //printf("=> process obstacle_cloud cloud: %.2f ms \n", k3);
+   
+    //std::cout << "s1_ms = " << s1_ms.count() << " ms" << " s2_ms = " << s2_ms.count() << " s3_ms = " << s3_ms.count() << " ms" << " s4_ms = " << s4_ms.count() << " ms\n";
+    /*将按照标签分类好的数据分配到不同的点云之中*/
+    timing[0] += k1;
+    timing[1] += k2;
+    timing[2] += k3;
+
+  }
+
+
+// void fodCallback(const sensor_msgs::PointCloud2::ConstPtr &msg){
+//     // pcl::PointCloud<pcl::PointXYZ> ground;
+//     pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, obstacle_cloud;
+//     pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_cloud, cloud_clustered;
+//     ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     obstacle_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     filter_ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     cloud_clustered.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     pcl::fromROSMsg(*msg, *ground_cloud);
+//     std::cout << "ground_cloud->points().size() = " << ground_cloud->points.size() << std::endl;
+//     processGround(ground_cloud, filter_ground_cloud);
+//     detectFod(filter_ground_cloud, obstacle_cloud, cloud_clustered);
+//     obstacle_pub_.publish(*cloud_clustered);
+// }
+
+
+  
+  // void scanCallback(const pcl::PointCloud<pcl::PointXYZ>& cloud) {
+  //   /*将我们设定的参数传递进去，这里就直接跳转到linefit_ground_segmentation之中*/
+  //   GroundSegmentation segmenter(params_);
+  //   /*定义一个是否属于地面点的标签，来进行区分*/
+  //   std::vector<int> labels;
+
+  //   /*这个是地面分割类中的主要函数，分割函数9*/
+  //   segmenter.segment(cloud, &labels);
+  //   /*定义了两种点云，一个是地面点，一个是障碍物点*/
+  //   pcl::PointCloud<pcl::PointXYZ> ground_cloud, obstacle_cloud;
+  //   ground_cloud.header = cloud.header;
+  //   obstacle_cloud.header = cloud.header;
+  //   /*将整个的无序点云按照标签分配到不同的点云之中*/
+  //   /*通过这里的标签对于是否属于地面点进行了划分*/
+  //   for (size_t i = 0; i < cloud.size(); ++i) {
+  //     if (labels[i] == 1) ground_cloud.push_back(cloud[i]);
+  //     else obstacle_cloud.push_back(cloud[i]);
+  //   }
+  //   /*将按照标签分类好的数据分配到不同的点云之中*/
+  //   ground_pub_.publish(ground_cloud);
+  //   obstacle_pub_.publish(obstacle_cloud);
+  // }
+  
+};
+
+
+
+/*主函数，给出了ros的接口，跳转到linefir_ground_segmentation之中*/
+int main(int argc, char** argv) {
+
+  /*调用glog中的函数来初始化日志，为后面做准备*/
+
+
+  std::string file_path = "../launch/rv4.yaml"; 
+
+  
+
+  std::ifstream fin(file_path);
+  if(!fin.good()){
+    std::cerr<< "Faild open the file : "<<file_path<<std::endl;
+    return 1;
+  }
+  fin.close();
+
+  YAML::Node config = YAML::LoadFile(file_path);
+
+
+  // Do parameter stuff.
+  /*参数设定，可以读入我们在yaml中设定的参数*/
+  GroundSegmentationParams params;
+  params.visualize = config["visualize"].as<bool>();
+  params.n_bins = config["n_bins"].as<int>();
+  params.n_segments = config["n_segments"].as<int>();
+  params.max_dist_to_line = config["max_dist_to_line"].as<double>();
+  params.max_slope = config["max_slope"].as<double>();
+  params.long_threshold = config["long_threshold"].as<double>();
+  params.max_long_height = config["max_long_height"].as<double>();
+  params.max_start_height = config["max_start_height"].as<double>();
+  params.sensor_height = config["sensor_height"].as<double>();
+  params.line_search_angle = config["line_search_angle"].as<double>();
+  params.n_threads= config["n_threads"].as<int>();
+
+  double r_min, r_max, max_fit_error;
+  r_min = config["r_min"].as<double>();
+  r_max = config["r_max"].as<double>();
+  max_fit_error = config["max_fit_error"].as<double>();
+
+  params.r_min_square = r_min*r_min;
+  params.r_max_square = r_max*r_max;
+  params.max_error_square = max_fit_error * max_fit_error;
+  std::cout<<"r_min_square :" <<params.r_min_square<<std::endl;
+/*
+  nh.param("visualize", params.visualize, params.visualize);
+  nh.param("n_bins", params.n_bins, params.n_bins);
+  nh.param("n_segments", params.n_segments, params.n_segments);
+  nh.param("max_dist_to_line", params.max_dist_to_line, params.max_dist_to_line);
+  nh.param("max_slope", params.max_slope, params.max_slope);
+  nh.param("long_threshold", params.long_threshold, params.long_threshold);
+  nh.param("max_long_height", params.max_long_height, params.max_long_height);
+  nh.param("max_start_height", params.max_start_height, params.max_start_height);
+  nh.param("sensor_height", params.sensor_height, params.sensor_height);
+  nh.param("line_search_angle", params.line_search_angle, params.line_search_angle);
+  nh.param("n_threads", params.n_threads, params.n_threads);
+   
+  std::string ground_topic, obstacle_topic, input_topic;
+  bool latch;
+  //input topic是需要我们自己设定的，根据自己雷达的数据而定
+  // Params that need to be squared.
+  nh.param<std::string>("input_topic", input_topic, "input_cloud");
+  nh.param<std::string>("ground_output_topic", ground_topic, "ground_cloud");
+  nh.param<std::string>("obstacle_output_topic", obstacle_topic, "obstacle_cloud");
+  nh.param("latch", latch, false);
+*/
+  
+
+
+  // Start node.
+  /*我们初始化类，从类中调到别的地方*/
+  SegmentationNode node(params);
+
+  typedef pcl::PointXYZ PointT;
+  pcl::PointCloud<pcl::PointXYZ> pl_full;
+  pcl::PointCloud<pcl::PointXYZ> pl_filter_z;
+
+  if(strcmp(argv[1], "bin") == 0){
+  std::cout<< "read bin point cloud " << argv[2] << std::endl;
+  std::fstream input(argv[2], std::ios::in | std::ios::binary);
+  if(!input.good()){
+    std::cerr << "Could not read file: " << argv[2] << std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  float item[4];
+  int count_tmp = 0;
+  // printf("count_tmp start at : %d....\n", count_tmp);
+
+  // std::ofstream OutFile("read_pcfile.txt");
+  while (input.read((char *)item, 16))
+  {
+    //std::cout << item[0] << " " << item[1] << " " << item[2]  << " " << item[3] <<std::endl;
+    pcl::PointXYZ point;
+    point.x = item[0];
+    point.y = item[1];
+    point.z = item[2];
+    
+    pl_full.push_back(point);
+    count_tmp++;
+  }
+  
+  // OutFile.close();
+  std::cout<<"read_pcfile.txt finished......... " << std::endl;
+  // exit(-1);
+  }
+  else if(strcmp(argv[1], "pcd") == 0){
+    std::cout<< "read pcd point cloud " << argv[2] << std::endl;
+    std::string file_name = std::string(argv[2]);
+
+    if (pcl::io::loadPCDFile<PointT> (file_name, pl_full) == -1){
+      // load the file
+      PCL_ERROR ("Couldn't read PCD file \n");
+      return (-1);
+    }
+  }
+
+
+  // 为保存的bbox.txt和 cluster.txt是根据pcd文件名命名的 便于区分
+  std::string tmp(argv[2]);
+  std::cout << "tmp: " << tmp << "\n";
+  size_t start_pos = tmp.find_last_of("/") + 1;
+  //size_t end_pos = tmp.find(".", start_pos);
+  size_t end_pos = tmp.find_last_of(".");
+  std::string number_str = tmp.substr(start_pos, end_pos - start_pos);
+  std::cout << "number_str: " << number_str << "\n";
+  std::string file_txt = "../result/clusters_gpu_" + number_str + ".txt" ;
+
+
+  //cuda::utils::warmUpGPU();
+
+  printf("init pcd size:%d \n",pl_full.size());
+  // pcl::PointCloud<pcl::PointXYZ>::Ptr pl_full_ptr(new pcl::PointCloud<pcl::PointXYZ>(pl_full));
+  std::vector<int> indices;
+  // pcl::removeNaNFromPointCloud(*pl_full_ptr, *pl_full_ptr, indices);
+  pcl::removeNaNFromPointCloud(pl_full, pl_full, indices);
+  printf("rmove NaN  pcd size:%d \n",pl_full.size());
+
+  for (int i=0; i<pl_full.size();i++){
+    if (pl_full[i].z > 1)
+      continue;
+    pl_filter_z.push_back(pl_full[i]);
+
+  }
+  
+  //printf("pl_filter_z size:%d \n",pl_filter_z.size());
+  int run_count = std::atoi(argv[3]); 
+  printf("=> read run_count: %d \n", run_count);
+  std::vector<float> timing(5, 0.0f);
+  for (int i = 0; i < run_count; i++)
+  {
+    //node.scanCallback(pl_filter_z,timing); //过滤point by z
+    node.scanCallback(pl_full,timing,file_txt,number_str);
+  }
+  
+
+
+  float p1 = timing[0] / run_count;
+  float p2 = timing[1] / run_count;
+  float p3 = timing[2] / run_count;
+
+  // printf("segment ground and obstacle: %.2f ms \n", p1);
+  // printf("process ground cloud: %.2f ms \n", p2);
+  printf("process obstacle_cloud cloud: %.2f ms \n", p3);
+}

test_project/linefit_ground_segmentation_local_gpu_test/src/ground_segmentation_node_mult_test.cc

+#include <pcl/io/ply_io.h>
+#include <experimental/filesystem>
+// #include <pcl_ros/point_cloud.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/segmentation/progressive_morphological_filter.h>
+#include <pcl/common/common.h>
+#include <pcl/common/time.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl/common/transforms.h>
+#include <yaml-cpp/yaml.h>
+#include <cuda_runtime.h>
+#include "ground_segmentation/ground_segmentation.h"
+#include <dbscan/CudaUtils.cuh>
+#include <dbscan/DBSCAN.cuh>
+#include "AppUtility.hpp"
+
+
+#define POINTS_TO_FILE  // 定义宏 通过注释其来确定是否要保存 txt
+
+namespace
+{
+  using PointCloudType = pcl::PointXYZ;
+  using PointCloud = pcl::PointCloud<PointCloudType>;
+  using PointCloudPtr = PointCloud::Ptr;
+  using DBSCAN = clustering::cuda::DBSCAN<PointCloudType>;
+
+  auto timer = pcl::StopWatch();
+  int NUM_TEST = 20;
+}  // namespace
+
+
+
+template <typename T = double> struct CudaTimer {
+  CudaTimer() {
+    //cudaDeviceSynchronize();
+    mCurTime = std::chrono::steady_clock::now();
+  }
+
+  T ticking() {
+    //cudaDeviceSynchronize();
+    auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now() - mCurTime);
+    T res = duration.count();
+    mCurTime = std::chrono::steady_clock::now();
+    return res * 1e-6;
+  }
+
+  private:
+    std::chrono::time_point<std::chrono::steady_clock> mCurTime;
+};
+
+// 定义包围盒信息
+struct BoxInfo {
+  Eigen::Vector3f center;
+  Eigen::Vector3f size;
+  Eigen::Quaternionf orientation;
+};
+
+void writeClusterToFile(const std::vector<PointCloudPtr>& clusters, const std::string& filename)
+{
+    //std::ofstream file(filename);
+    std::ofstream file(filename, std::ios::app);
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+    int index = 1;
+
+    for (auto& cluster : clusters) {
+        file << "cluster " << index << std::endl;
+        for (auto& point : cluster->points) {
+          file << point.x << ", " << point.y << ", " << point.z << std::endl;
+        }
+        ++index;
+    }
+    file.close();
+}
+
+void writeGroundPointToFile(pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_ground_cloud, const std::string& filename)
+{   
+    int index = 0;
+
+    std::ofstream file(filename);
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+    std::cout << "File created" << std::endl;
+    file << "cluster " << index << std::endl;
+   
+
+
+    // 遍历点云中的每个点
+    for (const auto& point : filter_ground_cloud->points)
+    {
+      // 以指定格式将点的坐标写入到文件中
+      file << point.x << ", " << point.y << ", " << point.z << endl;
+    }
+    file.close();
+}
+
+
+// for robosence solid lidar m1
+namespace robosence_ros {
+    struct EIGEN_ALIGN16 Point {
+        double timestamp;
+        PCL_ADD_POINT4D;
+        uint8_t intensity;
+        uint16_t ring;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+}  // namespace robosence_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(robosence_ros::Point,
+    (double, timestamp, timestamp)
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (uint8_t, intensity, intensity)
+    (uint16_t, ring, ring)
+)
+typedef pcl::PointXYZ PointType;
+typedef pcl::PointCloud<PointType> PointCloudXYZI;
+
+
+
+/*我们定义的分割节点的类*/
+class SegmentationNode {
+
+  GroundSegmentationParams params_;//地面分割的参数
+
+public:
+  SegmentationNode(const GroundSegmentationParams& params) : params_(params) {
+  
+  }
+
+  void getBoundingBoxInfo(PointCloudPtr cloud_cluster, Eigen::Vector3f& boxCenter, Eigen::Vector3f& size,Eigen::Quaternionf& boxOrientation)
+{   
+    //PCA 本质就是基于坐标系的转化
+
+    //5. 计算点云的几何中心和重心，注意这两个概念不一样，重心是所有点的平均值，中心是最大最小点的平均值
+    
+    Eigen::Vector4f centroid;     // 重心
+    pcl::compute3DCentroid(*cloud_cluster, centroid);
+    PointType min_pt, max_pt;
+    Eigen::Vector3f center;       // 中心
+    pcl::getMinMax3D(*cloud_cluster, min_pt, max_pt); //获取每个cloud_cluster中的(xmin，ymin,zmin),(xmax,ymax,zmax)
+    center = (max_pt.getVector3fMap() + min_pt.getVector3fMap())/2;
+    // 6. 计算协方差矩阵的特征向量(也就是PCA的特征向量--原始坐标系换成PCA坐标系的旋转矩阵，且原点已经偏移改为cloud_cluster的重心)
+    Eigen::Matrix3f covariance;
+    pcl::computeCovarianceMatrixNormalized(*cloud_cluster, centroid, covariance); // 这里必须用重心 重心作为PCA新坐标系的原点
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
+    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
+    Eigen::Vector3f eigenValuesPCA  = eigen_solver.eigenvalues();
+    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //正交化
+    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
+    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
+    // 按照特征值从大到小排列（eigenvalues函数返回的特征值是从小到大排列的）
+    Eigen::Matrix3f eigenVectorsPCA1;
+    eigenVectorsPCA1.col(0) = eigenVectorsPCA.col(2);
+    eigenVectorsPCA1.col(1) = eigenVectorsPCA.col(1);
+    eigenVectorsPCA1.col(2) = eigenVectorsPCA.col(0);
+    eigenVectorsPCA = eigenVectorsPCA1;
+
+    // 7. 计算变换矩阵，只考虑绕全局坐标系Z轴的转动
+    Eigen::Vector3f ea = (eigenVectorsPCA).eulerAngles(2, 1, 0); // 分别对应 yaw pitch roll 旋转矩阵转欧拉角
+    Eigen::AngleAxisf keep_Z_Rot(ea[0], Eigen::Vector3f::UnitZ()); //只取yaw角 也就是航向角绕z轴旋转的角度
+    Eigen::Affine3f transform = Eigen::Affine3f::Identity(); 
+    transform.translate(center);  // translate与rotate书写的顺序不可搞反  先将原始坐标系原点平移center个单位，也就是新坐标系的原点是center
+    transform.rotate(keep_Z_Rot); // radians 以重心为中心点进行的旋转，所以旋转以后center不再是几何中心了，重新找中心点
+    
+    // 8. 计算包围盒的尺寸
+    pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
+    pcl::transformPointCloud(*cloud_cluster, *transformedCloud, transform.inverse());
+    // 重新计算变换后的点云的中心，因为变换前的点云的中心不是其包围盒的几何中心
+    PointType min_pt_T, max_pt_T;
+    pcl::getMinMax3D(*transformedCloud, min_pt_T, max_pt_T);
+    Eigen::Vector3f center_new = (max_pt_T.getVector3fMap() + min_pt_T.getVector3fMap()) / 2;
+    Eigen::Vector3f box_dim;
+    box_dim = max_pt_T.getVector3fMap() - min_pt_T.getVector3fMap();
+    Eigen::Affine3f transform2 = Eigen::Affine3f::Identity();
+    transform2.translate(center_new); //再平移点坐标系的原点
+    Eigen::Affine3f transform3 = transform * transform2; //所以整个过程平移了两次，平移后的
+
+    size = box_dim;
+    boxCenter = transform3.translation(); // 平移原点的x，y，z的距离成为了新的原点，也就是新的"中心"，因为要显示的包围盒采用几何中心作为参考点
+    boxOrientation = Eigen::Quaternionf(keep_Z_Rot);
+  
+}
+
+  void detectFod(pcl::PointCloud<pcl::PointXYZ>::Ptr& ground_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& obstacle_cloud,  pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_clustered, const std::string& filename,const std::string& number_str)
+  {
+
+    cloud_clustered->header = ground_cloud->header;
+
+    //auto t = CudaTimer<>();
+ 
+    // 2: 获取ground_cloud外接凸包
+    // pcl::ConvexHull<pcl::PointXYZ> hull;                  //创建凸包对象
+    // hull.setInputCloud(ground_cloud);                  //设置输入点云
+    // hull.setDimension(2);                                 //设置凸包维度
+    // std::vector<pcl::Vertices> polygons;                  //设置向量，用于保存凸包定点
+    // pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);//该点运用于描述凸包形状
+    // hull.reconstruct(*surface_hull, polygons);            //计算2D凸包结果
+
+    // int mid = surface_hull->points.size() / 2;
+    // if (mid-3 >= 0 && mid+3 < surface_hull->points.size())
+    // {
+    //   surface_hull->points[mid-3].x = 150;
+    //   surface_hull->points[mid-2].x = 150;
+    //   surface_hull->points[mid-1].x = 150;
+    //   surface_hull->points[mid].x = 150;
+    //   surface_hull->points[mid+3].x = 150;
+    //   surface_hull->points[mid+2].x = 150;
+    //   surface_hull->points[mid+1].x = 150;
+    // }
+
+    /*
+    for(int i = 0; i < surface_hull->points.size(); i++)
+    {
+      pcl::PointXYZ tmp;
+      tmp.x = surface_hull->points[i].x;
+      tmp.y = surface_hull->points[i].y;
+      tmp.z = surface_hull->points[i].z;
+      std::cout << "x = " << tmp.x << " y = " << tmp.y << " z = " << tmp.z << std::endl;
+
+    }
+    */
+
+    //std::cout << " " << std::endl;
+    // 3: 根据凸包对obstacle_cloud进行过滤，保留路面上的点云
+    printf("=> obstacle_cloud points size : %d  \n", obstacle_cloud->points.size());
+    
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
+
+    objects = obstacle_cloud ;
+
+
+    //liuji -  凸包
+    // pcl::CropHull<pcl::PointXYZ> bb_filter;               //创建crophull对象
+    // bb_filter.setDim(2);                                  //设置维度：该维度需要与凸包维度一致
+    // bb_filter.setInputCloud(obstacle_cloud);              //设置需要滤波的点云
+    // bb_filter.setHullIndices(polygons);                   //输入封闭多边形的顶点
+    // bb_filter.setHullCloud(surface_hull);                 //输入封闭多边形的形状
+    // bb_filter.filter(*objects);                           //执行CropHull滤波，存出结果在objects
+    
+   
+
+
+    // std::cout << objects->size() << std::endl;            //
+    //auto k1= t.ticking();
+    //printf("=> get convex hull : %.2f ms \n", k1);
+    // 基于距离将点云分成两个，分别进行滤波
+    printf("=>obstacle_cloud crophull points size : %d  \n", objects->points.size());
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr far (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close (new pcl::PointCloud<pcl::PointXYZ>);
+
+    for(int i = 0; i < objects->points.size(); i++)
+    {
+      if (objects->points[i].x < 1000)
+      {
+        if (objects->points[i].z > -1.3) //-1.5m以上的点做聚类处理
+          close->points.push_back(objects->points[i]);
+      }
+      else
+        far->points.push_back(objects->points[i]);
+    }
+
+    printf("=>obstacle_cloud voxel before close points size : %d  \n", close->points.size());
+    //auto k2= t.ticking();
+    //printf("=> get close cloud : %.2f ms \n", k2);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::VoxelGrid<pcl::PointXYZ> vor;  //滤波处理对象
+    vor.setInputCloud(close);
+    vor.setLeafSize(0.10f, 0.10f, 0.10f);  //设置滤波器处理时采用的体素大小的参数
+    vor.filter(*close_voxel_filtered);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    vor.filter(*close_objects_filtered);
+    //auto k3= t.ticking();
+    //printf("=> Voxel close cloud : %.2f ms \n", k3);
+    // 4: 获取点云目标检测结果，对路面点云进行目标过滤
+    /* //liuji  注释
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr far_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> ssor;//创建滤波器对象
+    ssor.setInputCloud(close_voxel_filtered);                       //设置待滤波的点云
+    ssor.setMeanK(50);                                 //设置在进行统计时考虑查询点临近点数
+    ssor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值
+    ssor.filter(*close_objects_filtered);                    //存储
+    */
+
+    //auto k4= t.ticking();
+    //printf("=> OutlierRemoval close cloud : %.2f ms \n", k4);
+    //pcl::StatisticalOutlierRemoval<pcl::PointXYZ> fsor;//创建滤波器对象
+    //fsor.setInputCloud(far);                       //设置待滤波的点云
+    //fsor.setMeanK(5);                                 //设置在进行统计时考虑查询点临近点数
+    //fsor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值
+    //fsor.filter(*far_objects_filtered);                    //存储
+    printf("=>close_voxel_filtered  points  size : %d  \n", close_voxel_filtered->points.size());
+
+    if (close_objects_filtered->points.size() < 10)
+      return;
+    
+    
+    // 5: 对obstacle_cloud进行dbscan聚类，得到每个障碍物
+
+    auto t = CudaTimer<>();
+
+    int numPoints = close_objects_filtered->points.size();
+    printf("=>close_objects_filtered voxel(dbscan before) after points  size : %d  \n", numPoints);
+   
+    PointType* points = close_objects_filtered->points.data();
+    DBSCAN::Param param{.pointDimension = 3, .eps = 0.3, .minPoints = 20};
+    DBSCAN dbscanHandler(param);
+    std::vector<std::vector<int>> clusterIndices;
+
+    clusterIndices = dbscanHandler.run(points, numPoints);
+    std::vector<PointCloudPtr> clusters = ::toClusters<PointCloudType>(clusterIndices, close_objects_filtered);
+    auto k = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k);
+/* 原始cpu DBSCAN算法使用的是 KdTree
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(close_objects_filtered);
+    // Segmentation, [Euclidean Cluster Extraction](https://pcl.readthedocs.io/projects/tutorials/en/latest/cluster_extraction.html#cluster-extraction)
+    std::vector<pcl::PointIndices> cluster_indices;
+    auto k5= t.ticking();
+    //printf("=> KdTree close cloud : %.2f ms \n", k5);
+    DBSCANKdtreeCluster ec;
+    ec.setCorePointMinPts(20);//一个核心点邻域最少点数
+    ec.setClusterTolerance(0.8);  //算法半径
+    ec.setMinClusterSize(100);   //一个簇最少点数
+	//ec.setMinClusterSize(10);
+    ec.setMaxClusterSize(99999);  //一个簇最大点数
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(close_objects_filtered);
+	//ec.setInputCloud(close_voxel_filtered);
+    ec.extract(cluster_indices);
+    auto k7= t.ticking();
+    //printf("=> extract close cloud : %.2f ms \n", k7);
+    auto k6 = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k6);
+    printf("=> cluster_indices size : %.d \n", cluster_indices.size());
+*/
+
+
+/* 对每个簇进行处理，将point的值获取到
+    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++) {
+      for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
+            pcl::PointXYZ tmp;
+            tmp.x = close_objects_filtered->points[*pit].x;
+            tmp.y = close_objects_filtered->points[*pit].y;
+            tmp.z = close_objects_filtered->points[*pit].z;
+            // tmp.intensity = j%8;
+            // tmp.ring = objects->points[*pit].ring;
+            if (tmp.z < -1.4 || tmp.z > 4.2)
+              continue;
+            cloud_clustered->points.push_back(tmp);
+      }
+    }
+
+*/
+    //该循huai只是把聚类后的点云获取到，并没有去做分类处理
+    std::vector<PointCloudPtr> clusters_filter_z;
+    printf("=> clusters size : %.d \n", clusters.size());
+    auto k1 = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k);
+    std::string bbox_txt = "../result/test_mult/clusters_bbox_" + number_str + ".txt" ;
+    ofstream outfile(bbox_txt);
+    for (int i  = 0; i < clusters.size(); i++) {
+      pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_filter_z(new pcl::PointCloud<pcl::PointXYZ>);
+      // #ifdef POINTS_TO_FILE
+      // Eigen::Vector3f boxCenter, boxSize;
+      // Eigen::Quaternionf boxOrientation;
+      
+      // getBoundingBoxInfo(clusters[i], boxCenter, boxSize, boxOrientation);
+      // std::cout << "cluster: " << i << "\n";
+      // std::cout << "cloud_clustered->points.size()" <<cloud_clustered->points.size()<< std::endl;
+      // std::cout  << "box center: " << boxCenter.transpose() << "\n";
+      // std::cout  << "box size: " << boxSize.transpose() << "\n";
+      // std::cout  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n";
+      // outfile << "cluster: " << i << "\n";
+      // outfile  << "box center: " << boxCenter.transpose() << "\n";
+      // outfile  << "box size: " << boxSize.transpose() << "\n";
+      // outfile  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n"; 
+      // #endif
+     
+      for (auto it = clusters[i]->begin(); it !=clusters[i]->end(); ++it){
+        pcl::PointXYZ tmp;
+        tmp.x = it->x;
+        tmp.y = it->y;
+        tmp.z = it->z;
+        if (tmp.z < -1.3 || tmp.z > 1.2) //确定bbox内点云的范围
+                  continue;
+        cloud_clustered->points.push_back(tmp);
+        tmp_filter_z->points.push_back(tmp); 
+      }
+      clusters_filter_z.push_back(tmp_filter_z);
+
+      #ifdef POINTS_TO_FILE
+      Eigen::Vector3f boxCenter, boxSize;
+      Eigen::Quaternionf boxOrientation;
+      if (tmp_filter_z->points.size() == 0)
+          continue;
+      getBoundingBoxInfo(tmp_filter_z, boxCenter, boxSize, boxOrientation);
+      std::cout << "cluster: " << i << "\n";
+      std::cout << "cloud_clustered->points.size()" <<cloud_clustered->points.size()<< std::endl;
+      std::cout  << "box center: " << boxCenter.transpose() << "\n";
+      std::cout  << "box size: " << boxSize.transpose() << "\n";
+      std::cout  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n";
+      outfile << "cluster: " << i << "\n";
+      outfile  << "box center: " << boxCenter.transpose() << "\n";
+      outfile  << "box size: " << boxSize.transpose() << "\n";
+      outfile  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n"; 
+      #endif
+    }
+ 
+    outfile.close();
+
+    printf("=> final obstacle_cloud clustered points size : %.d \n", cloud_clustered->points.size());
+    #ifdef POINTS_TO_FILE
+    writeClusterToFile(clusters_filter_z,filename); // 将过滤z轴后的点 进行存储可视化
+    #endif
+
+    for(int i = 0; i < far->points.size(); i++)
+    {
+      if (far->points[i].z < -2 || far->points[i].z > 0)
+        continue;
+      cloud_clustered->points.push_back(far->points[i]);
+    }
+    //printf("=> processs result : %.2f ms \n", k1);
+    // std::cout<<"cluster size = "<<cloud_clustered->points.size()<<endl;
+    
+  }
+
+  void processGround(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_ground_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_ground_cloud,const std::string& filename)
+  {
+
+    // // 1. 过滤离群点
+    // pcl::PointCloud<pcl::PointXYZ>::Ptr outlier_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    // pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;//创建滤波器对象
+    // sor.setInputCloud(source_ground_cloud); //设置待滤波的点云
+    // sor.setMeanK(50);     //设置在进行统计时考虑查询点临近点数
+    // sor.setStddevMulThresh(1.0);      //设置判断是否为离群点的阀值
+    // sor.filter(*outlier_filtered);      //存储
+ 
+    // 体素滤波
+    pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::VoxelGrid<pcl::PointXYZ> ssor;  //滤波处理对象
+    ssor.setInputCloud(source_ground_cloud);
+    ssor.setLeafSize(0.15f, 0.15f, 0.15f);  //设置滤波器处理时采用的体素大小的参数
+    // ssor.filter(*voxel_filtered);
+	  ssor.filter(*filter_ground_cloud);
+    #ifdef POINTS_TO_FILE
+    writeGroundPointToFile(filter_ground_cloud,filename);
+    #endif
+	/*
+    std::chrono::high_resolution_clock::time_point s2 = std::chrono::high_resolution_clock::now();
+    // 欧式聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(voxel_filtered);
+    // Segmentation, [Euclidean Cluster Extraction](https://pcl.readthedocs.io/projects/tutorials/en/latest/cluster_extraction.html#cluster-extraction)
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+
+    ec.setClusterTolerance(0.8);  //算法半径
+    ec.setMinClusterSize(100);   //一个簇最少点数
+    ec.setMaxClusterSize(99999);  //一个簇最大点数
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(voxel_filtered);
+    ec.extract(cluster_indices);
+    std::chrono::high_resolution_clock::time_point s3 = std::chrono::high_resolution_clock::now();
+    // 提取聚类中最大的簇
+    std::vector<int> max_cluster;
+    int max_size = 0;
+
+    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+    {
+        //std::cout << it->indices.size() << std::endl;
+        if (it->indices.size() > max_size)
+        {
+            // std::cout << it->indices.size() << std::endl;
+            max_size = it->indices.size();
+            max_cluster = it->indices;
+        }
+    }
+    //std::cout << "max_cluster size = " << max_cluster.size() << std::endl;
+    //std::cout << "max_size = " << max_size << std::endl;
+    for (std::vector<int>::const_iterator pit = max_cluster.begin(); pit != max_cluster.end(); ++pit)
+        filter_ground_cloud->points.push_back(voxel_filtered->points[*pit]); 
+    filter_ground_cloud->header = source_ground_cloud->header;
+    std::chrono::high_resolution_clock::time_point s4 = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double, std::milli> s1_ms = s1 - start;
+    std::chrono::duration<double, std::milli> s2_ms = s2 - start;
+    std::chrono::duration<double, std::milli> s3_ms = s3 - start;
+    std::chrono::duration<double, std::milli> s4_ms = s4 - start;
+	*/
+    // std::cout << "s1_ms = " << s1_ms.count() << " ms" << " s2_ms = " << s2_ms.count() << " s3_ms = " << s3_ms.count() << " ms" << " s4_ms = " << s4_ms.count() << " ms\n";
+  }
+
+
+
+  /*回调函数*/
+
+  void scanCallback(pcl::PointCloud<pcl::PointXYZ>& pl_full,std::vector<float>& timing, const std::string& filename,std::string& number_str) {
+
+   
+    // m1点云数据格式转换
+    //pcl::PointCloud<robosence_ros::Point> pl_orig; //pcl::PointCloud是一个模板类pcl::PointCloud<pcl::PointXYZ>表示由三维点组成的点云
+    
+
+    /*将我们设定的参数传递进去，这里就直接跳转到linefit_ground_segmentation之中*/
+    auto s = CudaTimer<>(); 
+    GroundSegmentation segmenter(params_);
+    /*定义一个是否属于地面点的标签，来进行区分*/
+    std::vector<int> labels;
+
+    /*这个是地面分割类中的主要函数，分割函数9*/
+    segmenter.segment(pl_full, &labels);
+
+    auto k1 = s.ticking();
+    //printf("=> segment ground and obstacle: %.2f ms \n", k1);
+
+    /*定义了两种点云，一个是地面点，一个是障碍物点*/
+    // pcl::PointCloud<pcl::PointXYZ> ground_cloud, obstacle_cloud;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, obstacle_cloud;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_cloud, cloud_clustered;
+    ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    obstacle_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    filter_ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    cloud_clustered.reset(new pcl::PointCloud<pcl::PointXYZ>());
+  
+    /*将整个的无序点云按照标签分配到不同的点云之中*/
+    /*通过这里的标签对于是否属于地面点进行了划分*/
+    // for (size_t i = 0; i < pl_full.size(); ++i) {
+    //   if (labels[i] == 1) ground_cloud.push_back(pl_full[i]);
+    //   else obstacle_cloud.push_back(pl_full[i]);
+    // }
+
+    for (size_t i = 0; i < pl_full.size(); ++i) {
+      if (labels[i] == 1) ground_cloud->points.push_back(pl_full[i]);
+      else obstacle_cloud->points.push_back(pl_full[i]);
+    }
+    
+    std::cout << " process ground_cloud before size = " << ground_cloud->points.size() << std::endl;
+    processGround(ground_cloud, filter_ground_cloud, filename);
+    std::cout << " process ground_cloud after size  = " << filter_ground_cloud->points.size() << std::endl;
+
+    auto k2 = s.ticking();
+    //printf("=> process ground cloud: %.2f ms \n", k2);
+    //std::cout << " filter_ground_cloud.size() = " << filter_ground_cloud->points.size() << std::endl;
+    detectFod(filter_ground_cloud, obstacle_cloud, cloud_clustered, filename, number_str);
+
+    auto k3 = s.ticking();
+    //printf("=> process obstacle_cloud cloud: %.2f ms \n", k3);
+   
+    //std::cout << "s1_ms = " << s1_ms.count() << " ms" << " s2_ms = " << s2_ms.count() << " s3_ms = " << s3_ms.count() << " ms" << " s4_ms = " << s4_ms.count() << " ms\n";
+    /*将按照标签分类好的数据分配到不同的点云之中*/
+    timing[0] += k1;
+    timing[1] += k2;
+    timing[2] += k3;
+
+  }
+
+
+// void fodCallback(const sensor_msgs::PointCloud2::ConstPtr &msg){
+//     // pcl::PointCloud<pcl::PointXYZ> ground;
+//     pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, obstacle_cloud;
+//     pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_cloud, cloud_clustered;
+//     ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     obstacle_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     filter_ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     cloud_clustered.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     pcl::fromROSMsg(*msg, *ground_cloud);
+//     std::cout << "ground_cloud->points().size() = " << ground_cloud->points.size() << std::endl;
+//     processGround(ground_cloud, filter_ground_cloud);
+//     detectFod(filter_ground_cloud, obstacle_cloud, cloud_clustered);
+//     obstacle_pub_.publish(*cloud_clustered);
+// }
+
+
+  
+  // void scanCallback(const pcl::PointCloud<pcl::PointXYZ>& cloud) {
+  //   /*将我们设定的参数传递进去，这里就直接跳转到linefit_ground_segmentation之中*/
+  //   GroundSegmentation segmenter(params_);
+  //   /*定义一个是否属于地面点的标签，来进行区分*/
+  //   std::vector<int> labels;
+
+  //   /*这个是地面分割类中的主要函数，分割函数9*/
+  //   segmenter.segment(cloud, &labels);
+  //   /*定义了两种点云，一个是地面点，一个是障碍物点*/
+  //   pcl::PointCloud<pcl::PointXYZ> ground_cloud, obstacle_cloud;
+  //   ground_cloud.header = cloud.header;
+  //   obstacle_cloud.header = cloud.header;
+  //   /*将整个的无序点云按照标签分配到不同的点云之中*/
+  //   /*通过这里的标签对于是否属于地面点进行了划分*/
+  //   for (size_t i = 0; i < cloud.size(); ++i) {
+  //     if (labels[i] == 1) ground_cloud.push_back(cloud[i]);
+  //     else obstacle_cloud.push_back(cloud[i]);
+  //   }
+  //   /*将按照标签分类好的数据分配到不同的点云之中*/
+  //   ground_pub_.publish(ground_cloud);
+  //   obstacle_pub_.publish(obstacle_cloud);
+  // }
+  
+};
+
+
+
+/*主函数，给出了ros的接口，跳转到linefir_ground_segmentation之中*/
+int main(int argc, char** argv) {
+
+  /*调用glog中的函数来初始化日志，为后面做准备*/
+  /*
+  运行指令  ./linefit_ground_segmentation_test_jf_mult pcd ../data/testpcd/ 1
+  
+  ./linefit_ground_segmentation_test_jf_mult 可执行文件
+  pcd 测试文件的后缀
+  ../data/testpcd/ pcd文件的路径
+  1 ：运行次数
+
+  */
+
+  std::string file_path = "../launch/rv4.yaml"; 
+
+  
+
+  std::ifstream fin(file_path);
+  if(!fin.good()){
+    std::cerr<< "Faild open the file : "<<file_path<<std::endl;
+    return 1;
+  }
+  fin.close();
+
+  YAML::Node config = YAML::LoadFile(file_path);
+
+
+  // Do parameter stuff.
+  /*参数设定，可以读入我们在yaml中设定的参数*/
+  GroundSegmentationParams params;
+  params.visualize = config["visualize"].as<bool>();
+  params.n_bins = config["n_bins"].as<int>();
+  params.n_segments = config["n_segments"].as<int>();
+  params.max_dist_to_line = config["max_dist_to_line"].as<double>();
+  params.max_slope = config["max_slope"].as<double>();
+  params.long_threshold = config["long_threshold"].as<double>();
+  params.max_long_height = config["max_long_height"].as<double>();
+  params.max_start_height = config["max_start_height"].as<double>();
+  params.sensor_height = config["sensor_height"].as<double>();
+  params.line_search_angle = config["line_search_angle"].as<double>();
+  params.n_threads= config["n_threads"].as<int>();
+
+  double r_min, r_max, max_fit_error;
+  r_min = config["r_min"].as<double>();
+  r_max = config["r_max"].as<double>();
+  max_fit_error = config["max_fit_error"].as<double>();
+
+  params.r_min_square = r_min*r_min;
+  params.r_max_square = r_max*r_max;
+  params.max_error_square = max_fit_error * max_fit_error;
+  std::cout<<"r_min_square :" <<params.r_min_square<<std::endl;
+/*
+  nh.param("visualize", params.visualize, params.visualize);
+  nh.param("n_bins", params.n_bins, params.n_bins);
+  nh.param("n_segments", params.n_segments, params.n_segments);
+  nh.param("max_dist_to_line", params.max_dist_to_line, params.max_dist_to_line);
+  nh.param("max_slope", params.max_slope, params.max_slope);
+  nh.param("long_threshold", params.long_threshold, params.long_threshold);
+  nh.param("max_long_height", params.max_long_height, params.max_long_height);
+  nh.param("max_start_height", params.max_start_height, params.max_start_height);
+  nh.param("sensor_height", params.sensor_height, params.sensor_height);
+  nh.param("line_search_angle", params.line_search_angle, params.line_search_angle);
+  nh.param("n_threads", params.n_threads, params.n_threads);
+   
+  std::string ground_topic, obstacle_topic, input_topic;
+  bool latch;
+  //input topic是需要我们自己设定的，根据自己雷达的数据而定
+  // Params that need to be squared.
+  nh.param<std::string>("input_topic", input_topic, "input_cloud");
+  nh.param<std::string>("ground_output_topic", ground_topic, "ground_cloud");
+  nh.param<std::string>("obstacle_output_topic", obstacle_topic, "obstacle_cloud");
+  nh.param("latch", latch, false);
+*/
+  
+
+
+  // Start node.
+  /*我们初始化类，从类中调到别的地方*/
+  SegmentationNode node(params);
+
+  typedef pcl::PointXYZ PointT;
+  pcl::PointCloud<pcl::PointXYZ> pl_full;
+  pcl::PointCloud<pcl::PointXYZ> pl_filter_z;
+  std::vector<std::string> file_paths;
+  if(strcmp(argv[1], "bin") == 0){
+  std::cout<< "read bin point cloud " << argv[2] << std::endl;
+  std::fstream input(argv[2], std::ios::in | std::ios::binary);
+  if(!input.good()){
+    std::cerr << "Could not read file: " << argv[2] << std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  float item[4];
+  int count_tmp = 0;
+  // printf("count_tmp start at : %d....\n", count_tmp);
+
+  // std::ofstream OutFile("read_pcfile.txt");
+  while (input.read((char *)item, 16))
+  {
+    //std::cout << item[0] << " " << item[1] << " " << item[2]  << " " << item[3] <<std::endl;
+    pcl::PointXYZ point;
+    point.x = item[0];
+    point.y = item[1];
+    point.z = item[2];
+    
+    pl_full.push_back(point);
+    count_tmp++;
+  }
+  
+  // OutFile.close();
+  std::cout<<"read_pcfile.txt finished......... " << std::endl;
+  // exit(-1);
+  }
+  else if(strcmp(argv[1], "pcd") == 0){
+    std::cout<< "read pcd point cloud path" << argv[2] << std::endl;
+
+    std::string directory_path = std::string(argv[2]);
+    // Open the directory
+    DIR *dir = opendir(directory_path.c_str());
+    // Check if the directory was successfully opened
+    if (dir == NULL)
+    {
+        std::cerr << "Error opening directory: " << directory_path << std::endl;
+        return -1;
+    }
+    struct dirent *entry;
+    while ((entry = readdir(dir)) != nullptr)
+    {
+        // Check if the file extension is .pcd
+        std::string file_name = entry->d_name;
+        if (file_name.size() > 4 && file_name.substr(file_name.size() - 4) == ".pcd")
+        {
+            // Add the file path to the vector
+            file_paths.push_back(directory_path + file_name);
+        }
+    }
+  }
+
+  for (const auto &file_name : file_paths){
+    std::cout<<"file_name" <<file_name <<std::endl;
+    if (pcl::io::loadPCDFile<PointT> (file_name, pl_full) == -1){
+      // load the file
+      PCL_ERROR ("Couldn't read PCD file \n");
+      return (-1);
+    }
+
+    // 为保存的bbox.txt和 cluster.txt是根据pcd文件名命名的 便于区分
+    std::string tmp(file_name);
+    std::cout << "tmp: " << tmp << "\n";
+    size_t start_pos = tmp.find_last_of("/") + 1;
+    //size_t end_pos = tmp.find(".", start_pos);
+    size_t end_pos = tmp.find_last_of(".");
+    std::string number_str = tmp.substr(start_pos, end_pos - start_pos);
+    std::cout << "number_str: " << number_str << "\n";
+    std::string file_txt = "../result/test_mult/clusters_gpu_" + number_str + ".txt" ;
+
+
+    //cuda::utils::warmUpGPU();
+
+    printf("init pcd size:%d \n",pl_full.size());
+    // pcl::PointCloud<pcl::PointXYZ>::Ptr pl_full_ptr(new pcl::PointCloud<pcl::PointXYZ>(pl_full));
+    std::vector<int> indices;
+    // pcl::removeNaNFromPointCloud(*pl_full_ptr, *pl_full_ptr, indices);
+    pcl::removeNaNFromPointCloud(pl_full, pl_full, indices);
+    printf("remove NaN  pcd size:%d \n",pl_full.size());
+
+    for (int i=0; i<pl_full.size();i++){
+      if (pl_full[i].z > 1)
+        continue;
+      pl_filter_z.push_back(pl_full[i]);
+
+    }
+    
+    //printf("pl_filter_z size:%d \n",pl_filter_z.size());
+    int run_count = std::atoi(argv[3]); 
+    printf("=> read run_count: %d \n", run_count);
+    std::vector<float> timing(5, 0.0f);
+    for (int i = 0; i < run_count; i++)
+    {
+      //node.scanCallback(pl_filter_z,timing); //过滤point by z
+      node.scanCallback(pl_full,timing,file_txt,number_str);
+    }
+    
+    float p1 = timing[0] / run_count;
+    float p2 = timing[1] / run_count;
+    float p3 = timing[2] / run_count;
+
+    // printf("segment ground and obstacle: %.2f ms \n", p1);
+    // printf("process ground cloud: %.2f ms \n", p2);
+    printf("process obstacle_cloud cloud: %.2f ms \n", p3);
+
+  }
+}

test_project/linefit_ground_segmentation_local_gpu_test/src/ground_segmentation_node_single_test.cc

+#include <pcl/io/ply_io.h>
+// #include <pcl_ros/point_cloud.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/segmentation/progressive_morphological_filter.h>
+#include <pcl/common/common.h>
+#include <pcl/common/time.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl/common/transforms.h>
+#include <yaml-cpp/yaml.h>
+#include <cuda_runtime.h>
+#include "ground_segmentation/ground_segmentation.h"
+#include <dbscan/CudaUtils.cuh>
+#include <dbscan/DBSCAN.cuh>
+#include "AppUtility.hpp"
+
+
+//#define POINTS_TO_FILE  // 定义宏 生成txt 为可视化矩形框
+#define POINTS_TO_PLOYGON // 生成txt 为可视化对边形框
+
+namespace
+{
+  using PointCloudType = pcl::PointXYZ;
+  using PointCloud = pcl::PointCloud<PointCloudType>;
+  using PointCloudPtr = PointCloud::Ptr;
+  using DBSCAN = clustering::cuda::DBSCAN<PointCloudType>;
+
+  auto timer = pcl::StopWatch();
+  int NUM_TEST = 20;
+}  // namespace
+
+
+
+template <typename T = double> struct CudaTimer {
+  CudaTimer() {
+    //cudaDeviceSynchronize();
+    mCurTime = std::chrono::steady_clock::now();
+  }
+
+  T ticking() {
+    //cudaDeviceSynchronize();
+    auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now() - mCurTime);
+    T res = duration.count();
+    mCurTime = std::chrono::steady_clock::now();
+    return res * 1e-6;
+  }
+
+  private:
+    std::chrono::time_point<std::chrono::steady_clock> mCurTime;
+};
+
+// 定义包围盒信息
+struct BoxInfo {
+  Eigen::Vector3f center;
+  Eigen::Vector3f size;
+  Eigen::Quaternionf orientation;
+};
+
+void writeClusterToFile(const std::vector<PointCloudPtr>& clusters, const std::string& filename)
+{
+    //std::ofstream file(filename);
+    std::ofstream file(filename, std::ios::app);
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+    int index = 1;
+
+    for (auto& cluster : clusters) {
+        file << "cluster " << index << std::endl;
+        for (auto& point : cluster->points) {
+            file << point.x << ", " << point.y << ", " << point.z << std::endl;
+        }
+        ++index;
+    }
+    file.close();
+}
+
+void writeGroundPointToFile(pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_ground_cloud, const std::string& filename)
+{   
+    int index = 0;
+
+    std::ofstream file(filename);
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+    std::cout << "File created" << std::endl;
+    file << "cluster " << index << std::endl;
+   
+
+
+    // 遍历点云中的每个点
+    for (const auto& point : filter_ground_cloud->points)
+    {
+      // 以指定格式将点的坐标写入到文件中
+      file << point.x << ", " << point.y << ", " << point.z << endl;
+    }
+    file.close();
+}
+
+
+// for robosence solid lidar m1
+namespace robosence_ros {
+    struct EIGEN_ALIGN16 Point {
+        double timestamp;
+        PCL_ADD_POINT4D;
+        uint8_t intensity;
+        uint16_t ring;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+}  // namespace robosence_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(robosence_ros::Point,
+    (double, timestamp, timestamp)
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (uint8_t, intensity, intensity)
+    (uint16_t, ring, ring)
+)
+typedef pcl::PointXYZ PointType;
+typedef pcl::PointCloud<PointType> PointCloudXYZI;
+
+
+
+/*我们定义的分割节点的类*/
+class SegmentationNode {
+
+  GroundSegmentationParams params_;//地面分割的参数
+
+public:
+  SegmentationNode(const GroundSegmentationParams& params) : params_(params) {
+  
+  }
+
+  void getBoundingBoxInfo(PointCloudPtr cloud_cluster, Eigen::Vector3f& boxCenter, Eigen::Vector3f& size,Eigen::Quaternionf& boxOrientation)
+{   
+    //PCA 本质就是基于坐标系的转化
+
+    //5. 计算点云的几何中心和重心，注意这两个概念不一样，重心是所有点的平均值，中心是最大最小点的平均值
+    Eigen::Vector4f centroid;     // 重心
+    pcl::compute3DCentroid(*cloud_cluster, centroid);
+    PointType min_pt, max_pt;
+    Eigen::Vector3f center;       // 中心
+    pcl::getMinMax3D(*cloud_cluster, min_pt, max_pt); 
+    center = (max_pt.getVector3fMap() + min_pt.getVector3fMap())/2;
+    // 6. 计算协方差矩阵的特征向量
+    Eigen::Matrix3f covariance;
+    pcl::computeCovarianceMatrixNormalized(*cloud_cluster, centroid, covariance); // 这里必须用重心 重心作为PCA新坐标系的原点
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
+    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
+    Eigen::Vector3f eigenValuesPCA  = eigen_solver.eigenvalues();
+    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //正交化
+    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
+    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
+    // 按照特征值从大到小排列（eigenvalues函数返回的特征值是从小到大排列的）
+    Eigen::Matrix3f eigenVectorsPCA1;
+    eigenVectorsPCA1.col(0) = eigenVectorsPCA.col(2);
+    eigenVectorsPCA1.col(1) = eigenVectorsPCA.col(1);
+    eigenVectorsPCA1.col(2) = eigenVectorsPCA.col(0);
+    eigenVectorsPCA = eigenVectorsPCA1;
+
+    // 7. 计算变换矩阵，只考虑绕全局坐标系Z轴的转动
+    Eigen::Vector3f ea = (eigenVectorsPCA).eulerAngles(2, 1, 0); // 分别对应 yaw pitch roll
+    Eigen::AngleAxisf keep_Z_Rot(ea[0], Eigen::Vector3f::UnitZ());
+    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+    transform.translate(center);  // translate与rotate书写的顺序不可搞反
+    transform.rotate(keep_Z_Rot); // radians
+    
+    // 8. 计算包围盒的尺寸
+    pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
+    pcl::transformPointCloud(*cloud_cluster, *transformedCloud, transform.inverse());
+    // 重新计算变换后的点云的中心，因为变换前的点云的中心不是其包围盒的几何中心
+    PointType min_pt_T, max_pt_T;
+    pcl::getMinMax3D(*transformedCloud, min_pt_T, max_pt_T);
+    Eigen::Vector3f center_new = (max_pt_T.getVector3fMap() + min_pt_T.getVector3fMap()) / 2;
+    Eigen::Vector3f box_dim;
+    box_dim = max_pt_T.getVector3fMap() - min_pt_T.getVector3fMap();
+    Eigen::Affine3f transform2 = Eigen::Affine3f::Identity();
+    transform2.translate(center_new);
+    Eigen::Affine3f transform3 = transform * transform2;
+
+    size = box_dim;
+    boxCenter = transform3.translation(); // 这里用新的"中心"，因为要显示的包围盒采用几何中心作为参考点
+    boxOrientation = Eigen::Quaternionf(keep_Z_Rot);
+  
+}
+
+  void detectFod(pcl::PointCloud<pcl::PointXYZ>::Ptr& ground_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& obstacle_cloud,  pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_clustered, const std::string& filename,const std::string& number_str)
+  {
+
+    cloud_clustered->header = ground_cloud->header;
+
+    //auto t = CudaTimer<>();
+ 
+    // 2: 获取ground_cloud外接凸包
+    // pcl::ConvexHull<pcl::PointXYZ> hull;                  //创建凸包对象
+    // hull.setInputCloud(ground_cloud);                  //设置输入点云
+    // hull.setDimension(2);                                 //设置凸包维度
+    // std::vector<pcl::Vertices> polygons;                  //设置向量，用于保存凸包定点
+    // pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);//该点运用于描述凸包形状
+    // hull.reconstruct(*surface_hull, polygons);            //计算2D凸包结果
+
+    // int mid = surface_hull->points.size() / 2;
+    // if (mid-3 >= 0 && mid+3 < surface_hull->points.size())
+    // {
+    //   surface_hull->points[mid-3].x = 150;
+    //   surface_hull->points[mid-2].x = 150;
+    //   surface_hull->points[mid-1].x = 150;
+    //   surface_hull->points[mid].x = 150;
+    //   surface_hull->points[mid+3].x = 150;
+    //   surface_hull->points[mid+2].x = 150;
+    //   surface_hull->points[mid+1].x = 150;
+    // }
+
+    /*
+    for(int i = 0; i < surface_hull->points.size(); i++)
+    {
+      pcl::PointXYZ tmp;
+      tmp.x = surface_hull->points[i].x;
+      tmp.y = surface_hull->points[i].y;
+      tmp.z = surface_hull->points[i].z;
+      std::cout << "x = " << tmp.x << " y = " << tmp.y << " z = " << tmp.z << std::endl;
+
+    }
+    */
+
+    //std::cout << " " << std::endl;
+    // 3: 根据凸包对obstacle_cloud进行过滤，保留路面上的点云
+    printf("=> obstacle_cloud points size : %d  \n", obstacle_cloud->points.size());
+    
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
+
+    objects = obstacle_cloud ;
+    /* //liuji -  凸包
+    pcl::CropHull<pcl::PointXYZ> bb_filter;               //创建crophull对象
+    bb_filter.setDim(2);                                  //设置维度：该维度需要与凸包维度一致
+    bb_filter.setInputCloud(obstacle_cloud);              //设置需要滤波的点云
+    bb_filter.setHullIndices(polygons);                   //输入封闭多边形的顶点
+    bb_filter.setHullCloud(surface_hull);                 //输入封闭多边形的形状
+    bb_filter.filter(*objects);                           //执行CropHull滤波，存出结果在objects
+    */
+   
+
+
+    // std::cout << objects->size() << std::endl;            //
+    //auto k1= t.ticking();
+    //printf("=> get convex hull : %.2f ms \n", k1);
+    // 基于距离将点云分成两个，分别进行滤波
+    printf("=>obstacle_cloud crophull points size : %d  \n", objects->points.size());
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr far (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close (new pcl::PointCloud<pcl::PointXYZ>);
+
+    for(int i = 0; i < objects->points.size(); i++)
+    {
+      if (objects->points[i].x < 1000)
+      {
+        if (objects->points[i].z > -1.0)
+          close->points.push_back(objects->points[i]);
+      }
+      else
+        far->points.push_back(objects->points[i]);
+    }
+
+    printf("=>obstacle_cloud voxel before close points size : %d  \n", close->points.size());
+    //auto k2= t.ticking();
+    //printf("=> get close cloud : %.2f ms \n", k2);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::VoxelGrid<pcl::PointXYZ> vor;  //滤波处理对象
+    vor.setInputCloud(close);
+    vor.setLeafSize(0.10f, 0.10f, 0.10f);  //设置滤波器处理时采用的体素大小的参数
+    vor.filter(*close_voxel_filtered);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    vor.filter(*close_objects_filtered);
+    //auto k3= t.ticking();
+    //printf("=> Voxel close cloud : %.2f ms \n", k3);
+    // 4: 获取点云目标检测结果，对路面点云进行目标过滤
+    /* //liuji  注释
+    pcl::PointCloud<pcl::PointXYZ>::Ptr close_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr far_objects_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> ssor;//创建滤波器对象
+    ssor.setInputCloud(close_voxel_filtered);                       //设置待滤波的点云
+    ssor.setMeanK(50);                                 //设置在进行统计时考虑查询点临近点数
+    ssor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值
+    ssor.filter(*close_objects_filtered);                    //存储
+    */
+
+    //auto k4= t.ticking();
+    //printf("=> OutlierRemoval close cloud : %.2f ms \n", k4);
+    //pcl::StatisticalOutlierRemoval<pcl::PointXYZ> fsor;//创建滤波器对象
+    //fsor.setInputCloud(far);                       //设置待滤波的点云
+    //fsor.setMeanK(5);                                 //设置在进行统计时考虑查询点临近点数
+    //fsor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值
+    //fsor.filter(*far_objects_filtered);                    //存储
+    printf("=>close_voxel_filtered  points  size : %d  \n", close_voxel_filtered->points.size());
+
+    if (close_objects_filtered->points.size() < 10)
+      return;
+    
+    
+    // 5: 对obstacle_cloud进行dbscan聚类，得到每个障碍物
+
+    auto t = CudaTimer<>();
+
+    int numPoints = close_objects_filtered->points.size();
+    printf("=>close_objects_filtered voxel(dbscan before) after points  size : %d  \n", numPoints);
+   
+    PointType* points = close_objects_filtered->points.data();
+    DBSCAN::Param param{.pointDimension = 3, .eps = 0.8, .minPoints = 100};
+    DBSCAN dbscanHandler(param);
+    std::vector<std::vector<int>> clusterIndices;
+
+    clusterIndices = dbscanHandler.run(points, numPoints);
+    std::vector<PointCloudPtr> clusters = ::toClusters<PointCloudType>(clusterIndices, close_objects_filtered);
+    auto k = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k);
+/* 原始cpu DBSCAN算法使用的是 KdTree
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(close_objects_filtered);
+    // Segmentation, [Euclidean Cluster Extraction](https://pcl.readthedocs.io/projects/tutorials/en/latest/cluster_extraction.html#cluster-extraction)
+    std::vector<pcl::PointIndices> cluster_indices;
+    auto k5= t.ticking();
+    //printf("=> KdTree close cloud : %.2f ms \n", k5);
+    DBSCANKdtreeCluster ec;
+    ec.setCorePointMinPts(20);//一个核心点邻域最少点数
+    ec.setClusterTolerance(0.8);  //算法半径
+    ec.setMinClusterSize(100);   //一个簇最少点数
+	//ec.setMinClusterSize(10);
+    ec.setMaxClusterSize(99999);  //一个簇最大点数
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(close_objects_filtered);
+	//ec.setInputCloud(close_voxel_filtered);
+    ec.extract(cluster_indices);
+    auto k7= t.ticking();
+    //printf("=> extract close cloud : %.2f ms \n", k7);
+    auto k6 = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k6);
+    printf("=> cluster_indices size : %.d \n", cluster_indices.size());
+*/
+
+
+/* 对每个簇进行处理，将point的值获取到
+    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++) {
+      for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
+            pcl::PointXYZ tmp;
+            tmp.x = close_objects_filtered->points[*pit].x;
+            tmp.y = close_objects_filtered->points[*pit].y;
+            tmp.z = close_objects_filtered->points[*pit].z;
+            // tmp.intensity = j%8;
+            // tmp.ring = objects->points[*pit].ring;
+            if (tmp.z < -1.4 || tmp.z > 4.2)
+              continue;
+            cloud_clustered->points.push_back(tmp);
+      }
+    }
+
+*/
+    //该循坏只是把聚类后的点云获取到，并没有去做分类处理
+    printf("=> clusters size : %.d \n", clusters.size());
+    auto k1 = t.ticking();
+    //printf("=> DBSCAN obstacle_cloud : %.2f ms \n", k);
+    std::string bbox_txt = "../result/test_mult/clusters_bbox_" + number_str + ".txt" ;
+    ofstream outfile(bbox_txt);
+    for (int i  = 0; i < clusters.size(); i++) {
+      #ifdef POINTS_TO_FILE
+      Eigen::Vector3f boxCenter, boxSize;
+      Eigen::Quaternionf boxOrientation;
+      getBoundingBoxInfo(clusters[i], boxCenter, boxSize, boxOrientation);
+      std::cout << "cluster: " << i << "\n";
+      std::cout  << "box center: " << boxCenter.transpose() << "\n";
+      std::cout  << "box size: " << boxSize.transpose() << "\n";
+      std::cout  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n";
+      outfile << "cluster: " << i << "\n";
+      outfile  << "box center: " << boxCenter.transpose() << "\n";
+      outfile  << "box size: " << boxSize.transpose() << "\n";
+      outfile  << "box orientation: " << boxOrientation.coeffs().transpose() << "\n";
+      #endif
+      for (auto it = clusters[i]->begin(); it !=clusters[i]->end(); ++it){
+        pcl::PointXYZ tmp;
+        tmp.x = it->x;
+        tmp.y = it->y;
+        tmp.z = it->z;
+        if (tmp.z < -1.4 || tmp.z > 4.2)
+                  continue;
+        cloud_clustered->points.push_back(tmp);
+      }
+    }
+    outfile.close();
+    printf("=> final obstacle_cloud clustered points size : %.d \n", cloud_clustered->points.size());
+    #ifdef POINTS_TO_FILE
+    writeClusterToFile(clusters,filename);
+    #endif
+
+    for(int i = 0; i < far->points.size(); i++)
+    {
+      if (far->points[i].z < -1.4 || far->points[i].z > 4.2)
+        continue;
+      cloud_clustered->points.push_back(far->points[i]);
+    }
+    //printf("=> processs result : %.2f ms \n", k1);
+    // std::cout<<"cluster size = "<<cloud_clustered->points.size()<<endl;
+    
+  }
+
+  void processGround(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_ground_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_ground_cloud,const std::string& filename)
+  {
+
+    // // 1. 过滤离群点
+    // pcl::PointCloud<pcl::PointXYZ>::Ptr outlier_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+    // pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;//创建滤波器对象
+    // sor.setInputCloud(source_ground_cloud); //设置待滤波的点云
+    // sor.setMeanK(50);     //设置在进行统计时考虑查询点临近点数
+    // sor.setStddevMulThresh(1.0);      //设置判断是否为离群点的阀值
+    // sor.filter(*outlier_filtered);      //存储
+ 
+    // 体素滤波
+    pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::VoxelGrid<pcl::PointXYZ> ssor;  //滤波处理对象
+    ssor.setInputCloud(source_ground_cloud);
+    ssor.setLeafSize(0.15f, 0.15f, 0.15f);  //设置滤波器处理时采用的体素大小的参数
+    // ssor.filter(*voxel_filtered);
+	  ssor.filter(*filter_ground_cloud);
+    #ifdef POINTS_TO_FILE
+    writeGroundPointToFile(filter_ground_cloud,filename);
+    #endif
+	/*
+    std::chrono::high_resolution_clock::time_point s2 = std::chrono::high_resolution_clock::now();
+    // 欧式聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(voxel_filtered);
+    // Segmentation, [Euclidean Cluster Extraction](https://pcl.readthedocs.io/projects/tutorials/en/latest/cluster_extraction.html#cluster-extraction)
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+
+    ec.setClusterTolerance(0.8);  //算法半径
+    ec.setMinClusterSize(100);   //一个簇最少点数
+    ec.setMaxClusterSize(99999);  //一个簇最大点数
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(voxel_filtered);
+    ec.extract(cluster_indices);
+    std::chrono::high_resolution_clock::time_point s3 = std::chrono::high_resolution_clock::now();
+    // 提取聚类中最大的簇
+    std::vector<int> max_cluster;
+    int max_size = 0;
+
+    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+    {
+        //std::cout << it->indices.size() << std::endl;
+        if (it->indices.size() > max_size)
+        {
+            // std::cout << it->indices.size() << std::endl;
+            max_size = it->indices.size();
+            max_cluster = it->indices;
+        }
+    }
+    //std::cout << "max_cluster size = " << max_cluster.size() << std::endl;
+    //std::cout << "max_size = " << max_size << std::endl;
+    for (std::vector<int>::const_iterator pit = max_cluster.begin(); pit != max_cluster.end(); ++pit)
+        filter_ground_cloud->points.push_back(voxel_filtered->points[*pit]); 
+    filter_ground_cloud->header = source_ground_cloud->header;
+    std::chrono::high_resolution_clock::time_point s4 = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double, std::milli> s1_ms = s1 - start;
+    std::chrono::duration<double, std::milli> s2_ms = s2 - start;
+    std::chrono::duration<double, std::milli> s3_ms = s3 - start;
+    std::chrono::duration<double, std::milli> s4_ms = s4 - start;
+	*/
+    // std::cout << "s1_ms = " << s1_ms.count() << " ms" << " s2_ms = " << s2_ms.count() << " s3_ms = " << s3_ms.count() << " ms" << " s4_ms = " << s4_ms.count() << " ms\n";
+  }
+
+
+
+  /*回调函数*/
+
+  void scanCallback(pcl::PointCloud<pcl::PointXYZ>& pl_full,std::vector<float>& timing, const std::string& filename,std::string& number_str) {
+
+   
+    // m1点云数据格式转换
+    //pcl::PointCloud<robosence_ros::Point> pl_orig; //pcl::PointCloud是一个模板类pcl::PointCloud<pcl::PointXYZ>表示由三维点组成的点云
+    
+
+    /*将我们设定的参数传递进去，这里就直接跳转到linefit_ground_segmentation之中*/
+    auto s = CudaTimer<>(); 
+    GroundSegmentation segmenter(params_);
+    /*定义一个是否属于地面点的标签，来进行区分*/
+    std::vector<int> labels;
+
+    /*这个是地面分割类中的主要函数，分割函数9*/
+    segmenter.segment(pl_full, &labels);
+
+    auto k1 = s.ticking();
+    //printf("=> segment ground and obstacle: %.2f ms \n", k1);
+
+    /*定义了两种点云，一个是地面点，一个是障碍物点*/
+    // pcl::PointCloud<pcl::PointXYZ> ground_cloud, obstacle_cloud;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, obstacle_cloud;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_cloud, cloud_clustered;
+    ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    obstacle_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    filter_ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    cloud_clustered.reset(new pcl::PointCloud<pcl::PointXYZ>());
+  
+    /*将整个的无序点云按照标签分配到不同的点云之中*/
+    /*通过这里的标签对于是否属于地面点进行了划分*/
+    // for (size_t i = 0; i < pl_full.size(); ++i) {
+    //   if (labels[i] == 1) ground_cloud.push_back(pl_full[i]);
+    //   else obstacle_cloud.push_back(pl_full[i]);
+    // }
+
+    for (size_t i = 0; i < pl_full.size(); ++i) {
+      if (labels[i] == 1) ground_cloud->points.push_back(pl_full[i]);
+      else obstacle_cloud->points.push_back(pl_full[i]);
+    }
+    
+    std::cout << " process ground_cloud before size = " << ground_cloud->points.size() << std::endl;
+    processGround(ground_cloud, filter_ground_cloud, filename);
+    std::cout << " process ground_cloud after size  = " << filter_ground_cloud->points.size() << std::endl;
+
+    auto k2 = s.ticking();
+    //printf("=> process ground cloud: %.2f ms \n", k2);
+    //std::cout << " filter_ground_cloud.size() = " << filter_ground_cloud->points.size() << std::endl;
+    detectFod(filter_ground_cloud, obstacle_cloud, cloud_clustered, filename, number_str);
+
+    auto k3 = s.ticking();
+    //printf("=> process obstacle_cloud cloud: %.2f ms \n", k3);
+   
+    //std::cout << "s1_ms = " << s1_ms.count() << " ms" << " s2_ms = " << s2_ms.count() << " s3_ms = " << s3_ms.count() << " ms" << " s4_ms = " << s4_ms.count() << " ms\n";
+    /*将按照标签分类好的数据分配到不同的点云之中*/
+    timing[0] += k1;
+    timing[1] += k2;
+    timing[2] += k3;
+
+  }
+
+
+// void fodCallback(const sensor_msgs::PointCloud2::ConstPtr &msg){
+//     // pcl::PointCloud<pcl::PointXYZ> ground;
+//     pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, obstacle_cloud;
+//     pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_cloud, cloud_clustered;
+//     ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     obstacle_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     filter_ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     cloud_clustered.reset(new pcl::PointCloud<pcl::PointXYZ>());
+//     pcl::fromROSMsg(*msg, *ground_cloud);
+//     std::cout << "ground_cloud->points().size() = " << ground_cloud->points.size() << std::endl;
+//     processGround(ground_cloud, filter_ground_cloud);
+//     detectFod(filter_ground_cloud, obstacle_cloud, cloud_clustered);
+//     obstacle_pub_.publish(*cloud_clustered);
+// }
+
+
+  
+  // void scanCallback(const pcl::PointCloud<pcl::PointXYZ>& cloud) {
+  //   /*将我们设定的参数传递进去，这里就直接跳转到linefit_ground_segmentation之中*/
+  //   GroundSegmentation segmenter(params_);
+  //   /*定义一个是否属于地面点的标签，来进行区分*/
+  //   std::vector<int> labels;
+
+  //   /*这个是地面分割类中的主要函数，分割函数9*/
+  //   segmenter.segment(cloud, &labels);
+  //   /*定义了两种点云，一个是地面点，一个是障碍物点*/
+  //   pcl::PointCloud<pcl::PointXYZ> ground_cloud, obstacle_cloud;
+  //   ground_cloud.header = cloud.header;
+  //   obstacle_cloud.header = cloud.header;
+  //   /*将整个的无序点云按照标签分配到不同的点云之中*/
+  //   /*通过这里的标签对于是否属于地面点进行了划分*/
+  //   for (size_t i = 0; i < cloud.size(); ++i) {
+  //     if (labels[i] == 1) ground_cloud.push_back(cloud[i]);
+  //     else obstacle_cloud.push_back(cloud[i]);
+  //   }
+  //   /*将按照标签分类好的数据分配到不同的点云之中*/
+  //   ground_pub_.publish(ground_cloud);
+  //   obstacle_pub_.publish(obstacle_cloud);
+  // }
+  
+};
+
+
+
+/*主函数，给出了ros的接口，跳转到linefir_ground_segmentation之中*/
+int main(int argc, char** argv) {
+
+  /*调用glog中的函数来初始化日志，为后面做准备*/
+
+
+  std::string file_path = "../launch/rv4.yaml"; 
+
+  
+
+  std::ifstream fin(file_path);
+  if(!fin.good()){
+    std::cerr<< "Faild open the file : "<<file_path<<std::endl;
+    return 1;
+  }
+  fin.close();
+
+  YAML::Node config = YAML::LoadFile(file_path);
+
+
+  // Do parameter stuff.
+  /*参数设定，可以读入我们在yaml中设定的参数*/
+  GroundSegmentationParams params;
+  params.visualize = config["visualize"].as<bool>();
+  params.n_bins = config["n_bins"].as<int>();
+  params.n_segments = config["n_segments"].as<int>();
+  params.max_dist_to_line = config["max_dist_to_line"].as<double>();
+  params.max_slope = config["max_slope"].as<double>();
+  params.long_threshold = config["long_threshold"].as<double>();
+  params.max_long_height = config["max_long_height"].as<double>();
+  params.max_start_height = config["max_start_height"].as<double>();
+  params.sensor_height = config["sensor_height"].as<double>();
+  params.line_search_angle = config["line_search_angle"].as<double>();
+  params.n_threads= config["n_threads"].as<int>();
+
+  double r_min, r_max, max_fit_error;
+  r_min = config["r_min"].as<double>();
+  r_max = config["r_max"].as<double>();
+  max_fit_error = config["max_fit_error"].as<double>();
+
+  params.r_min_square = r_min*r_min;
+  params.r_max_square = r_max*r_max;
+  params.max_error_square = max_fit_error * max_fit_error;
+  std::cout<<"r_min_square :" <<params.r_min_square<<std::endl;
+/*
+  nh.param("visualize", params.visualize, params.visualize);
+  nh.param("n_bins", params.n_bins, params.n_bins);
+  nh.param("n_segments", params.n_segments, params.n_segments);
+  nh.param("max_dist_to_line", params.max_dist_to_line, params.max_dist_to_line);
+  nh.param("max_slope", params.max_slope, params.max_slope);
+  nh.param("long_threshold", params.long_threshold, params.long_threshold);
+  nh.param("max_long_height", params.max_long_height, params.max_long_height);
+  nh.param("max_start_height", params.max_start_height, params.max_start_height);
+  nh.param("sensor_height", params.sensor_height, params.sensor_height);
+  nh.param("line_search_angle", params.line_search_angle, params.line_search_angle);
+  nh.param("n_threads", params.n_threads, params.n_threads);
+   
+  std::string ground_topic, obstacle_topic, input_topic;
+  bool latch;
+  //input topic是需要我们自己设定的，根据自己雷达的数据而定
+  // Params that need to be squared.
+  nh.param<std::string>("input_topic", input_topic, "input_cloud");
+  nh.param<std::string>("ground_output_topic", ground_topic, "ground_cloud");
+  nh.param<std::string>("obstacle_output_topic", obstacle_topic, "obstacle_cloud");
+  nh.param("latch", latch, false);
+*/
+  
+
+
+  // Start node.
+  /*我们初始化类，从类中调到别的地方*/
+  SegmentationNode node(params);
+
+  typedef pcl::PointXYZ PointT;
+  pcl::PointCloud<pcl::PointXYZ> pl_full;
+  pcl::PointCloud<pcl::PointXYZ> pl_filter_z;
+
+  if(strcmp(argv[1], "bin") == 0){
+  std::cout<< "read bin point cloud " << argv[2] << std::endl;
+  std::fstream input(argv[2], std::ios::in | std::ios::binary);
+  if(!input.good()){
+    std::cerr << "Could not read file: " << argv[2] << std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  float item[4];
+  int count_tmp = 0;
+  // printf("count_tmp start at : %d....\n", count_tmp);
+
+  // std::ofstream OutFile("read_pcfile.txt");
+  while (input.read((char *)item, 16))
+  {
+    //std::cout << item[0] << " " << item[1] << " " << item[2]  << " " << item[3] <<std::endl;
+    pcl::PointXYZ point;
+    point.x = item[0];
+    point.y = item[1];
+    point.z = item[2];
+    
+    pl_full.push_back(point);
+    count_tmp++;
+  }
+  
+  // OutFile.close();
+  std::cout<<"read_pcfile.txt finished......... " << std::endl;
+  // exit(-1);
+  }
+  else if(strcmp(argv[1], "pcd") == 0){
+    std::cout<< "read pcd point cloud " << argv[2] << std::endl;
+    std::string file_name = std::string(argv[2]);
+
+    if (pcl::io::loadPCDFile<PointT> (file_name, pl_full) == -1){
+      // load the file
+      PCL_ERROR ("Couldn't read PCD file \n");
+      return (-1);
+    }
+  }
+
+
+  // 为保存的bbox.txt和 cluster.txt是根据pcd文件名命名的 便于区分
+  std::string tmp(argv[2]);
+  std::cout << "tmp: " << tmp << "\n";
+  size_t start_pos = tmp.find_last_of("/") + 1;
+  //size_t end_pos = tmp.find(".", start_pos);
+  size_t end_pos = tmp.find_last_of(".");
+  std::string number_str = tmp.substr(start_pos, end_pos - start_pos);
+  std::cout << "number_str: " << number_str << "\n";
+  std::string file_txt = "../result/test_mult/clusters_gpu_" + number_str + ".txt" ;
+
+
+  //cuda::utils::warmUpGPU();
+
+  printf("init pcd size:%d \n",pl_full.size());
+  // pcl::PointCloud<pcl::PointXYZ>::Ptr pl_full_ptr(new pcl::PointCloud<pcl::PointXYZ>(pl_full));
+  std::vector<int> indices;
+  // pcl::removeNaNFromPointCloud(*pl_full_ptr, *pl_full_ptr, indices);
+  pcl::removeNaNFromPointCloud(pl_full, pl_full, indices);
+  printf("rmove NaN  pcd size:%d \n",pl_full.size());
+
+  for (int i=0; i<pl_full.size();i++){
+    if (pl_full[i].z > 1)
+      continue;
+    pl_filter_z.push_back(pl_full[i]);
+
+  }
+  
+  //printf("pl_filter_z size:%d \n",pl_filter_z.size());
+  int run_count = std::atoi(argv[3]); 
+  printf("=> read run_count: %d \n", run_count);
+  std::vector<float> timing(5, 0.0f);
+  for (int i = 0; i < run_count; i++)
+  {
+    //node.scanCallback(pl_filter_z,timing); //过滤point by z
+    node.scanCallback(pl_full,timing,file_txt,number_str);
+  }
+  
+
+
+  float p1 = timing[0] / run_count;
+  float p2 = timing[1] / run_count;
+  float p3 = timing[2] / run_count;
+
+  // printf("segment ground and obstacle: %.2f ms \n", p1);
+  // printf("process ground cloud: %.2f ms \n", p2);
+  printf("process obstacle_cloud cloud: %.2f ms \n", p3);
+}

test_project/linefit_ground_segmentation_local_gpu_test/src/segment.cc

+#include "ground_segmentation/segment.h"
+
+Segment::Segment(const unsigned int& n_bins,
+                 const double& max_slope,
+                 const double& max_error,
+                 const double& long_threshold,
+                 const double& max_long_height,
+                 const double& max_start_height,
+                 const double& sensor_height) :
+                 bins_(n_bins),
+                 max_slope_(max_slope),
+                 max_error_(max_error),
+                 long_threshold_(long_threshold),
+                 max_long_height_(max_long_height),
+                 max_start_height_(max_start_height),
+                 sensor_height_(sensor_height){}
+
+/*分割线拟合*/
+void Segment::fitSegmentLines() {
+  // Find first point.
+  auto line_start = bins_.begin();
+  /*在整个的bins中找第一个点*/
+  while (!line_start->hasPoint()) {
+    ++line_start;
+    // Stop if we reached last point.
+    if (line_start == bins_.end()) return;
+  }
+  // Fill lines.
+  bool is_long_line = false;
+  double cur_ground_height = -sensor_height_;
+  /*将线的第一个点的信息传递到*/
+  std::list<Bin::MinZPoint> current_line_points(1, line_start->getMinZPoint());
+  LocalLine cur_line = std::make_pair(0,0);
+  /*从第一个点开始对于bins上的每一个点都进行遍历操作*/
+  for (auto line_iter = line_start+1; line_iter != bins_.end(); ++line_iter) {
+    /*如果我们设定的线上有点，则进行后面的操作*/
+    if (line_iter->hasPoint()) {
+      Bin::MinZPoint cur_point = line_iter->getMinZPoint();
+      /*如果两个的线的长度大于我们设定的阈值，则我们认为这两个点之间是长线*/
+      if (cur_point.d - current_line_points.back().d > long_threshold_) is_long_line = true;
+      /*针对于后面几次遍历而言的，至少有三个点的情况下*/
+      if (current_line_points.size() >= 2) {
+
+        // Get expected z value to possibly reject far away points.
+        /*获取远离点的z值*/
+        double expected_z = std::numeric_limits<double>::max();
+        /*如果是长线段且当前线段的点数大于2，则我们获取到期待的z，这个在第一次迭代的时候不会被执行*/
+        if (is_long_line && current_line_points.size() > 2) {
+          expected_z = cur_line.first * cur_point.d + cur_line.second;
+        }
+        /*将当前点插入到current_line之中*/
+        current_line_points.push_back(cur_point);
+        /*对于当前线点传入到本地线拟合中，得到最后的结果*/
+        cur_line = fitLocalLine(current_line_points);
+        /*将我们经过本地线拟合之后的*/
+        const double error = getMaxError(current_line_points, cur_line);
+        // Check if not a good line.
+        /*将算出来的误差和最大误差进行比较，判断是否是一个合格的线*/
+        if (error > max_error_ ||
+            std::fabs(cur_line.first) > max_slope_ ||
+            is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_) {
+          // Add line until previous point as ground.
+          /*添加线直到浅一点是地面点*/
+          current_line_points.pop_back();
+          // Don't let lines with 2 base points through.
+          /*不要让有两个基点的线穿过*/
+          if (current_line_points.size() >= 3) {
+            /*对于当前线点进行本地拟合，得到一条新的线*/
+            const LocalLine new_line = fitLocalLine(current_line_points);
+            /*将进行处理后的点放入到线中*/
+            lines_.push_back(localLineToLine(new_line, current_line_points));
+            /*计算出当前地面点的高度*/
+            cur_ground_height = new_line.first * current_line_points.back().d + new_line.second;
+          }
+          // Start new line.
+          is_long_line = false;
+          /*erase在删除的过程中还是减少vector的size*/
+          current_line_points.erase(current_line_points.begin(), --current_line_points.end());
+          --line_iter;
+        }
+        // Good line, continue.
+        else { }
+      }
+    /*在有1到2个点的情况下的处理*/
+      else {
+        // Not enough points.
+        /*判断是否满足添加条件，添加这些点*/
+        if (cur_point.d - current_line_points.back().d < long_threshold_ &&
+            std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_) {
+          // Add point if valid.
+          current_line_points.push_back(cur_point);
+        }
+        /*开始一条新的线*/
+        else {
+          // Start new line.
+          current_line_points.clear();
+          current_line_points.push_back(cur_point);
+        }
+      }
+    }
+  }
+  // Add last line.
+  /*添加最后一条线*/
+  if (current_line_points.size() > 2) {
+    const LocalLine new_line = fitLocalLine(current_line_points);
+    lines_.push_back(localLineToLine(new_line, current_line_points));
+  }
+}
+
+/*本地线到线，得到两个点，构建出一条直线*/
+Segment::Line Segment::localLineToLine(const LocalLine& local_line,
+                                       const std::list<Bin::MinZPoint>& line_points) {
+  Line line;
+  const double first_d = line_points.front().d;
+  const double second_d = line_points.back().d;
+  /*跟去前面的值来进行locl_line的处理*/
+  const double first_z = local_line.first * first_d + local_line.second;
+  const double second_z = local_line.first * second_d + local_line.second;
+  line.first.z = first_z;
+  line.first.d = first_d;
+  line.second.z = second_z;
+  line.second.d = second_d;
+  return line;
+}
+
+/*到线的垂直距离*/
+double Segment::verticalDistanceToLine(const double &d, const double &z) {
+  static const double kMargin = 0.1;
+  double distance = -1;
+  for (auto it = lines_.begin(); it != lines_.end(); ++it) {
+    /*这里设定了论文中要求的距离*/
+    /*针对于d点，按照设定的余量在前后范围内找到两个点*/
+    if (it->first.d - kMargin < d && it->second.d + kMargin > d) {
+      /*这里是先算出斜率，将传入的两个点传入到直线中，算出一个最近的额z值差，也就是垂直的距离*/
+      /*算出找到的两个点之间的斜率*/
+      const double delta_z = it->second.z - it->first.z;
+      const double delta_d = it->second.d - it->first.d;
+      const double expected_z = (d - it->first.d)/delta_d *delta_z + it->first.z;//(delta_z/delta_d)是一个斜率
+      //算出最终的距离
+      distance = std::fabs(z - expected_z);
+    }
+  }
+  return distance;
+}
+
+double Segment::getMeanError(const std::list<Bin::MinZPoint> &points, const LocalLine &line) {
+  double error_sum = 0;
+  for (auto it = points.begin(); it != points.end(); ++it) {
+    const double residual = (line.first * it->d + line.second) - it->z;
+    error_sum += residual * residual;
+  }
+  return error_sum/points.size();
+}
+
+double Segment::getMaxError(const std::list<Bin::MinZPoint> &points, const LocalLine &line) {
+  double max_error = 0;
+  for (auto it = points.begin(); it != points.end(); ++it) {
+    const double residual = (line.first * it->d + line.second) - it->z;
+    const double error = residual * residual;
+    if (error > max_error) max_error = error;
+  }
+  return max_error;
+}
+
+/*本地线拟合*/
+Segment::LocalLine Segment::fitLocalLine(const std::list<Bin::MinZPoint> &points) {
+  const unsigned int n_points = points.size();
+  Eigen::MatrixXd X(n_points, 2);
+  Eigen::VectorXd Y(n_points);
+  unsigned int counter = 0;
+  for (auto iter = points.begin(); iter != points.end(); ++iter) {
+    X(counter, 0) = iter->d;
+    X(counter, 1) = 1;
+    Y(counter) = iter->z;
+    ++counter;
+  }
+  Eigen::VectorXd result = X.colPivHouseholderQr().solve(Y);
+  LocalLine line_result;
+  line_result.first = result(0);
+  line_result.second = result(1);
+  return line_result;
+}
+
+bool Segment::getLines(std::list<Line> *lines) {
+  if (lines_.empty()) {
+    return false;
+  }
+  else {
+    *lines = lines_;
+    return true;
+  }
+}