调整代码结构

cloud_common/CMakeLists.txt

+project(CloudCommon)
+cmake_minimum_required(VERSION 3.10)
+set(CMAKE_CXX_STANDARD 14)
+#ADD_COMPILE_OPTIONS(-std=c++14 )
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake)
+
+IF (WIN32)
+    set(PCL_DIR G:/PCL1.11/)
+    add_definitions("-DGLOG_NO_ABBREVIATED_SEVERITIES")
+    add_definitions("-DGOOGLE_GLOG_DLL_DECL=")
+    include_directories(${PCL_DIR}/include/pcl-1.11)
+    include_directories(${PCL_DIR}/3rdParty/VTK/include/vtk-8.2)
+    include_directories(${PCL_DIR}/3rdParty/Boost/include/boost-1_74)
+    include_directories(${PCL_DIR}/3rdParty/Eigen/eigen3)
+    include_directories(${PCL_DIR}/3rdParty/FLANN/include)
+    include_directories(${PCL_DIR}/3rdParty/Qhull/include)
+
+    include_directories(denpends/glog/win_x64/include)
+    include_directories(denpends/gflags/win_x64/include)
+    include_directories(denpends/GeographicLib/win_x64/include)
+
+    SET(PCL_LIBRARIES libboost_system-vc142-mt-x64-1_74 libboost_thread-vc142-mt-x64-1_74
+            libboost_filesystem-vc142-mt-x64-1_74
+            vtkCommonCore-8.2
+            pcl_common pcl_kdtree pcl_io pcl_ml pcl_visualization pcl_filters pcl_segmentation pcl_search pcl_features
+            pcl_octree pcl_surface flann_cpp OpenGL32 GLU32
+            gflags_nothreads_static glog shlwapi)
+
+    link_directories(${PCL_DIR}/lib)
+    link_directories(${PCL_DIR}/3rdParty/VTK/lib/)
+    link_directories(${PCL_DIR}/3rdParty/Boost/lib/)
+    link_directories(${PCL_DIR}/3rdParty/FLANN/lib/)
+    link_directories(denpends/glog/win_x64/lib/Release)
+    link_directories(denpends/gflags/win_x64/lib/Release)
+    link_directories(denpends/GeographicLib/win_x64/lib/Release)
+ELSEIF (APPLE)
+    MESSAGE(STATUS "Now is Apple systems.")
+ELSEIF (UNIX)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+    set(PCL_INCLUDE_DIRS /usr/include/pcl-1.10)
+    set(PCL_LIBRARY_DIRS /usr/lib/x86_64-linux-gnu)
+    set(CV_DIR ./denpends/opencv4)
+    set(INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/RoadDetector)
+
+    include_directories(${CV_DIR}/include/opencv4)
+    include_directories(../glog/include)
+    include_directories(../gflags/include)
+    include_directories(../GeographicLib/include)
+    include_directories(../libjsoncpp/include)
+    include_directories(.)
+    include_directories(/usr/include/pcl-1.10)
+    include_directories(/usr/include/vtk-7.1)
+    include_directories(/usr/include/eigen3)
+    include_directories(/usr/include/gdal/)
+
+    link_directories(/usr/local/lib)
+    link_directories(/usr/lib/)
+    link_directories(/usr/lib/x86_64-linux-gnu)
+    link_directories(${CV_DIR}/lib/)
+
+    message(CV_DIR=${CV_DIR})
+    link_directories(../glog/lib)
+    link_directories(../gflags/lib)
+    link_directories(../GeographicLib/lib)
+
+    link_directories(${PCL_LIBRARY_DIRS})
+    SET(PCL_LIBRARIES boost_system boost_thread
+            pcl_segmentation pcl_kdtree pcl_ml pcl_filters pcl_search pcl_features
+            pcl_surface flann_cpp pcl_octree pcl_io pcl_common lz4
+            glog gflags)
+
+    set(CV_LIBRARIES opencv_imgproc opencv_imgcodecs opencv_highgui opencv_calib3d opencv_features2d
+            opencv_objdetect opencv_flann  opencv_core)
+ENDIF ()
+add_definitions(${PCL_DEFINITIONS})
+
+
+add_subdirectory(common)
+add_subdirectory(utility)
\ No newline at end of file

cloud_common/common/CMakeLists.txt

+cmake_minimum_required(VERSION 3.10)
+
+file(GLOB_RECURSE CPP_FILES *.cpp *.h *.hpp)
+
+add_library(common ${CPP_FILES})
+target_link_libraries(common ${PCL_LIBRARIES})
+
+install(TARGETS common
+        DESTINATION  ${CMAKE_SOURCE_DIR}/lib
+        )
\ No newline at end of file

cloud_common/common/Cell.h

+//
+// Created by xueyimeng on 21-3-14.
+//
+
+#ifndef LIDARGROUNDDETECT_CELL_H
+#define LIDARGROUNDDETECT_CELL_H
+
+#include <vector>
+#include "TypeDef.h"
+
+class Cell {
+public:
+    Cell() {}
+    virtual ~Cell() {}
+
+public:
+    int cell_id;
+    std::vector<RefPointInfo> pt_idxs;
+};
+typedef std::shared_ptr<Cell> CellPtr;
+#endif //LIDARGROUNDDETECT_CELL_H

cloud_common/common/Grid.h

+//
+// Created by xueyimeng on 20-12-8.
+//
+
+#ifndef LIDARGROUNDDETECT_GRID_H
+#define LIDARGROUNDDETECT_GRID_H
+
+#include <vector>
+#include <memory>
+#include <float.h>
+#include <set>
+#include "Cell.h"
+#include <Eigen/Dense>
+
+class GridCell : public Cell{
+public:
+    GridCell() {
+        road_planeInfo = nullptr;
+        max_elev = -DBL_MAX;
+        min_elev = DBL_MAX;
+        next_level_cell = nullptr;
+        avg_elev = -DBL_MAX;
+    }
+
+    virtual ~GridCell() {
+        std::vector<RefPointInfo>().swap(pt_idxs);
+    }
+
+public:
+    float max_elev;
+    float min_elev;
+    float avg_elev;
+    PlanePtr road_planeInfo;
+    Eigen::Vector3d vec_track_dir;
+    bool is_flat_area;
+    std::shared_ptr<GridCell> next_level_cell; //for bridge case
+};
+typedef std::shared_ptr<GridCell> GridCellPtr;
+
+class RoadGrid {
+public:
+    RoadGrid() {}
+    virtual ~RoadGrid() {
+        std::vector<GridCellPtr>().swap(vec_grid_cells);
+    }
+
+public:
+    float DistanceBetweenCell(int c1_id, int c2_id) {
+        GridCellPtr c1 = vec_grid_cells[c1_id];
+        GridCellPtr c2 = vec_grid_cells[c2_id];
+
+        if(nullptr == c1 || nullptr == c2) {
+            return -1;
+        }
+
+        int c1_row = c1_id / grid_width;
+        int c1_col = c1_id % grid_width;
+        int c2_row = c2_id / grid_width;
+        int c2_col = c2_id % grid_width;
+
+        float dx = (c1_col - c2_col) * grid_size_x;
+        float dy = (c1_row - c2_row) * grid_size_y;
+        return std::sqrt(dx*dx + dy*dy);
+    }
+
+    int GetCellIdFromPoint(LocalCoordinate pt) {
+        int row = (pt.y - min_y) / grid_size_y;
+        int col = (pt.x - min_x) / grid_size_x;
+        return row * grid_width + col;
+    }
+
+    std::vector<GridCellPtr> GetCellsNearby(LocalCoordinate local_pt, double radius) {
+        std::vector<GridCellPtr> vec_cells;
+        int row = (local_pt.y-min_y)/grid_size_y;
+        int col = (local_pt.x-min_x)/grid_size_x;
+        int search_extent = radius/grid_size_x + 1;
+        for(int i=row-search_extent; i<=row+search_extent; i++) {
+            for(int j=col-search_extent; j<=col+search_extent; j++) {
+                GridCellPtr cell = GetCell(i, j);
+                while(nullptr != cell) {
+                    vec_cells.push_back(cell);
+                    cell = cell->next_level_cell;
+                }
+            }
+        }
+        return vec_cells;
+    }
+    GridCellPtr GetCell(int cell_row, int cell_col) {
+        if(cell_row<0 || cell_row>=grid_height) {
+            return nullptr;
+        }
+
+        if(cell_col<0 || cell_col>=grid_width) {
+            return nullptr;
+        }
+
+        int cell_idx = cell_row*grid_width + cell_col;
+        return vec_grid_cells[cell_idx];
+    }
+
+public:
+    int grid_width;
+    int grid_height;
+    float grid_size_x;
+    float grid_size_y;
+    double min_x, min_y;
+    double max_x, max_y;
+    std::vector<GridCellPtr> vec_grid_cells;
+};
+typedef std::shared_ptr<RoadGrid> RoadGridPtr;
+#endif //LIDARGROUNDDETECT_GRID_H

cloud_common/common/PCLType.h

+//
+// Created by xueyimeng on 20-12-8.
+//
+
+#ifndef LIDARGROUNDDETECT_PCLTYPE_H
+#define LIDARGROUNDDETECT_PCLTYPE_H
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+typedef struct PointXYZIL {
+    PCL_ADD_POINT4D
+    float intensity;
+    float label;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16 PointXYZIL;
+
+// pcl point cloud
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+        PointXYZIL, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
+(float, label, label))
+typedef pcl::PointCloud<PointXYZIL> PointCloudXYZIL;
+
+typedef struct CarPoint {
+    PCL_ADD_POINT4D
+    float intensity;
+    uint8_t label;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW //make sure our new allocators are aligned
+} EIGEN_ALIGN16 CarPoint;
+//pcl point cloud
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+        CarPoint, (float, x,x)(float,y,y)(float,z,z)(float, intensity, intensity)(uint8_t, label, label)
+        )
+typedef pcl::PointCloud<CarPoint> PointCloudCar;
+
+//defined by weiji
+//info stores instance id of clustered object result
+typedef struct PointXYZIX_old {
+    PCL_ADD_POINT4D
+    float intensity;
+    float label;
+    float info;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16 PointXYZIX_old;
+
+// pcl point cloud
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+        PointXYZIX_old, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
+(float, label, label)(float, info, info))
+typedef pcl::PointCloud<PointXYZIX_old> PointCloudXYZIX_old;
+
+//defined by wangdawei
+struct PointXYZIX {
+    PCL_ADD_POINT4D
+    PCL_ADD_RGB
+    float intensity;
+    float label;
+    float info;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIX, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb) (float, intensity, intensity)(float, label, label)(float, info, info))
+
+typedef pcl::PointCloud<PointXYZIX> PointCloudXYZIX;
+
+
+
+//defined for road interpolation
+struct TrainedPoint {
+    PCL_ADD_POINT4D
+    PCL_ADD_RGB
+    float intensity;
+    float label;
+    float info;
+    float trainLabel;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+POINT_CLOUD_REGISTER_POINT_STRUCT(TrainedPoint, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb) (float, intensity, intensity)(float, label, label)(float, info, info)(float, trainLabel, trainLabel))
+typedef pcl::PointCloud<TrainedPoint> TrainedCloud;
+#endif //LIDARGROUNDDETECT_PCLTYPE_H

cloud_common/common/TypeDef.h

+//
+// Created by xueyimeng on 20-10-1.
+//
+
+#ifndef LIDAR_MAPPING_PROJECT_SERVER_TYPEDEF_H
+#define LIDAR_MAPPING_PROJECT_SERVER_TYPEDEF_H
+
+#include <memory>
+#include <vector>
+
+#include <float.h>
+#include <set>
+#include <cmath>
+#include <iostream>
+#include "Eigen/Dense"
+
+//
+enum PointType {
+    //value specified by hanlixuan and wuweiji
+    UNKNOWN = 0,
+    NORMAL_POINT = 10000,
+    POTENTIAL_CURB_POINT = 11000, //object points 50cm above ground
+    ROAD_POINT = 20000,
+    GROUND_POINT = 40000,
+    NOISE_POINT = 60000,
+
+    //typedef for wangying
+    /* 1: 'vehicle',
+       2: 'person',
+       3: 'cyclist',
+       5: 'traffic-light',
+       6: 'guardrail',
+       7: 'pole',
+       8: 'gelizhuang',
+       9: 'sign',
+     * */
+    VEHICLE_POINT = 1,
+    PERSON_POINT = 2,
+    BICYCLE_POINT = 3,
+    CONSTRUCTION_VEHICLE_POINT = 4,
+    TRAFFICLIGHT_POINT = 5,
+    GUARDRAIL_POINT = 6,
+    POLE_POINT = 7,
+    GELIZHUANG_POINT = 8,
+    SIGN_POINT = 9,
+
+    //internal types
+
+    CURB_POINT = 21000,
+    LEFT_CURB_POINT = CURB_POINT,
+    EXTRA_CAR_POINT = 100000,
+
+    NOISE_OR_OBJECT = 8, //used for not sure case
+    NORMAL_POINT_1 = 10001,
+    NORMAL_POINT_2 = 10002,
+    NORMAL_POINT_3 = 10003,
+    NORMAL_POINT_4 = 10004,
+    NORMAL_POINT_5 = 10005,
+    NORMAL_POINT_6 = 10006,
+    NORMAL_POINT_7 = 10007,
+    NORMAL_POINT_8 = 10008,
+    NORMAL_POINT_9 = 10009,
+    NORMAL_POINT_10= 10010,
+
+    GROUND_POINT_1 = 40001,
+    GROUND_POINT_2 = 40002,
+    GROUND_POINT_3 = 40003,
+    GROUND_POINT_4 = 40004,
+    GROUND_POINT_5 = 40005,
+    GROUND_POINT_6 = 40006,
+    GROUND_POINT_7 = 40007,
+    GROUND_POINT_8 = 40008,
+    GROUND_POINT_9 = 40009,
+    GROUND_POINT_10= 40010,
+    //
+    GUARD_RAIL_POINT = 50000,
+
+    NOISE_POINT_1 = 60001,
+    NOISE_POINT_2 = 60002,
+    NOISE_POINT_3 = 60003,
+
+    TRACK_POINT = 100,
+    LANE_EDGE_POINT = 101,
+    TRAFFIC_LIGHT_POINT = 103,
+    CROSS_WALK_POINT = 104,
+    STOPLINE_POINT = 105,
+};
+
+enum CellType {
+    CellType_UNKNOWN = 0,
+    GROUND_CELL = 1,
+    ROAD_CELL = 2,
+    MIX_CELL = 3,
+    ROAD_BACKGROUND = 4,
+    GROUND_BACKGROUND = 5,
+};
+
+
+struct RefPoint {
+    int orig_pt_idx;
+    double x;
+    double y;
+    double z;
+
+    RefPoint() {}
+
+    RefPoint(int orig_pt_idx, double x, double y, double z) {
+        this->orig_pt_idx = orig_pt_idx;
+        this->x =x;
+        this->y = y;
+        this->z = z;
+    }
+
+    RefPoint(const RefPoint& ref_pt) {
+        x = ref_pt.x;
+        y = ref_pt.y;
+        z = ref_pt.z;
+        orig_pt_idx = ref_pt.orig_pt_idx;
+    }
+
+    RefPoint&operator=(const RefPoint& refPoint) {
+        this->x = refPoint.x;
+        this->y = refPoint.y;
+        this->z = refPoint.z;
+        this->orig_pt_idx = refPoint.orig_pt_idx;
+        return *this;
+    }
+};
+
+class LocalCoordinate {
+public:
+    LocalCoordinate() {
+        x = y = z = 0;
+    }
+
+    LocalCoordinate(double x, double y, double z=0.0f) : x(x), y(y), z(z) {}
+    double x;
+    double y;
+    double z;
+
+    bool operator==(const LocalCoordinate& p1) const {
+        return std::fabs(p1.x - x) < 1e-3 &&
+               fabs(p1.y - y) < 1e-3 &&
+               fabs(p1.z - z) < 1e-3;
+    }
+
+    LocalCoordinate&operator=(const RefPoint& ref_pt) {
+        this->x = ref_pt.x;
+        this->y = ref_pt.y;
+        this->z = ref_pt.z;
+        return *this;
+    }
+
+    LocalCoordinate&operator=(const LocalCoordinate& ref_pt) {
+        this->x = ref_pt.x;
+        this->y = ref_pt.y;
+        this->z = ref_pt.z;
+        return *this;
+    }
+};
+typedef std::shared_ptr<LocalCoordinate> LocalCoordinatePtr;
+
+struct Plane {
+    double a;
+    double b;
+    double c;
+    double d;
+
+    Plane() {}
+
+    Plane(double a, double b, double c, double d) {
+        this->a = a;
+        this->b = b;
+        this->c = c;
+        this->d = d;
+    }
+
+    virtual ~Plane() {
+
+    }
+};
+typedef std::shared_ptr<Plane> PlanePtr;
+
+
+struct RefPointInfo {
+    int pt_idx;
+    //PointType pt_type;
+
+    RefPointInfo() {}
+
+    RefPointInfo(int pt_idx, PointType pt_type=PointType::UNKNOWN) {
+        this->pt_idx = pt_idx;
+        //this->pt_type = pt_type;
+    }
+};
+
+struct ProfilePointInfo {
+    int profile_id; //
+    int orig_pt_idx;
+    int profile_seg_idx;
+    double local_x;
+    double local_y;
+    double local_z;
+    PointType pt_type;
+};
+typedef std::shared_ptr<ProfilePointInfo> ProfilePointInfoPtr;
+
+struct SensorInfo {
+    SensorInfo() {
+        sensor_height = 1.9;
+    }
+
+    virtual ~SensorInfo() {}
+
+    double sensor_height;
+};
+typedef std::shared_ptr<SensorInfo> SensorInfoPtr;
+
+struct ClusterInfo {
+    double min_z;
+    double max_z;
+    int cluster_idx;
+    int num_pts;
+
+    ClusterInfo(double min_z, double max_z, int cluster_idx, int num_pts) {
+        this->min_z = min_z;
+        this->max_z = max_z;
+        this->cluster_idx = cluster_idx;
+        this->num_pts = num_pts;
+    }
+};
+
+struct DistanceInfo {
+    int pt_idx;
+    double distance;
+
+    DistanceInfo() {}
+
+    DistanceInfo(int pt_idx, double distance) {
+        this->pt_idx = pt_idx;
+        this->distance = distance;
+    }
+};
+
+struct LevelInfo {
+    LevelInfo() {
+        min_distance = DBL_MAX;
+        max_distance = -DBL_MAX;
+    }
+
+    int level_idx;
+    double min_distance;
+    double max_distance;
+    double step;
+    float density;
+    std::vector<int> pt_idxs;
+};
+
+class DebugPointInfo {
+public:
+    DebugPointInfo() {
+
+    }
+
+    DebugPointInfo(int pt_idx, LocalCoordinate coord) {
+        this->pt_idx = pt_idx;
+        pt = coord;
+    }
+
+    bool operator==(const DebugPointInfo& dbg_pt) const {
+        return dbg_pt.pt_idx == pt_idx && dbg_pt.pt == this->pt;
+    }
+
+    bool operator < (const DebugPointInfo& dbg_pt) const {
+        return dbg_pt.pt_idx < this->pt_idx;
+    }
+
+    void SetPtIdx(int idx) {
+        this->pt_idx = idx;
+    }
+
+    void Show() {
+        std::cout << "pt_idx: " << pt_idx << ", distance: " << distance_to_plane << std::endl;
+    }
+public:
+    int pt_idx;
+    LocalCoordinate pt;
+    double distance_to_plane;
+};
+
+struct StationInfo {
+    StationInfo() {
+        is_valid = false;
+    }
+
+    ~StationInfo(){}
+
+    int srid;
+    double ref_lat;
+    double ref_lon;
+    double ref_height;
+
+    double utm_offset_x;
+    double utm_offset_y;
+    double utm_offset_z;
+    bool is_valid;
+};
+
+struct AggSeg {
+    AggSeg() {
+        min_x = min_y = min_z = DBL_MAX;
+        max_x = max_y = max_z = -DBL_MAX;
+    }
+
+    std::vector<RefPoint> curbSegment;
+    float min_x, max_x;
+    float min_z, max_z;
+    float min_y, max_y;
+    float length;
+};
+#endif //LIDAR_MAPPING_PROJECT_SERVER_TYPEDEF_H

cloud_common/common/context.cpp

+//
+// Created by xueyimeng on 21-1-13.
+//
+
+#include "context.h"
+
+DataContext* DataContext::instance() {
+    static DataContext context;
+    return &context;
+}
+
+std::set<PointType> DataContext::GetGroundFilters() {
+    if(ground_filters.empty()) {
+        ground_filters.insert(PointType::ROAD_POINT);
+        ground_filters.insert(PointType::GROUND_POINT);
+        ground_filters.insert(PointType::GROUND_POINT_1);
+        ground_filters.insert(PointType::GROUND_POINT_2);
+        ground_filters.insert(PointType::GROUND_POINT_3);
+        ground_filters.insert(PointType::GROUND_POINT_4);
+        ground_filters.insert(PointType::GROUND_POINT_5);
+        ground_filters.insert(PointType::GROUND_POINT_6);
+        ground_filters.insert(PointType::GROUND_POINT_7);
+        ground_filters.insert(PointType::GROUND_POINT_8);
+        ground_filters.insert(PointType::GROUND_POINT_9);
+        ground_filters.insert(PointType::GROUND_POINT_10);
+    }
+    return ground_filters;
+}
+
+std::set<PointType> DataContext::GetObjectFilters() {
+    if(object_filters.empty()) {
+        object_filters.insert(PointType::NORMAL_POINT);
+        object_filters.insert(PointType::NORMAL_POINT_1);
+        object_filters.insert(PointType::NORMAL_POINT_2);
+        object_filters.insert(PointType::NORMAL_POINT_3);
+        object_filters.insert(PointType::NORMAL_POINT_4);
+        object_filters.insert(PointType::NORMAL_POINT_5);
+        object_filters.insert(PointType::NORMAL_POINT_6);
+        object_filters.insert(PointType::NORMAL_POINT_7);
+        object_filters.insert(PointType::NORMAL_POINT_8);
+        object_filters.insert(PointType::NORMAL_POINT_9);
+        object_filters.insert(PointType::NORMAL_POINT_10);
+        object_filters.insert(PointType::POTENTIAL_CURB_POINT);
+    }
+    return object_filters;
+}
+
+std::set<PointType> DataContext::GetNoiseFilters() {
+    if(noise_filters.empty()) {
+        noise_filters.insert(PointType::NOISE_OR_OBJECT);
+        noise_filters.insert(PointType::NOISE_POINT);
+        noise_filters.insert(PointType::NOISE_POINT_1);
+        noise_filters.insert(PointType::NOISE_POINT_2);
+        noise_filters.insert(PointType::NOISE_POINT_3);
+    }
+    return noise_filters;
+}
+
+std::set<PointType> DataContext::GetCarFilters() {
+    if(car_filters.empty()) {
+        car_filters.insert(PointType::VEHICLE_POINT);
+        car_filters.insert(PointType::PERSON_POINT);
+        car_filters.insert(PointType::BICYCLE_POINT);
+    }
+    return car_filters;
+}
+
+std::set<PointType> DataContext::GetDetectedFilter() {
+    if(detect_point_filters.empty()) {
+       detect_point_filters.insert(PointType::VEHICLE_POINT);
+       detect_point_filters.insert(PointType::PERSON_POINT);
+       detect_point_filters.insert(PointType::BICYCLE_POINT);
+       detect_point_filters.insert(PointType::CONSTRUCTION_VEHICLE_POINT);
+       detect_point_filters.insert(PointType::TRAFFICLIGHT_POINT);
+       detect_point_filters.insert(PointType::GUARDRAIL_POINT);
+       detect_point_filters.insert(PointType::POLE_POINT);
+       detect_point_filters.insert(PointType::GELIZHUANG_POINT);
+       detect_point_filters.insert(PointType::SIGN_POINT);
+    }
+    return detect_point_filters;
+}
+
+std::map<PointType, std::string> DataContext::GetLabelTypeInfo() {
+    if(map_type_name.empty()) {
+        map_type_name[PointType::NORMAL_POINT] = "normal point";
+        map_type_name[PointType::GROUND_POINT] = "ground point";
+        map_type_name[PointType::ROAD_POINT] = "road point";
+        map_type_name[PointType::NOISE_POINT] = "noise point";
+        map_type_name[PointType::POTENTIAL_CURB_POINT] = "points 200cm or lower above ground or road";
+
+        //map_type_name[PointType::VEHICLE_POINT] = "car points";
+        //map_type_name[PointType::PERSON_POINT] = "truck point";
+        //map_type_name[PointType::BYCYCLE_POINT] = "bus points";
+        //map_type_name[PointType::CONSTRUCTION_VEHICLE_POINT] = "construction vehicle point";
+        //map_type_name[PointType::TRAFFICLIGHT_POINT] = "motorcycle point";
+        //map_type_name[PointType::BICYCLE_POINT] = "bicycle point";
+        //map_type_name[PointType::POLE_POINT] = "pedestrian points";
+    }
+    return map_type_name;
+}
\ No newline at end of file

cloud_common/common/context.h

+//
+// Created by xueyimeng on 21-1-13.
+//
+
+#ifndef LIDARGROUNDDETECT_CONTEXT_H
+#define LIDARGROUNDDETECT_CONTEXT_H
+
+#include "TypeDef.h"
+#include <map>
+
+class DataContext {
+public:
+    static DataContext* instance();
+
+    virtual ~DataContext() {}
+
+public:
+    std::set<PointType> GetGroundFilters();
+    std::set<PointType> GetObjectFilters();
+    std::set<PointType> GetNoiseFilters();
+    std::set<PointType> GetCarFilters();
+    std::set<PointType> GetDetectedFilter();
+
+    std::map<PointType, std::string> GetLabelTypeInfo();
+private:
+    DataContext() {}
+
+private:
+    std::set<PointType> ground_filters;
+    std::set<PointType> object_filters;
+    std::set<PointType> noise_filters;
+    std::set<PointType> car_filters;
+    std::set<PointType> detect_point_filters;
+    std::map<PointType, std::string> map_type_name;
+};
+
+
+#endif //LIDARGROUNDDETECT_CONTEXT_H

cloud_common/common/low_level_io.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *  Copyright (c) 2018 Fizyr BV. - https://fizyr.com
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * This file defines compatibility wrappers for low level I/O functions.
+ * Implemented as inlinable functions to prevent any performance overhead.
+ */
+
+#pragma once
+
+#ifdef _WIN32
+# ifndef WIN32_LEAN_AND_MEAN
+#  define WIN32_LEAN_AND_MEAN
+# endif
+# ifndef NOMINMAX
+#  define NOMINMAX
+# endif
+# include <io.h>
+# include <windows.h>
+# include <BaseTsd.h>
+using ssize_t = SSIZE_T;
+#else
+# include <unistd.h>
+# include <sys/mman.h>
+# include <sys/types.h>
+# include <sys/stat.h>
+# include <sys/fcntl.h>
+# include <cerrno>
+#endif
+#include <cstddef>
+
+namespace pcl
+{
+  namespace io
+  {
+#ifdef _WIN32
+    inline int raw_open(const char * pathname, int flags, int mode)
+    {
+      return ::_open(pathname, flags, mode);
+    }
+
+    inline int raw_open(const char * pathname, int flags)
+    {
+      return ::_open(pathname, flags);
+    }
+
+    inline int raw_close(int fd)
+    {
+      return ::_close(fd);
+    }
+
+    inline int raw_lseek(int fd, long offset, int whence)
+    {
+      return ::_lseek(fd, offset, whence);
+    }
+
+    inline int raw_read(int fd, void * buffer, std::size_t count)
+    {
+      return ::_read(fd, buffer, count);
+    }
+
+    inline int raw_write(int fd, const void * buffer, std::size_t count)
+    {
+      return ::_write(fd, buffer, count);
+    }
+
+    inline int raw_ftruncate(int fd, long length)
+    {
+      return ::_chsize(fd, length);
+    }
+
+    inline int raw_fallocate(int fd, long length)
+    {
+      // Doesn't actually allocate, but best we can do?
+      return raw_ftruncate(fd, length);
+    }
+#else
+    inline int raw_open(const char * pathname, int flags, int mode)
+    {
+      return ::open(pathname, flags, mode);
+    }
+
+    inline int raw_open(const char * pathname, int flags)
+    {
+      return ::open(pathname, flags);
+    }
+
+    inline int raw_close(int fd)
+    {
+      return ::close(fd);
+    }
+
+    inline off_t raw_lseek(int fd, off_t offset, int whence)
+    {
+      return ::lseek(fd, offset, whence);
+    }
+
+    inline ssize_t raw_read(int fd, void * buffer, std::size_t count)
+    {
+      return ::read(fd, buffer, count);
+    }
+
+    inline ssize_t raw_write(int fd, const void * buffer, std::size_t count)
+    {
+      return ::write(fd, buffer, count);
+    }
+
+    inline int raw_ftruncate(int fd, off_t length)
+    {
+      return ::ftruncate(fd, length);
+    }
+
+# ifdef __APPLE__
+    inline int raw_fallocate(int fd, off_t length)
+    {
+      // OS X doesn't have posix_fallocate, but it has a fcntl that does the job.
+      // It may make the file too big though, so we truncate before returning.
+
+      // Try to allocate contiguous space first.
+      ::fstore_t store = {F_ALLOCATEALL | F_ALLOCATECONTIG, F_PEOFPOSMODE, 0, length, 0};
+      if (::fcntl(fd, F_PREALLOCATE, &store) != -1)
+        return raw_ftruncate(fd, length);
+
+      // Try fragmented if it failed.
+      store.fst_flags = F_ALLOCATEALL;
+      if (::fcntl(fd, F_PREALLOCATE, &store) != -1)
+        return raw_ftruncate(fd, length);
+
+      // Fragmented also failed.
+      return -1;
+    }
+
+# else // __APPLE__
+    inline int raw_fallocate(int fd, off_t length)
+    {
+#  ifdef ANDROID
+      // Android's libc doesn't have posix_fallocate.
+      if (::fallocate(fd, 0, 0, length) == 0)
+        return 0;
+#  else
+      // Conforming POSIX systems have posix_fallocate.
+      if (::posix_fallocate(fd, 0, length) == 0)
+        return 0;
+#  endif
+
+      // EINVAL should indicate an unsupported filesystem.
+      // All other errors are passed up.
+      if (errno != EINVAL)
+        return -1;
+
+      // Try to deal with unsupported filesystems by simply seeking + writing.
+      // This may not really allocate space, but the file size will be set.
+      // Writes to the mmapped file may still trigger SIGBUS errors later.
+
+      // Remember the old position and seek to the desired length.
+      off_t old_pos = raw_lseek(fd, 0, SEEK_CUR);
+      if (old_pos == -1)
+        return -1;
+      if (raw_lseek(fd, length - 1, SEEK_SET) == -1)
+        return -1;
+
+      // Write a single byte to resize the file.
+      char buffer = 0;
+      ssize_t written = raw_write(fd, &buffer, 1);
+
+      // Seek back to the old position.
+      if (raw_lseek(fd, old_pos, SEEK_SET) == -1)
+        return -1;
+
+      // Fail if we didn't write exactly one byte,
+      if (written != 1)
+        return -1;
+
+      return 0;
+    }
+# endif // __APPLE
+#endif // _WIN32
+
+  }
+}

cloud_common/common/pcd_io_ext.hpp

+//
+// Created by xueyimeng on 21-7-28.
+//
+
+#ifndef PCL_NO_PRECOMPILE
+#define PCL_NO_PRECOMPILE
+#endif
+
+#ifndef PROJECT_JF_PCD_IO_EXT_HPP
+#define PROJECT_JF_PCD_IO_EXT_HPP
+
+#include <fstream>
+#include <fcntl.h>
+#include <string>
+#include <stdlib.h>
+#include <pcl/io/boost.h>
+#include <pcl/console/print.h>
+#include <pcl/io/pcd_io.h>
+
+#ifdef _WIN32
+# include <io.h>
+# ifndef WIN32_LEAN_AND_MEAN
+#  define WIN32_LEAN_AND_MEAN
+# endif // WIN32_LEAN_AND_MEAN
+# ifndef NOMINMAX
+#  define NOMINMAX
+# endif // NOMINMAX
+# include <windows.h>
+# define pcl_open                    _open
+# define pcl_close(fd)               _close(fd)
+# define pcl_lseek(fd,offset,origin) _lseek(fd,offset,origin)
+#else
+# include <sys/mman.h>
+# define pcl_open                    open
+# define pcl_close(fd)               close(fd)
+# define pcl_lseek(fd,offset,origin) lseek(fd,offset,origin)
+#endif
+
+#include <pcl/io/lzf.h>
+
+class PCDWriterExt : public pcl::PCDWriter {
+public:
+    PCDWriterExt() : map_synchronization_(false){}
+    ~PCDWriterExt() {}
+
+public:
+
+    /** \brief Save point cloud data to a PCD file containing n-D points
+        * \param[in] file_name the output file name
+        * \param[in] cloud the pcl::PointCloud data
+        * \param[in] binary set to true if the file is to be written in a binary
+        * PCD format, false (default) for ASCII
+        *
+        * Caution: PointCloud structures containing an RGB field have
+        * traditionally used packed float values to store RGB data. Storing a
+        * float as ASCII can introduce variations to the smallest bits, and
+        * thus significantly alter the data. This is a known issue, and the fix
+        * involves switching RGB data to be stored as a packed integer in
+        * future versions of PCL.
+        */
+    template<typename PointT> inline int
+    write (const std::string &file_name,
+           const pcl::PointCloud<PointT> &cloud,
+           const bool binary = false)
+    {
+        if (binary)
+            return (writeBinary<PointT> (file_name, cloud));
+        return (writeASCII<PointT> (file_name, cloud));
+    }
+
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> int
+writeBinary (const std::string &file_name,
+             const pcl::PointCloud<PointT> &cloud)
+{
+  if (cloud.empty ())
+  {
+    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
+    return (-1);
+  }
+  int data_idx = 0;
+  std::ostringstream oss;
+  oss << generateHeader<PointT> (cloud) << "DATA binary\n";
+  oss.flush ();
+  data_idx = static_cast<int> (oss.tellp ());
+
+#if _WIN32
+  HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
+  if (h_native_file == INVALID_HANDLE_VALUE)
+  {
+    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
+    return (-1);
+  }
+#else
+  int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+  if (fd < 0)
+  {
+    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
+    return (-1);
+  }
+#endif
+  // Mandatory lock file
+  boost::interprocess::file_lock file_lock;
+  setLockingPermissions (file_name, file_lock);
+
+  std::vector<pcl::PCLPointField> fields;
+  std::vector<int> fields_sizes;
+  size_t fsize = 0;
+  size_t data_size = 0;
+  size_t nri = 0;
+  pcl::getFields (cloud, fields);
+  // Compute the total size of the fields
+  for (size_t i = 0; i < fields.size (); ++i)
+  {
+    if (fields[i].name == "_")
+      continue;
+
+    int fs = fields[i].count * pcl::getFieldSize (fields[i].datatype);
+    fsize += fs;
+    fields_sizes.push_back (fs);
+    fields[nri++] = fields[i];
+  }
+  fields.resize (nri);
+
+  data_size = cloud.points.size () * fsize;
+
+  // Prepare the map
+#if _WIN32
+  HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
+  char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
+  CloseHandle (fm);
+
+#else
+  // Stretch the file size to the size of the data
+  off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
+
+  if (result < 0)
+  {
+    pcl_close (fd);
+    resetLockingPermissions (file_name, file_lock);
+    PCL_ERROR ("[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
+
+    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
+    return (-1);
+  }
+  // Write a bogus entry so that the new file size comes in effect
+  result = static_cast<int> (::write (fd, "", 1));
+  if (result != 1)
+  {
+    pcl_close (fd);
+    resetLockingPermissions (file_name, file_lock);
+    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!");
+    return (-1);
+  }
+
+  char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
+  if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
+  {
+    pcl_close (fd);
+    resetLockingPermissions (file_name, file_lock);
+    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
+    return (-1);
+  }
+#endif
+
+  // Copy the header
+  memcpy (&map[0], oss.str ().c_str (), data_idx);
+
+  // Copy the data
+  char *out = &map[0] + data_idx;
+  for (size_t i = 0; i < cloud.points.size (); ++i)
+  {
+    int nrj = 0;
+    for (size_t j = 0; j < fields.size (); ++j)
+    {
+      memcpy (out, reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[nrj]);
+      out += fields_sizes[nrj++];
+    }
+  }
+
+  // If the user set the synchronization flag on, call msync
+#if !_WIN32
+  if (map_synchronization_)
+    msync (map, data_idx + data_size, MS_SYNC);
+#endif
+
+  // Unmap the pages of memory
+#if _WIN32
+    UnmapViewOfFile (map);
+#else
+  if (munmap (map, (data_idx + data_size)) == -1)
+  {
+    pcl_close (fd);
+    resetLockingPermissions (file_name, file_lock);
+    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
+    return (-1);
+  }
+#endif
+  // Close file
+#if _WIN32
+  CloseHandle (h_native_file);
+#else
+  pcl_close (fd);
+#endif
+  resetLockingPermissions (file_name, file_lock);
+  return (0);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    template <typename PointT> std::string
+    generateHeader (const pcl::PointCloud<PointT> &cloud,
+                    const int nr_points = std::numeric_limits<int>::max ())
+    {
+        std::ostringstream oss;
+        oss.imbue (std::locale::classic ());
+
+        oss << "# .PCD v0.7 - Point Cloud Data file format"
+               "\nVERSION 0.7" << ", FRAME_ID:" << cloud.header.frame_id
+            << "\nFIELDS";
+
+        const auto fields = pcl::getFields<PointT> ();
+
+        std::stringstream field_names, field_types, field_sizes, field_counts;
+        for (const auto &field : fields)
+        {
+            if (field.name == "_")
+                continue;
+            // Add the regular dimension
+            field_names << " " << field.name;
+            field_sizes << " " << pcl::getFieldSize (field.datatype);
+            if ("rgb" == field.name)
+                field_types << " " << "U";
+            else
+                field_types << " " << pcl::getFieldType (field.datatype);
+            int count = std::abs (static_cast<int> (field.count));
+            if (count == 0) count = 1;  // check for 0 counts (coming from older converter code)
+            field_counts << " " << count;
+        }
+        oss << field_names.str ();
+        oss << "\nSIZE" << field_sizes.str ()
+            << "\nTYPE" << field_types.str ()
+            << "\nCOUNT" << field_counts.str ();
+        // If the user passes in a number of points value, use that instead
+        if (nr_points != std::numeric_limits<int>::max ())
+            oss << "\nWIDTH " << nr_points << "\nHEIGHT " << 1 << "\n";
+        else
+            oss << "\nWIDTH " << cloud.width << "\nHEIGHT " << cloud.height << "\n";
+
+        oss << "VIEWPOINT " << cloud.sensor_origin_[0] << " " << cloud.sensor_origin_[1] << " " << cloud.sensor_origin_[2] << " " <<
+            cloud.sensor_orientation_.w () << " " <<
+            cloud.sensor_orientation_.x () << " " <<
+            cloud.sensor_orientation_.y () << " " <<
+            cloud.sensor_orientation_.z () << "\n";
+
+        // If the user passes in a number of points value, use that instead
+        if (nr_points != std::numeric_limits<int>::max ())
+            oss << "POINTS " << nr_points << "\n";
+        else
+            oss << "POINTS " << cloud.points.size () << "\n";
+
+        //oss << "FRAME_ID " << cloud.header.frame_id << "\n";
+        return (oss.str ());
+    }
+
+private:
+    bool map_synchronization_;
+};
+
+template<typename PointT> inline int
+savePCDFileExt (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, bool binary_mode = false)
+{
+    PCDWriterExt* w = new PCDWriterExt();
+    return (w->write<PointT> (file_name, cloud, binary_mode));
+}
+#endif //PROJECT_JF_PCD_IO_EXT_HPP

cloud_common/utility/CMakeLists.txt

+cmake_minimum_required(VERSION 3.5)
+
+file(GLOB_RECURSE CPP_FILES *.cpp *.h *.hpp)
+
+add_library(utility ${CPP_FILES})
+target_link_libraries(utility ${PCL_LIBRARIES})
+
+install(TARGETS utility
+        DESTINATION  ${CMAKE_SOURCE_DIR}/lib
+        )
\ No newline at end of file

cloud_common/utility/Circle.cpp

+//
+// Created by xueyimeng on 21-5-8.
+//
+
+#include "Circle.h"
+#include "Utility.h"
+
+Circle::Circle() {
+    circle_center.x = circle_center.y = circle_center.z = 0;
+    radius = 0;
+    is_valid = false;
+}
+
+Circle::Circle(LocalCoordinate center_pos, double radius) {
+    this->circle_center = center_pos;
+    this->radius = radius;
+    is_valid = true;
+}
+
+Circle::Circle(LocalCoordinate start_pt, LocalCoordinate end_Pt, double radius) {
+    //reference https://blog.csdn.net/mikasoi/article/details/79590019
+    double x1 = start_pt.x, x2 = end_Pt.x;
+    double y1 = start_pt.y, y2 = end_Pt.y;
+    double R = radius;
+
+    double distance = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
+
+    if(radius > 0 && distance <= radius*2) {
+        is_valid = true;
+    }
+
+    if(is_valid) {
+        double c1 = (x2*x2 - x1*x1 + y2*y2 - y1*y1) / (2 *(x2 - x1));
+        double c2 = (y2 - y1) / (x2 - x1);
+        double A = (c2*c2 + 1);
+        double B = (2 * x1*c2 - 2 * c1*c2 - 2 * y1);
+        double C = x1*x1 - 2 * x1*c1 + c1*c1 + y1*y1 - R*R;
+        double temp_value = B*B-4*A*C;
+        is_valid = temp_value>=0 ? true : false;
+        if(is_valid) {
+            circle_center.y = (-B + sqrt(temp_value)) / (2*A);
+            circle_center.x = c1 - c2 * circle_center.y;
+            this->radius = radius;
+        }
+    }
+}
+
+Circle::~Circle() {
+
+}
+
+bool Circle::IsPtContained(LocalCoordinate test_pt) {
+    if(!is_valid)
+        return false;
+
+    double dx = test_pt.x-circle_center.x;
+    double dy = test_pt.y-circle_center.y;
+    double distance = sqrt(dx*dx+dy*dy);
+    if(distance > radius)
+        return false;
+    return true;
+}
\ No newline at end of file

cloud_common/utility/Circle.h

+//
+// Created by xueyimeng on 21-5-8.
+//
+
+#ifndef LIDARGROUNDDETECT_CIRCLE_H
+#define LIDARGROUNDDETECT_CIRCLE_H
+
+#include "common/TypeDef.h"
+class Circle {
+public:
+    Circle();
+    Circle(LocalCoordinate center_pos, double radius);
+    Circle(LocalCoordinate start_pt, LocalCoordinate end_Pt, double radius);
+    virtual ~Circle();
+
+public:
+    bool IsValid() {return is_valid;}
+
+    bool IsPtContained(LocalCoordinate test_pt);
+private:
+    bool is_valid;
+    LocalCoordinate circle_center;
+    double radius;
+};
+
+
+#endif //LIDARGROUNDDETECT_CIRCLE_H

cloud_common/utility/Config.cpp

+//
+// Created by xueyimeng on 20-10-16.
+//
+
+#include <iostream>
+#include <fstream>
+
+#include "Config.h"
+#include "Utility.h"
+
+Config::Config() {
+    save_debug_type = false;
+    is_road_labeld = false;
+    grid_size = 0;
+}
+
+Config::~Config() {
+
+}
+
+Config* Config::instance() {
+    static Config config;
+    return &config;
+}
+
+bool Config::LoadFromJson(const std::string &json_pathname) {
+    if(json_pathname.empty()){
+        std::cout << "An empty json file path: " << json_pathname << std::endl;
+        return false;
+    }
+
+    Json::Reader reader;
+    Json::Value root;
+
+    std::ifstream in(json_pathname, std::ios::binary);
+    if (!in.is_open())
+    {
+        std::cout << "Error opening file: " << json_pathname << std::endl;
+        return false;
+    }
+
+    if(reader.parse(in, root))
+    {
+        data_dir = root["IO_Config"]["data_dir"].asString();
+        pcd_shortname = root["IO_Config"]["pcd_shortname"].asString();
+        out_dir = root["IO_Config"]["out_dir"].asString();
+        std::string task_list = root["IO_Config"]["task_list"].asString();
+        if(!task_list.empty()) {
+            std::vector<std::string> vec_tasks;
+            Utility::SplitString(task_list, vec_tasks, ",");
+            this->task_list = vec_tasks;
+        }
+
+        Json::Value io = root["IO_Config"];
+        if(io.isMember("save_debug_type")) {
+            std::string str = io["save_debug_type"].asString();
+            if(str == "true") {
+                save_debug_type = true;
+            } else {
+                save_debug_type = false;
+            }
+        }
+
+        if(io.isMember("pcd_list")) {
+            Json::Value v = io["pcd_list"];
+            for(int i=0; i<v.size(); i++) {
+                std::string pcd_pathname = v[i].asString();
+                pcd_list.push_back(pcd_pathname);
+            }
+        }
+
+        if(io.isMember("station_filename")) {
+            station_filename = io["station_filename"].asString();
+        }
+
+        if(io.isMember("result_gpkg_pathname")) {
+            result_gpkg_pathname = io["result_gpkg_pathname"].asString();
+        }
+
+        if(io.isMember("track_pathname")) {
+            track_pathname = io["track_pathname"].asString();
+        }
+
+        if(io.isMember("spatial_filter_path")) {
+            spatial_filter_pathname = io["spatial_filter_path"].asString();
+        }
+
+        if(root.isMember("other_config")) {
+            Json::Value other_config = root["other_config"];
+            if(other_config.isMember("is_road_labeled")) {
+                is_road_labeld = other_config["is_road_labeled"].asBool();
+            }
+
+            if(other_config.isMember("grid_size")) {
+                grid_size = other_config["grid_size"].asFloat();
+            }
+        }
+
+
+        if(root.isMember("debug_points")) {
+            std::string str_pt_list = root["debug_points"]["pt_list"].asString();
+            std::vector<std::string> pt_pairs;
+            Utility::SplitString(str_pt_list, pt_pairs, ";");
+            for(int i=0; i<pt_pairs.size(); i++) {
+                std::vector<std::string> vec_pts;
+                Utility::SplitString(pt_pairs[i], vec_pts, ",");
+                if(vec_pts.size() == 4) {
+                    int pt_idx = std::atol(vec_pts[0].c_str());
+                    float x = std::atof(vec_pts[1].c_str());
+                    float y = std::atof(vec_pts[2].c_str());
+                    float z = std::atof(vec_pts[3].c_str());
+
+                    LocalCoordinate coord(x, y, z);
+                    DebugPointInfo dbg_pt(pt_idx, coord);
+                    vec_debugPts.push_back(dbg_pt);
+                }
+            }
+        }
+    }
+    else
+    {
+        std::cout << "parse error\n" << std::endl;
+        in.close();
+        return false;
+    }
+
+    in.close();
+
+    return true;
+}
+
+
+

cloud_common/utility/Config.h

+//
+// Created by xueyimeng on 20-10-16.
+//
+
+#ifndef LIDARGROUNDDETECT_CONFIG_H
+#define LIDARGROUNDDETECT_CONFIG_H
+
+#include <memory>
+#include <json/reader.h>
+#include <json/value.h>
+#include "common/TypeDef.h"
+
+class Config {
+private:
+    Config();
+
+public:
+    virtual ~Config();
+
+public:
+    static Config* instance();
+
+public:
+    bool LoadFromJson(const std::string& json_pathname);
+
+private:
+    std::shared_ptr<Config> config;
+public:
+    std::vector<std::string> task_list;
+    std::vector<std::string> pcd_list;
+    std::string data_dir;
+    std::string out_dir;
+    std::string pcd_shortname; //without extension
+    std::string station_filename;
+    std::string result_gpkg_pathname;
+    std::string track_pathname;
+    std::string spatial_filter_pathname;
+    bool save_debug_type;
+    bool is_road_labeld;
+    float grid_size;
+    std::vector<DebugPointInfo> vec_debugPts;
+};
+typedef std::shared_ptr<Config> ConfigPtr;
+#endif //LIDARGROUNDDETECT_CONFIG_H

cloud_common/utility/FileUtility.cpp

+//
+// Created by xueyimeng on 19-11-24.
+//
+
+#include "FileUtility.h"
+#include "Utility.h"
+//#include <filesystem> // C++17
+#include <experimental/filesystem> // C++14
+#include <iostream>
+#include <fstream>
+#include <glog/logging.h>
+
+std::vector<std::string> FileUtility::ReadAllFiles(const std::string& parent_path, const std::string& filter) {
+    std::vector<std::string> vec_filenames;
+    if(!FileUtility::IsFolderExist(parent_path)) {
+        LOG(ERROR) << "path " << parent_path << " do not exist!";
+        return vec_filenames;
+    }
+
+#ifdef __linux__
+    struct dirent *dirp;
+    DIR* dir = opendir(parent_path.c_str());
+    while ((dirp = readdir(dir)) != nullptr) {
+        if (dirp->d_type == DT_REG) {
+            // 文件
+            std::string filename = dirp->d_name;
+            int pos = filename.find_last_of(".");
+            std::string ext = filename.substr(pos, filename.length()-pos);
+            if(ext == filter) {
+                std::string filepathname = parent_path + "/" + filename;
+                vec_filenames.push_back(filepathname);
+            }
+        } else if (dirp->d_type == DT_DIR) {
+            // 文件夹
+            std::string sub_dirname = dirp->d_name;
+            if(sub_dirname == "." || sub_dirname == "..") {
+                continue;
+            }
+            std::string dir_name = parent_path + "/" + dirp->d_name + "/";
+            std::vector<std::string> vec_pathnames = ReadAllFiles(dir_name, filter);
+            if(!vec_pathnames.empty()) {
+                vec_filenames.insert(vec_filenames.end(), vec_pathnames.begin(), vec_pathnames.end());
+            }
+        }
+    }
+
+    closedir(dir);
+#endif
+    return vec_filenames;
+}
+
+std::vector<std::string> FileUtility::ReadAllSubDir(const std::string& parent_path) {
+    std::vector<std::string> vec_subpath;
+    struct dirent *dirp;
+#ifdef __linux__
+    DIR* dir = opendir(parent_path.c_str());
+    while ((dirp = readdir(dir)) != nullptr) {
+        if (dirp->d_type == DT_DIR) {
+            // 文件夹
+            std::string path = dirp->d_name;
+            if(path == "." || path == ".." || path == "tmp")
+                continue;
+
+            std::string fullpath = parent_path + "/" + path;
+            vec_subpath.push_back(fullpath);
+        } else if (dirp->d_type == DT_REG) {
+
+        }
+    }
+
+    closedir(dir);
+#endif
+    return vec_subpath;
+}
+
+bool FileUtility::MakeDir(const std::string& path, int mode) {
+    namespace fs = std::experimental::filesystem;
+    return fs::create_directories(path);
+}
+
+
+bool FileUtility::IsFolderExist(const std::string &path)
+{
+#ifdef __linux__
+    DIR *dp;
+    if ((dp = opendir(path.c_str())) == NULL)
+    {
+        return false;
+    }
+
+    closedir(dp);
+#endif
+    return true;
+}
+
+bool FileUtility::IsDirectory(const std::string& path) {
+    namespace fs = std::experimental::filesystem;
+    std::error_code ec; // For using the non-throwing overloads of functions below.
+    if (fs::is_directory(path, ec)) {
+        return true;
+    }
+    return false;
+}
+//-1是存在
+bool FileUtility::IsFileExist(const std::string& path)
+{
+#ifdef _WIN32
+    return true;
+#elif __linux__
+    //for linux
+    int ret = access(path.c_str(), F_OK);
+    return 0 == ret;
+#endif
+    return true;
+}
+
+bool FileUtility::RemoveDir(const std::string& dir_to_remove) {
+    if(!FileUtility::IsFolderExist(dir_to_remove))
+        return false;
+
+    namespace fs = std::experimental::filesystem;
+    fs::remove_all(dir_to_remove);
+    return true;
+}
+
+bool Parse(const std::map<std::string, std::string>& map_key_value, std::string key, std::string& value) {
+    auto iter = map_key_value.find(key);
+    if(iter == map_key_value.end()) {
+        LOG(ERROR) << "failed to find keyword " << key;
+        return false;
+    }
+
+    value = iter->second;
+    return true;
+}
+
+bool FileUtility::LoadStationInfo(const std::string& station_filename, StationInfo& stationInfo) {
+    std::ifstream ifs(station_filename.c_str());
+    if(!ifs.is_open()) {
+        LOG(ERROR) << "open " << station_filename << " failed!";
+        return false;
+    }
+
+    char buf[1024];
+    std::map<std::string, std::string> map_key_value;
+    while(!ifs.eof()) {
+        ifs.getline(buf, 1024);
+        std::string line = buf;
+        std::vector<std::string> vec_key_value;
+        Utility::SplitString(line, vec_key_value,":");
+        if(vec_key_value.size() != 2) {
+            continue;
+        }
+
+        map_key_value[vec_key_value[0]] = vec_key_value[1];
+    }
+
+    std::string str_ref_lat, str_ref_lon, str_ref_height, str_srid;
+    std::string str_utm_offset_x, str_utm_offset_y, str_utm_offset_z;
+
+    if(!Parse(map_key_value, "LON_WGS84", str_ref_lon)) {
+        return false;
+    }
+
+    if(!Parse(map_key_value, "LAT_WGS84", str_ref_lat)) {
+        return false;
+    }
+
+    if(!Parse(map_key_value, "HIGH_WGS84", str_ref_height)) {
+        return false;
+    }
+
+    if(!Parse(map_key_value, "SRID", str_srid)) {
+        return false;
+    }
+
+    if(!Parse(map_key_value, "E0_PROJECT", str_utm_offset_x)) {
+        return false;
+    }
+
+    if(!Parse(map_key_value, "N0_PROJECT", str_utm_offset_y)) {
+        return false;
+    }
+
+    if(!Parse(map_key_value, "H0_PROJECT", str_utm_offset_z)) {
+        return false;
+    }
+
+    stationInfo.ref_lat = std::atof(str_ref_lat.c_str());
+    stationInfo.ref_lon = std::atof(str_ref_lon.c_str());
+    stationInfo.ref_height = std::atof(str_ref_height.c_str());
+    stationInfo.srid = std::atoi(str_srid.c_str())%100;
+    stationInfo.utm_offset_x = std::atof(str_utm_offset_x.c_str());
+    stationInfo.utm_offset_y = std::atof(str_utm_offset_y.c_str());
+    stationInfo.utm_offset_z = std::atof(str_utm_offset_z.c_str());
+
+    int srid = 31 + std::floor(stationInfo.ref_lon / 6.0);
+    if(srid != stationInfo.srid)
+    {
+        LOG(ERROR) << "source and computed srid is not equal," <<  stationInfo.srid << " vs " <<srid;
+        LOG(ERROR) << stationInfo.ref_lon << "," << stationInfo.ref_lat << "," << stationInfo.ref_height;
+        return false;
+    }
+    return true;
+}
\ No newline at end of file

cloud_common/utility/FileUtility.h

+//
+// Created by xueyimeng on 19-11-24.
+//
+
+#ifndef CPT_CALIBRATION_FILEUTILITY_H
+#define CPT_CALIBRATION_FILEUTILITY_H
+
+#include <map>
+#include <string>
+#include <stdio.h>
+#include <vector>
+#include <stdio.h>
+#include <iostream>
+#include "common/TypeDef.h"
+
+#ifndef _WIN32
+#include <dirent.h>
+#include <unistd.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#endif
+
+class FileUtility {
+public:
+    static std::vector<std::string> ReadAllFiles(const std::string& parent_path, const std::string& filter);
+
+    static std::vector<std::string> ReadAllSubDir(const std::string& parent_path);
+
+    static bool MakeDir(const std::string& path, int mode = 0);
+
+    static bool RemoveDir(const std::string& dir_to_remove);
+
+    static bool IsFolderExist(const std::string &path);
+
+//-1是存在
+    static bool IsFileExist(const std::string& path);
+
+    static bool IsDirectory(const std::string& path);
+
+    static bool LoadStationInfo(const std::string& station_filename, StationInfo& stationInfo);
+};
+
+
+#endif //CPT_CALIBRATION_FILEUTILITY_H

cloud_common/utility/GeomUtility.h

+//
+// Created by xueyimeng on 21-1-25.
+//
+
+//code is originally from wangdawei
+
+#ifndef LIDARGROUNDDETECT_GEOMUTILITY_H
+#define LIDARGROUNDDETECT_GEOMUTILITY_H
+#include <Eigen/Core>
+#include <vector>
+#include "common/TypeDef.h"
+#include "common/PCLType.h"
+
+class GeomUtility {
+public:
+    GeomUtility() {}
+    virtual ~GeomUtility() = default;
+
+public:
+    //返回 点与点的平面距离
+    static double TwoDistancePointAndPoint(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2);
+
+    //返回 点（p0）  到线（p1，p2）的距离
+    static double TwoDistancePointAndLine(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1,const Eigen::Vector3d &p2);
+
+    static double GetCrossProduct(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, const Eigen::Vector3d& p0);
+
+    static std::vector<Eigen::Vector3d> GetConvexHull(const std::vector<Eigen::Vector3d>& list);
+
+    static std::vector<Eigen::Vector2d> MinimumBoundingRectangle(const std::vector<Eigen::Vector3d> &ListConvexHull);
+
+    static std::vector<Eigen::Vector2d> MinimumBoundingRectangleNew(const std::vector<Eigen::Vector3d> &ListConvexHull);
+
+    static std::vector<Eigen::Vector2d> MinimumBounding(const std::vector<Eigen::Vector3d>& list);
+
+    //calculate intersect point of line p1p2 and p3p4
+    static bool Intersect(LocalCoordinate p1, LocalCoordinate p2, LocalCoordinate p3, LocalCoordinate p4, LocalCoordinate& pt_intersect);
+
+    //calculate intersect point of norm direction and line p3p4
+    static bool Intersect(LocalCoordinate p0, Eigen::Vector2d vec_norm, LocalCoordinate p3, LocalCoordinate p4, LocalCoordinate& pt_intersect);
+
+    static bool IsPointOnLineSeg(LocalCoordinate p1, LocalCoordinate p2, LocalCoordinate pt_to_check);
+
+    static bool IsPointInPolygon(LocalCoordinate p1, std::vector<Eigen::Vector2d>& vec_rect_pts);
+
+    static bool IsPointInRect(LocalCoordinate p1, std::vector<Eigen::Vector2d>& vec_rect_pts);
+
+    static std::vector<Eigen::Vector2d> Buffer(const std::vector<Eigen::Vector2d>& orig_rect, double buffer_width);
+
+    static bool IsExtentOverlap(PointXYZIX min_xyz1, PointXYZIX max_xyz1, PointXYZIX min_xyz2, PointXYZIX max_xyz2);
+
+    static bool IsOverlap(double min_a, double max_a, double min_b, double max_b, double buffer = 0.0);
+
+    static bool IsRectOverlap(std::vector<Eigen::Vector2d> vec_rect1, std::vector<Eigen::Vector2d> vec_rect2);
+
+    static double GetFullAngle(double cos_theta, double sin_theta);
+
+    static std::vector<LocalCoordinate> DouglasPeucker2(const std::vector<LocalCoordinate>& vec_orig_pts, double eps,
+                                                 int beg,
+                                                 int last);
+
+    static std::vector<RefPoint> DouglasPeucker2(const std::vector<RefPoint>& vec_orig_pts, double eps,
+                                                        int beg,
+                                                        int last);
+
+    static bool
+    HermitCurve(LocalCoordinate p0, LocalCoordinate p1, LocalCoordinate p2, LocalCoordinate p3, float interval,
+                             std::vector<LocalCoordinate> &result_contour);
+
+public:
+    static double epsilon;
+};
+
+
+#endif //LIDARGROUNDDETECT_GEOMUTILITY_H

cloud_common/utility/LineSegment.cpp

+//
+// Created by xueyimeng on 21-2-2.
+//
+
+#include "LineSegment.h"
+#include <cmath>
+
+bool zeroDist(float dist) {
+    return std::fabs(dist) < 0.1f;
+}
+
+bool nearZero(const float& value) {
+    const float epsilon = 0.0001f;
+
+    return std::fabs(value) < epsilon;
+}
+
+float Point::x() const {
+    return m_x;
+}
+
+void Point::setX(float x) {
+    m_x = x;
+}
+
+float Point::y() const {
+    return m_y;
+}
+
+void Point::setY(float y) {
+    m_y = y;
+}
+
+Point &Point::operator-=(const Point &other) {
+    m_x -= other.x();
+    m_y -= other.y();
+    return *this;
+}
+
+Point &Point::operator+=(const Point &other) {
+    m_x += other.x();
+    m_y += other.y();
+    return *this;
+}
+
+Point &Point::operator*=(float value) {
+    m_x *= value;
+    m_y *= value;
+    return *this;
+}
+
+Point Point::normalized() const {
+    float len_inv = 1 / this->length();
+    return Point(this->x() * len_inv, this->y() * len_inv);
+}
+
+float Point::cross(const Point &other) const {
+    return this->x() * other.y() - this->y() * other.x();
+}
+
+float Point::dot(const Point &other) const {
+    return this->x() * other.x() + this->y() * other.y();
+}
+
+float Point::length() const {
+    return std::sqrt(this->squaredLength());
+}
+
+float Point::squaredLength() const {
+    return this->x() * this->x() + this->y() * this->y();
+}
+
+
+
+LineSegment::LineSegment(const Point &start, const Point &end) : m_start(start), m_end(end) {
+}
+
+LineSegmentPtr LineSegment::create(const Point &start, const Point &end) {
+    return std::shared_ptr<LineSegment>(new LineSegment(start, end));
+}
+
+LineSegmentPtr LineSegment::create(float x1, float y1, float x2, float y2) {
+    return create(Point(x1, y1), Point(x2, y2));
+}
+
+LineSegment LineSegment::init(const Point &start, const Point &end) {
+    return LineSegment(start, end);
+}
+
+LineSegment LineSegment::init(float x1, float y1, float x2, float y2) {
+    return init(Point(x1, y1), Point(x2, y2));
+}
+
+const Point &LineSegment::start() const {
+    return m_start;
+}
+
+void LineSegment::setStart(const Point &start) {
+    m_start = start;
+}
+
+const Point &LineSegment::end() const {
+    return m_end;
+}
+
+void LineSegment::setEnd(const Point &end) {
+    m_end = end;
+}
+
+void LineSegment::swapEndPoints() {
+    Point
+    tmp = m_end;
+    m_end = m_start;
+    m_start = tmp;
+}
+
+Point LineSegment::vec() const {
+    return m_end - m_start;
+}
+
+float LineSegment::length() const {
+    return vec().length();
+}
+
+bool LineSegment::valid() const {
+    return std::isfinite(m_start.x()) &&
+           std::isfinite(m_start.y()) &&
+           std::isfinite(m_end.x()) &&
+           std::isfinite(m_end.y()) &&
+           !nearZero(this->length());
+}
+
+bool LineSegment::operator==(const LineSegment &other) const {
+    return (nearZero((m_start - other.start()).squaredLength()) &&
+            nearZero((m_end - other.end()).squaredLength())) ||
+           (nearZero((m_end - other.start()).squaredLength()) &&
+            nearZero((m_start - other.end()).squaredLength()));
+}
+
+Point operator*(float scalar, Point point) {
+    return point *= scalar;
+}
+
+Point operator*(Point point, float scalar) {
+    return point *= scalar;
+}
+
+Point operator-(const Point &point) {
+    return Point(-point.x(), -point.y());
+}
+
+Point operator+(Point lhs, const Point &rhs) {
+    return lhs += rhs;
+}
+
+Point operator-(Point lhs, const Point &rhs) {
+    return lhs -= rhs;
+}
\ No newline at end of file

cloud_common/utility/LineSegment.h

+//
+// Created by xueyimeng on 21-2-2.
+//
+
+#ifndef LIDARGROUNDDETECT_LINESEGMENT_H
+#define LIDARGROUNDDETECT_LINESEGMENT_H
+
+#include "common/TypeDef.h"
+#include <memory>
+
+bool zeroDist(float dist);
+bool nearZero(const float& value);
+
+
+class Point {
+public:
+   constexpr Point() : m_x(0.f), m_y(0.f) {
+   }
+
+   constexpr Point(float x, float y) : m_x(x), m_y(y) {
+   }
+
+   float x() const;
+   void setX(float x);
+
+   float y() const;
+   void setY(float y);
+
+   Point& operator-=(const Point& other);
+   Point& operator+=(const Point& other);
+   Point& operator*=(float value);
+   Point normalized() const;
+   float cross(const Point& other) const;
+   float dot(const Point& other) const;
+   float length() const;
+   float squaredLength() const;
+
+private:
+   float m_x;
+   float m_y;
+};
+
+Point operator+(Point lhs, const Point& rhs);
+Point operator-(Point lhs, const Point& rhs);
+Point operator*(float scalar, Point point);
+Point operator*(Point point, float scalar);
+Point operator-(const Point& point);
+
+class LineSegment;
+typedef std::shared_ptr<LineSegment> LineSegmentPtr;
+
+class LineSegment {
+public:
+   static LineSegmentPtr create(const Point& start, const Point& end);
+   static LineSegmentPtr create(float x1, float y1, float x2, float y2);
+   static LineSegment init(const Point& start, const Point& end);
+   static LineSegment init(float x1, float y1, float x2, float y2);
+
+   const Point& start() const;
+   void setStart(const Point& start);
+
+   const Point& end() const;
+   void setEnd(const Point& end);
+
+   void swapEndPoints();
+
+   Point vec() const;
+
+   float length() const;
+
+   bool valid() const;
+
+   bool operator==(const LineSegment& other) const;
+
+private:
+   LineSegment(const Point& start, const Point& end);
+   Point m_start;
+   Point m_end;
+};
+
+
+#endif //LIDARGROUNDDETECT_LINESEGMENT_H

cloud_common/utility/LineUtility.cpp

+//
+// Created by xueyimeng on 21-2-2.
+//
+
+#include "LineUtility.h"
+
+#include <cmath>
+#include <limits>
+#include <stdexcept>
+#include <assert.h>
+
+constexpr Point scanLineVec(1.f, 0.f);
+
+
+bool LineUtility::inOrder(const Point& lhs, const Point& rhs) {
+    if (nearZero(lhs.y() - rhs.y())) {
+        return lhs.x() < rhs.x();
+    }
+    return lhs.y() > rhs.y();
+}
+
+//adjust direction of linesegment
+LineSegmentPtr LineUtility::directSegment(LineSegmentPtr frag) {
+    if (!inOrder(frag->start(), frag->end())) {
+        frag->swapEndPoints();
+    }
+
+    return frag;
+}
+
+int LineUtility::intersection(const LineSegment& a, const LineSegment& b, Point& outIntersectionPoint, float& outDistanceFromAStart, float& outDistanceFromBStart,
+                 float maxDistance) {
+    // Let: a_s = a.start(), a_v = a.vec(), b_s = b.start() and b_v = b.vec()
+    // Solve: a_s + t * a_v = b_s + r * b_v
+    // (a_s + t * a_v) x b_v        = (b_s + r * b_v) x b_v | x denotes the cross product
+    // a_s x b_v + t * a_v x b_v    = b_s x b_v             | v x v = 0
+    // t * a_v x b_v                = b_s x b_v - a_s x b_v
+    // t                            = ((b_s - a_s) x b_v) / (a_v x b_v)
+    // r                            = ((a_s - b_s) x a_v) / -(a_v x b_v)
+
+    Point a_vec = a.vec().normalized();
+    Point b_vec = b.vec().normalized();
+
+    float a_len = a.length();
+    float b_len = b.length();
+
+    Point diff = b.start() - a.start();
+    float cross_vecs = a_vec.cross(b_vec);
+    float cross_diff = diff.cross(b_vec);
+
+    if (nearZero(cross_vecs)) {
+        // a and b are collinear
+        if (!nearZero(cross_diff)) {
+            // a and b are parallel
+            return -1;
+        }
+
+        // a and b are on the same line... check for overlap by projecting b's endpoints onto a
+        float proj_bs = a_vec.dot(diff);                 // b.start() onto a
+        float proj_be = a_vec.dot(b.end() - a.start());  // b.end() onto a
+
+        float nearest = proj_bs;
+        outDistanceFromBStart = 0.f;
+        if (std::fabs(proj_be) < std::fabs(proj_bs)) {
+            nearest = proj_be;
+            outDistanceFromBStart = b_len;
+        }
+
+        if (nearest < -maxDistance || nearest > a_len + maxDistance) {
+            // no overlap
+            return -2;
+        }
+
+        outDistanceFromAStart = nearest;
+        outIntersectionPoint = a.start() + nearest * a_vec;
+
+        return 2;
+    }
+
+    float t = cross_diff / cross_vecs;
+    if (t < -maxDistance || t > a_len + maxDistance) {
+        return 0;
+    }
+
+    float r = (-diff).cross(a_vec) / -cross_vecs;
+    if (r < -maxDistance || r > b_len + maxDistance) {
+        return 0;
+    }
+
+    outDistanceFromAStart = t;
+    outDistanceFromBStart = r;
+    outIntersectionPoint = a.start() + t * a_vec;
+
+    return 1;
+}
+
+int LineUtility::intersection(const LineSegmentPtr& a, const LineSegmentPtr& b, Point& outIntersectionPoint, float& outDistanceFromAStart, float& outDistanceFromBStart,
+                 float maxDistance) {
+    return LineUtility::intersection(*a, *b, outIntersectionPoint, outDistanceFromAStart, outDistanceFromBStart, maxDistance);
+}
+
+float LineUtility::lineIntersection(const Point& lineSupportVector, const Point& lineNormedVector, const LineSegment& segment) {
+    // See intersection function for explanation
+    Point diff = segment.start() - lineSupportVector;
+    Point frag_vec = segment.vec();
+    float crossVecs = lineNormedVector.cross(frag_vec);
+    float crossDiff = diff.cross(frag_vec);
+
+    if (nearZero(crossVecs)) {
+        if (!nearZero(crossDiff)) {
+            return std::numeric_limits<float>::quiet_NaN();
+        }
+
+        float distance = diff.length();
+
+        if (lineNormedVector.dot(diff) > 0) {
+            return distance;
+        }
+
+        return -distance;
+    }
+
+    float r = (-diff).cross(lineNormedVector) / -crossVecs;
+    if ((r < 0 && !nearZero(r)) || (r > 1 && !nearZero(1.f - r))) {
+        return std::numeric_limits<float>::quiet_NaN();
+    }
+
+    float t = crossDiff / crossVecs;
+
+    return t;
+}
+
+float LineUtility::lineIntersection(const Point& lineSupportVector, const Point& lineNormedVector, const LineSegmentPtr& segment) {
+    return LineUtility::lineIntersection(lineSupportVector, lineNormedVector, *segment);
+}
+
+float LineUtility::getPositionOnScanLine(const LineSegment& segment, const Point& eventPoint) {
+    float val = lineIntersection(eventPoint, scanLineVec, segment);
+    assert(!std::isnan(val));
+    return val;
+}
+
+float LineUtility::getPositionOnScanLine(const LineSegmentPtr& segment, const Point& eventPoint) {
+    return LineUtility::getPositionOnScanLine(*segment, eventPoint);
+}
+
+bool LineUtility::isALeftOfBOnScanLine(const LineSegment& segment_a, const LineSegment& segment_b, const Point& eventPoint) {
+    float posA = getPositionOnScanLine(segment_a, eventPoint);
+    float posB = getPositionOnScanLine(segment_b, eventPoint);
+
+    if (nearZero(posA - posB)) {
+        // Both fragments have the same position on the scan line (they intersect the scan line at the same point)
+
+        // Fragments parallel to the scan line are always right of fragments not parallel to the scan line
+        // (note: this convention only works if event points are ordered from left to right if they have the same y-coordinate...
+        // hence a fragment parallel to the scan line always ends right of the event point. Otherwise its end point would be its start point)
+        bool aParallelToScanLine = nearZero(segment_a.vec().cross(scanLineVec));
+        bool bParallelToScanLine = nearZero(segment_b.vec().cross(scanLineVec));
+
+        if (aParallelToScanLine && bParallelToScanLine) {
+            // If both are parallel...
+            return segment_a.end().x() < segment_b.end().x();
+        } else if (aParallelToScanLine) {
+            return false;
+        } else if (bParallelToScanLine) {
+            return true;
+        }
+
+        // Move the scan line down (as far as possible) and use the fragments position on that scan line
+        Point newScanLinePos(0.f, std::max(segment_a.end().y(), segment_b.end().y()));
+
+        posA = LineUtility::getPositionOnScanLine(segment_a, newScanLinePos);
+        posB = LineUtility::getPositionOnScanLine(segment_b, newScanLinePos);
+
+        if (nearZero(posA - posB)) {
+            // The fragments are on the same (vertical) line... let the one with the lowest y-coordinate be left
+            if (!nearZero(segment_a.end().y() - segment_b.end().y())) {
+                return segment_a.end().y() < segment_b.end().y();
+            }
+
+            if (!nearZero(segment_a.start().y() - segment_b.start().y())) {
+                return segment_a.start().y() < segment_b.start().y();
+            }
+
+            throw std::runtime_error("Duplicate Fragment.");
+        }
+    }
+
+    return posA < posB;
+}
+
+bool LineUtility::isALeftOfBOnScanLine(const LineSegmentPtr& segment_a, const LineSegmentPtr& segment_b, const Point& eventPoint) {
+    return isALeftOfBOnScanLine(*segment_a, *segment_b, eventPoint);
+}
\ No newline at end of file

cloud_common/utility/LineUtility.h

+//
+// Created by xueyimeng on 21-2-2.
+//
+
+#ifndef LIDARGROUNDDETECT_LINEUTILITY_H
+#define LIDARGROUNDDETECT_LINEUTILITY_H
+
+#include "LineSegment.h"
+
+enum IntersectStatus {
+    PARALLEL_NO_INTERSECTION = -1,
+    NO_OVERLAP = -2,
+};
+
+class LineUtility {
+public:
+    LineUtility() {}
+    virtual ~LineUtility() {}
+
+public:
+    static bool inOrder(const Point& lhs, const Point& rhs);
+
+    static LineSegmentPtr directSegment(LineSegmentPtr frag);
+
+    static int intersection(const LineSegment& a, const LineSegment& b, Point& outIntersectionPoint, float& outDistanceFromAStart, float& outDistanceFromBStart,
+                            float maxDistance);
+
+    static int intersection(const LineSegmentPtr& a, const LineSegmentPtr& b, Point& outIntersectionPoint, float& outDistanceFromAStart, float& outDistanceFromBStart,
+                            float maxDistance);
+
+    static float lineIntersection(const Point& lineSupportVector, const Point& lineNormedVector, const LineSegment& segment);
+
+    static float lineIntersection(const Point& lineSupportVector, const Point& lineNormedVector, const LineSegmentPtr& segment);
+
+    static float getPositionOnScanLine(const LineSegment& segment, const Point& eventPoint);
+
+    static float getPositionOnScanLine(const LineSegmentPtr& segment, const Point& eventPoint);
+
+    static bool isALeftOfBOnScanLine(const LineSegment& segment_a, const LineSegment& segment_b, const Point& eventPoint);
+
+    static bool isALeftOfBOnScanLine(const LineSegmentPtr& segment_a, const LineSegmentPtr& segment_b, const Point& eventPoint);
+};
+
+
+#endif //LIDARGROUNDDETECT_LINEUTILITY_H

cloud_common/utility/PointInPolygon.cpp

+//
+// Created by xueyimeng on 21-3-6.
+//
+
+#include "PointInPolygon.h"
+
+Polygon::Polygon(std::vector<LocalCoordinate>& points): points(points) {
+    calcBoundingBox(points);
+}
+
+bool Polygon::inPolygon(LocalCoordinate point) {
+    if (!inBoundingBox(point)) {
+        return false;
+    }
+
+    //create a ray (segment) starting from the given point,
+    //and to the point outside of polygon.
+    LocalCoordinate outside(bbox.xmin - 10, bbox.ymin);
+    int intersections = 0;
+    //check intersections between the ray and every side of the polygon.
+    for (int i = 0; i < points.size() - 1; ++i) {
+        if (segmentIntersect(point, outside, points.at(i), points.at(i + 1))) {
+            intersections++;
+        }
+    }
+    //check the last line
+    if (segmentIntersect(point, outside, points.at(points.size() - 1), points.at(0))) {
+        intersections++;
+    }
+    return (intersections % 2 != 0);
+}
+
+
+int Polygon::direction(LocalCoordinate pi, LocalCoordinate pj, LocalCoordinate pk) {
+    return (pk.x - pi.x) * (pj.y - pi.y) - (pj.x - pi.x) * (pk.y - pi.y);
+}
+
+bool Polygon::onSegment(LocalCoordinate pi, LocalCoordinate pj, LocalCoordinate pk) {
+    if (std::min(pi.x, pj.x) <= pk.x && pk.x <= std::max(pi.x, pj.x)
+        && std::min(pi.y, pj.y) <= pk.y && pk.y <= std::max(pi.y, pj.y)) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool Polygon::segmentIntersect(LocalCoordinate p1, LocalCoordinate p2, LocalCoordinate p3, LocalCoordinate p4) {
+    int d1 = direction(p3, p4, p1);
+    int d2 = direction(p3, p4, p2);
+    int d3 = direction(p1, p2, p3);
+    int d4 = direction(p1, p2, p4);
+
+    if (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) &&
+        ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) {
+        return true;
+    } else if (d1 == 0 && onSegment(p3, p4, p1)) {
+        return true;
+    } else if (d2 == 0 && onSegment(p3, p4, p2)) {
+        return true;
+    } else if (d3 == 0 && onSegment(p1, p2, p3)) {
+        return true;
+    } else if (d4 == 0 && onSegment(p1, p2, p4)) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool Polygon::inBoundingBox(LocalCoordinate point) {
+    if (point.x < bbox.xmin || point.x > bbox.xmax || point.y < bbox.ymin || point.y > bbox.ymax) {
+        return false;
+    } else {
+        return true;
+    }
+}
+
+void Polygon::calcBoundingBox(std::vector<LocalCoordinate> points) {
+    for (auto &point : points) {
+        if (point.x < bbox.xmin) {
+            bbox.xmin = point.x;
+        }
+
+        if (point.x > bbox.xmax) {
+            bbox.xmax = point.x;
+        }
+
+        if (point.y < bbox.ymin) {
+            bbox.ymin = point.y;
+        }
+
+        if (point.y > bbox.ymax) {
+            bbox.ymax = point.y;
+        }
+    }
+}

cloud_common/utility/PointInPolygon.h

+//
+// Created by xueyimeng on 21-3-6.
+//
+
+/**
+ * Header only class for handling point in polygon problem.
+ * The line-segments intersection algorithm is implemented based on Introduction to Algorithms 3rd (CLRS).
+ * Point in polygon is implemented based on https://stackoverflow.com/questions/217578/how-can-i-determine-whether-a-2d-point-is-within-a-polygon.
+ *
+ * Created by Zebin Xu on 9/27/2017
+ */
+#ifndef _POLYGON_H_
+#define _POLYGON_H_
+
+#include <vector>
+#include <limits>
+#include <algorithm>
+#include "common/TypeDef.h"
+
+struct BoundingBox {
+    double xmin;
+    double xmax;
+    double ymin;
+    double ymax;
+
+    BoundingBox(double xmin, double xmax, double ymin, double ymax) {
+        this->xmin = xmin;
+        this->xmax = xmax;
+        this->ymin = ymin;
+        this->ymax = ymax;
+    }
+
+    BoundingBox() {
+        this->xmin = DBL_MAX;
+        this->xmax = -DBL_MAX;
+        this->ymin = DBL_MAX;
+        this->ymax = -DBL_MAX;
+    }
+};
+
+class Polygon {
+public:
+    Polygon(std::vector<LocalCoordinate>& points);
+
+    bool inPolygon(LocalCoordinate point);
+
+protected:
+
+    int direction(LocalCoordinate pi, LocalCoordinate pj, LocalCoordinate pk);
+
+    bool onSegment(LocalCoordinate pi, LocalCoordinate pj, LocalCoordinate pk);
+
+    bool segmentIntersect(LocalCoordinate p1, LocalCoordinate p2, LocalCoordinate p3, LocalCoordinate p4);
+
+private:
+
+    bool inBoundingBox(LocalCoordinate point);
+
+    void calcBoundingBox(std::vector<LocalCoordinate> points);
+
+private:
+    std::vector<LocalCoordinate> points;
+    BoundingBox bbox;
+};
+
+
+#endif /* _POLYGON_H_ */

cloud_common/utility/Reporter.cpp

+//
+// Created by xueyimeng on 21-9-17.
+//
+
+#include "Reporter.h"
+#include <fstream>
+
+FileReporter::~FileReporter() {
+    if(fs.is_open()) {
+        fs.close();
+    }
+}
+
+FileReporter::FileReporter(const std::string& result_pathname) {
+    this->result_pathname = result_pathname;
+    if(!fs.is_open()) {
+        fs.open(result_pathname.c_str());
+    }
+}
+
+void FileReporter::report(const std::string& message) {
+    if(fs.is_open()) {
+        mutex.lock();
+        fs << message << std::endl;
+        mutex.unlock();
+    }
+}
\ No newline at end of file

cloud_common/utility/Reporter.h

+//
+// Created by xueyimeng on 21-9-17.
+//
+
+#ifndef LIDARGROUNDDETECT_REPORTER_H
+#define LIDARGROUNDDETECT_REPORTER_H
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <fstream>
+#include <mutex>
+
+class Reporter {
+public:
+    Reporter() {}
+    virtual ~Reporter() = default;
+
+    virtual void report(const std::string& message) = 0;
+};
+typedef std::shared_ptr<Reporter> ReporterPtr;
+
+
+class FileReporter : public Reporter {
+protected:
+    FileReporter() {}
+public:
+    FileReporter(const std::string& reporter);
+    virtual ~FileReporter();
+
+public:
+    virtual void report(const std::string& message);
+
+
+private:
+    std::string result_pathname;
+    std::fstream fs;
+    std::timed_mutex mutex;
+};
+typedef std::shared_ptr<FileReporter> FileReporterPtr;
+#endif //LIDARGROUNDDETECT_REPORTER_H

cloud_common/utility/SafeQueue.h

+//
+// Created by xueyimeng on 20-10-18.
+//
+
+#ifndef LIDARGROUNDDETECT_SAFEQUEUE_H
+#define LIDARGROUNDDETECT_SAFEQUEUE_H
+#include <mutex>
+#include <queue>
+// Thread safe implementation of a Queue using a std::queue
+
+template <typename T>
+class SafeQueue {
+private:
+    std::queue<T> m_queue; //利用模板函数构造队列
+
+    std::mutex m_mutex;//访问互斥信号量
+
+public:
+    SafeQueue() { //空构造函数
+
+
+    }
+
+    SafeQueue(SafeQueue& other) {//拷贝构造函数
+
+        //TODO:
+    }
+
+    ~SafeQueue() { //析构函数
+
+    }
+
+
+    bool empty() {  //队列是否为空
+
+        std::unique_lock<std::mutex> lock(m_mutex); //互斥信号变量加锁，防止m_queue被改变
+
+        return m_queue.empty();
+    }
+
+    int size() {
+        std::unique_lock<std::mutex> lock(m_mutex); //互斥信号变量加锁，防止m_queue被改变
+
+        return m_queue.size();
+    }
+//队列添加元素
+
+    void enqueue(T& t) {
+        std::unique_lock<std::mutex> lock(m_mutex);
+        m_queue.push(t);
+    }
+//队列取出元素
+
+    bool dequeue(T& t) {
+        std::unique_lock<std::mutex> lock(m_mutex); //队列加锁
+
+        if (m_queue.empty()) {
+            return false;
+        }
+        t = std::move(m_queue.front()); //取出队首元素，返回队首元素值，并进行右值引用
+
+        m_queue.pop(); //弹出入队的第一个元素
+
+        return true;
+    }
+};
+
+#endif //LIDARGROUNDDETECT_SAFEQUEUE_H

cloud_common/utility/SegmentationHelper.h

+//
+// Created by xueyimeng on 21-3-2.
+//
+
+#ifndef LIDARGROUNDDETECT_SEGMENTATIONHELPER_H
+#define LIDARGROUNDDETECT_SEGMENTATIONHELPER_H
+
+#ifndef PCL_NO_PRECOMPILE
+#define PCL_NO_PRECOMPILE
+#endif
+
+#include <pcl/PointIndices.h>
+#include <set>
+#include "common/Grid.h"
+#include "common/TypeDef.h"
+#include "common/PCLType.h"
+
+class SegmentationHelper {
+public:
+    static bool FilterNoiseByDensity(pcl::PointCloud<PointXYZIX>::Ptr cloud_data, double radius, int min_pts, int threshold, std::set<int>& noise_ptIdxs);
+
+    static std::vector <pcl::PointIndices> MinCutSegment(pcl::PointCloud<PointXYZIX>::Ptr cloud_data);
+
+    static std::vector <pcl::PointIndices> SuperVoxelSegment(pcl::PointCloud<PointXYZIX>::Ptr cloud_data);
+
+    static std::vector <pcl::PointIndices> LccpSegment(pcl::PointCloud<PointXYZIX>::Ptr cloud_data);
+
+    static std::vector<pcl::PointIndices> AdaptiveSegment(pcl::PointCloud<PointXYZIX>::Ptr cloud_data);
+
+public:
+
+    static void GroupPoints(PointType group_type, RoadGridPtr grid, const std::set<GridCellPtr>& seed_cells, std::vector<std::vector<GridCellPtr>>& clusters);
+
+
+    static void GroupObjectPointsNew(pcl::PointCloud<PointXYZIX>::Ptr cloud_data, float grid_size,
+                                     std::set<PointType> object_filter,
+                                     std::vector<std::vector<GridCellPtr>>& clusters,
+                                     RoadGridPtr obj_grid = nullptr);
+
+    static void MergeClusters(pcl::PointCloud<PointXYZIX>::Ptr cloud_data, float grid_size,
+                              const std::vector<std::set<GridCellPtr>>& orig_clusters,
+                              std::vector<std::set<GridCellPtr>>& result_clusters);
+
+    static bool GroupEmptyCells(RoadGridPtr road_grid, std::vector<std::vector<int64_t>>& vec_emptycell_clusters);
+};
+#endif //LIDARGROUNDDETECT_SEGMENTATIONHELPER_H

cloud_common/utility/ThreadPool.h

+//
+// Created by xueyimeng on 20-10-18.
+//
+
+#ifndef LIDARGROUNDDETECT_THREADPOOL_H
+#define LIDARGROUNDDETECT_THREADPOOL_H
+
+
+//ThreadPool.h
+
+#pragma once
+#include <functional>
+#include <future>
+#include <mutex>
+#include <queue>
+#include <thread>
+#include <utility>
+#include <vector>
+#include "SafeQueue.h"
+
+class ThreadPool {
+private:
+    class ThreadWorker {//内置线程工作类
+
+    private:
+        int m_id; //工作id
+
+        ThreadPool * m_pool;//所属线程池
+
+    public:
+        //构造函数
+
+        ThreadWorker(ThreadPool * pool, const int id)
+                : m_pool(pool), m_id(id) {
+        }
+        //重载`()`操作
+
+        void operator()() {
+            std::function<void()> func; //定义基础函数类func
+
+            bool dequeued; //是否正在取出队列中元素
+
+            //判断线程池是否关闭，没有关闭，循环提取
+
+            while (!m_pool->m_shutdown) {
+                {
+                    //为线程环境锁加锁，互访问工作线程的休眠和唤醒
+
+                    std::unique_lock<std::mutex> lock(m_pool->m_conditional_mutex);
+                    //如果任务队列为空，阻塞当前线程
+
+                    if (m_pool->m_queue.empty()) {
+                        m_pool->m_conditional_lock.wait(lock); //等待条件变量通知，开启线程
+
+                    }
+                    //取出任务队列中的元素
+
+                    dequeued = m_pool->m_queue.dequeue(func);
+                }
+                //如果成功取出，执行工作函数
+
+                if (dequeued) {
+                    func();
+                }
+            }
+        }
+    };
+
+    bool m_shutdown; //线程池是否关闭
+
+    SafeQueue<std::function<void()>> m_queue;//执行函数安全队列，即任务队列
+
+    std::vector<std::thread> m_threads; //工作线程队列
+
+    std::mutex m_conditional_mutex;//线程休眠锁互斥变量
+
+    std::condition_variable m_conditional_lock; //线程环境锁，让线程可以处于休眠或者唤醒状态
+
+public:
+    //线程池构造函数
+
+    ThreadPool(const int n_threads)
+            : m_threads(std::vector<std::thread>(n_threads)), m_shutdown(false) {
+    }
+
+    ThreadPool(const ThreadPool &) = delete; //拷贝构造函数，并且取消默认父类构造函数
+
+    ThreadPool(ThreadPool &&) = delete; // 拷贝构造函数，允许右值引用
+
+    ThreadPool & operator=(const ThreadPool &) = delete; // 赋值操作
+
+    ThreadPool & operator=(ThreadPool &&) = delete; //赋值操作
+
+    // Inits thread pool
+
+    void init() {
+        for (int i = 0; i < m_threads.size(); ++i) {
+            m_threads[i] = std::thread(ThreadWorker(this, i));//分配工作线程
+        }
+    }
+
+    // Waits until threads finish their current task and shutdowns the pool
+
+    void shutdown() {
+        m_shutdown = true;
+        m_conditional_lock.notify_all(); //通知 唤醒所有工作线程
+
+        for (int i = 0; i < m_threads.size(); ++i) {
+            if(m_threads[i].joinable()) { //判断线程是否正在等待
+
+                m_threads[i].join();  //将线程加入等待队列
+
+            }
+        }
+    }
+
+    // Submit a function to be executed asynchronously by the pool
+    //线程的主要工作函数，使用了后置返回类型，自动判断函数返回值
+
+    template<typename F, typename...Args>
+    auto submit(F&& f, Args&&... args) -> std::future<decltype(f(args...))> {
+        // Create a function with bounded parameters ready to execute
+        //
+        using return_type = typename std::result_of<F(Args...)>::type;
+        std::function<decltype(f(args...))()> func = std::bind(std::forward<F>(f), std::forward<Args>(args)...);//连接函数和参数定义，特殊函数类型,避免左、右值错误
+
+        // Encapsulate it into a shared ptr in order to be able to copy construct // assign
+        //封装获取任务对象，方便另外一个线程查看结果
+
+        auto task_ptr = std::make_shared<std::packaged_task<decltype(f(args...))()>>(func);
+
+        // Wrap packaged task into void function
+        //利用正则表达式，返回一个函数对象
+
+        std::function<void()> wrapper_func = [task_ptr]() {
+            (*task_ptr)();
+        };
+
+        // 队列通用安全封包函数，并压入安全队列
+
+        m_queue.enqueue(wrapper_func);
+
+        // 唤醒一个等待中的线程
+
+        m_conditional_lock.notify_one();
+
+        // 返回先前注册的任务指针
+        std::future<return_type> res = task_ptr->get_future();
+        return res;
+    }
+};
+
+#endif //LIDARGROUNDDETECT_THREADPOOL_H

cloud_common/utility/Utility.h

+//
+// Created by xueyimeng on 20-9-23.
+//
+
+#ifndef LIDAR_MAPPING_PROJECT_SERVER_UTILITY_H
+#define LIDAR_MAPPING_PROJECT_SERVER_UTILITY_H
+
+#include <map>
+#include "common/TypeDef.h"
+#include "common/Grid.h"
+#include "common/PCLType.h"
+
+#include <Eigen/Dense>
+
+class Utility {
+public:
+    Utility();
+
+    virtual ~Utility();
+
+public:
+    static void SplitString(const std::string &srcstr,                   //原始字符串
+                            std::vector<std::string> &splitstrs,         //返回结果
+                            const std::string &pattern);            //指定的分隔符
+    static LocalCoordinate FootOnPlane(PlanePtr plane, LocalCoordinate local_coord);
+
+    static double Distance2Plane(PlanePtr plane, LocalCoordinate pt);
+
+    static double Distance2PlaneWithDir(PlanePtr plane, LocalCoordinate pt);
+
+    static PlanePtr FitPlaneWithPoints(const std::vector<LocalCoordinate> &vec_pts);
+
+    static PlanePtr
+    FitPlaneRansac(const std::vector<LocalCoordinate> &points, float err_thres, std::vector<int> &vec_outlier_Idxs,
+                   int iteration = 200);
+
+    static float DistanceBetween(LocalCoordinate p1, LocalCoordinate p2);
+
+    static float DistanceBetween(RefPoint p1, RefPoint p2);
+
+    static bool Project3dVectorToPlane(PlanePtr plane, Eigen::Vector3d src_vec, Eigen::Vector3d &dst_vec);
+
+    static bool obb(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                    Eigen::Matrix3f &rotational_matrix_OBB,
+                    pcl::PointXYZ &min_point_OBB,
+                    pcl::PointXYZ &max_point_OBB,
+                    pcl::PointXYZ &position_OBB);
+
+    static bool IsPCDValid(const std::string &pcd_pathname);
+
+    static std::string PointTypeToName(PointType pt_type);
+
+    static PointType PointNameToType(const std::string &pt_typename);
+
+    static RoadGridPtr BuildGrid(TrainedCloud::Ptr cloud_data, float grid_size, bool split_level = false,
+                                 std::set<PointType> filters = std::set<PointType>(), float level_gap = 0.5);
+
+    static RoadGridPtr BuildGrid(pcl::PointCloud<PointXYZIX>::Ptr cloud_data, float grid_size, bool split_level = false,
+                                 std::set<PointType> filters = std::set<PointType>(), float level_gap = 0.5);
+
+    static void UpdateGridCell(pcl::PointCloud<PointXYZIX>::Ptr cloud_data, RoadGridPtr grid, GridCellPtr cell);
+
+    static bool ExpandGrid3x3(RoadGridPtr road_grid,
+                              GridCellPtr seed_cell,
+                              std::vector<GridCellPtr> &cluster_cells,
+                              std::set<GridCellPtr> &visited_cells,
+                              PointType group_type);
+
+    static bool IsCellEdge(RoadGridPtr grid, GridCellPtr cell);
+
+    static bool CellHasPointType(PointCloudXYZIX::Ptr cloud_data, GridCellPtr cell, PointType pt_type);
+
+
+    static bool FilterNoiseByDensity(pcl::PointCloud<PointXYZIX>::Ptr cloud_data, double radius, int threshold,
+                                     std::set<PointType> filter,
+                                     std::vector<int> &noise_ptIdxs);
+
+    static bool FilterNoiseByDensity(const std::vector<Eigen::Vector3d> &cloud_data, double radius, int threshold,
+                                     std::set<int> &noise_ptIdxs);
+
+    static bool
+    CalculateDensity(pcl::PointCloud<PointXYZIX>::Ptr cloud_data, RoadGridPtr road_grid, int pt_idx, double radius,
+                     int &density);
+
+    static bool CheckHasInvalidLabel(PointCloudXYZIX::Ptr cloud_data);
+
+    static void SaveLabelInfo(const std::string &result_dir, const std::string &result_name);
+
+    static void SignalHandle(const char *data, int size);
+};
+
+
+#endif //LIDAR_MAPPING_PROJECT_SERVER_UTILITY_H

cloud_common/utility/lat_lon_cartersian.hpp

+#pragma once
+#include <memory>
+#include <cmath>
+#include <GeographicLib/LocalCartesian.hpp>
+
+#include <Eigen/Dense>
+
+namespace jf {
+
+class LatLonCartesian {
+ public:
+	using Ptr = std::shared_ptr<LatLonCartesian>;
+	using ConstPtr = std::shared_ptr<const LatLonCartesian>;
+	
+	LatLonCartesian()
+			: geographic_local_(std::numeric_limits<double>::max(),
+			                    std::numeric_limits<double>::max(),
+			                    std::numeric_limits<double>::max()) {}
+	LatLonCartesian(double lat, double lng, double h)
+			: geographic_local_(lat, lng, h) {}
+	
+	inline bool isOriginSet() const {
+		return std::abs(geographic_local_.LatitudeOrigin()) < 360.0;
+	}
+	
+	inline void setOrigin(double lat0, double lon0, double h0) {
+		geographic_local_.Reset(lat0, lon0, h0);
+	}
+	
+	inline Eigen::Vector3d getOrigin() const {
+		return {geographic_local_.LatitudeOrigin(),
+		        geographic_local_.LongitudeOrigin(),
+		        geographic_local_.HeightOrigin()};
+	}
+	
+	/**
+	 * Given another lat lon h, calculate it's coordinates in the local ENU
+	 * coordinate system of origin
+	 * @param lat
+	 * @param lon
+	 * @param h
+	 * @return  X, Y, Z
+	 */
+	inline Eigen::Vector3d LocalXYZ(double lat, double lon, double h) const {
+		Eigen::Vector3d local;
+		geographic_local_.Forward(lat, lon, h, local[0], local[1], local[2]);
+		return local;
+	}
+	
+	/**
+	 * Given another X, Y, Z, calculate it's coordinates in the LatLon
+	 * coordinate system.
+	 * @param X
+	 * @param Y
+	 * @param Z
+	 * @return  lat, lon, height
+	 */
+	inline Eigen::Vector3d LatLonHeight(double x, double y, double z) const {
+		Eigen::Vector3d latlon;
+		// not our navigation is north east earth, but navigation in GeographicLib
+		// is east north sky
+		geographic_local_.Reverse(x, y, z, latlon[0], latlon[1], latlon[2]);
+		return latlon;
+	}
+	
+	inline double getDistance(double lat, double lon, double h) const {
+		Eigen::Vector3d local = LocalXYZ(lat, lon, h);
+		return local.norm();
+	}
+	
+	/**
+	 * Given another lat lon position, calculate the relative rotation its NED
+	 * system and the origin's NED system
+	 * @param lat
+	 * @param lon
+	 * @return
+	 */
+	inline Eigen::Matrix3d C_n_no(double lat, double lon) const {
+		return (Eigen::AngleAxisd(
+				(lat - geographic_local_.LatitudeOrigin()) * M_PI / 180.0,
+				Eigen::Vector3d::UnitY()) *
+		        Eigen::AngleAxisd(
+				        (geographic_local_.LongitudeOrigin() - lon) * M_PI / 180.0,
+				        Eigen::Vector3d::UnitX()))
+				.toRotationMatrix();
+	}
+	
+	/**
+	 * Given another lat lon position, calculate the relative rotation its NED
+	 * system and the origin's NED system
+	 * @param lat
+	 * @param lon
+	 * @return
+	 */
+	inline Eigen::Matrix3d C_no_n(double lat, double lon) const {
+		return (Eigen::AngleAxisd(
+				(lon - geographic_local_.LongitudeOrigin()) * M_PI / 180.0,
+				Eigen::Vector3d::UnitX()) *
+		        Eigen::AngleAxisd(
+				        (geographic_local_.LatitudeOrigin() - lat) * M_PI / 180.0,
+				        Eigen::Vector3d::UnitY()))
+				.toRotationMatrix();
+	}
+ 
+ protected:
+	GeographicLib::LocalCartesian geographic_local_;
+};
+
+inline double getDistance(const double lat1, const double lon1, const double h1,
+                          const double lat2, const double lon2, const double h2) {
+	LatLonCartesian lat_lon_cartersian(lat1, lon1, h1);
+	return lat_lon_cartersian.getDistance(lat2, lon2, h2);
+}
+
+}  // namespace jf
\ No newline at end of file

cloud_common/utility/track.h

+#pragma once
+#include <vector>
+#include "common/TypeDef.h"
+#include "utility/lat_lon_cartersian.hpp"
+
+namespace jf {
+
+
+class TrackPoint {
+public:
+    TrackPoint() {}
+    virtual ~TrackPoint() {}
+ public:
+	double lat;
+	double lng;
+	double height;
+	double yaw;
+	double pitch;
+	double roll;
+	
+	long loc_time;    // ms
+};
+typedef std::shared_ptr<TrackPoint> TrackPointPtr;
+
+class Track {
+public:
+	Track() {}
+	Track(const Track& track);
+
+	virtual ~Track() {
+	}
+	
+ public:
+	double start_time;
+	double end_time;
+	const double TRACK_SEG_DISTANCE = 3.0;
+	
+	std::vector<LocalCoordinate> coord_list;
+	std::vector<LocalCoordinate> utm_coord_list;
+	std::vector<TrackPoint> point_list;
+	LatLonCartesian cartesian;
+    std::vector<std::vector<LocalCoordinate> > seg_coord_lists;
+	
+	//bool init(const PpkFileReader& ppk, double& start_time, double& end_time);
+	bool init(const std::string& track_file);
+	bool generateCartesian();
+	void getSegCoordLists();
+
+	bool LocalXYZ2latlon(LocalCoordinate local_xyz, double& lat, double& lon, double& height);
+
+	bool LocalXYZ2UTM(StationInfo stationInfo);
+
+public:
+    Track&operator=(const jf::Track& track);
+
+private:
+	bool is_track_latlon;
+};
+} // namespace jf
+
+