freespace本地注释源码

test_project/livox_free_space/CMakeLists.txt

+cmake_minimum_required(VERSION 3.10)
+project(livox_free_space)
+
+add_definitions(-std=c++17)
+
+
+find_package(PCL REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(Boost 1.65.1 REQUIRED system)
+find_package(Eigen3 REQUIRED)
+include_directories(${Eigen_INCLUDE_DIRS})
+include_directories(${PCL_INCLUDE_DIRS})
+include_directories(${catkin_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+set(PCL_PTH "pcl-1.8")
+
+include_directories(include)
+
+
+
+message("=> xavier off")
+link_directories(/usr/local/lib)
+link_directories(/usr/lib/x86_64-linux-gnu)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Ofast -Wfatal-errors -D_MWAITXINTRIN_H_INCLUDED")
+
+
+# ground_segmentation_node.cc原始代码 
+add_executable(livox_free_space ${PROJECT_SOURCE_DIR}/src/FreeSpace_node.cpp ${PROJECT_SOURCE_DIR}/src/FreeSpace.cpp )              
+
+
+set(LIBRARIES_TO_LINK  yaml-cpp -lm -pthread -lboost_system ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_LIBRARIES})
+
+# target_link_libraries(linefit_ground_segmentation_test_jf -lm -pthread -lboost_system)
+# target_link_libraries(linefit_ground_segmentation_test_jf ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES})
+# target_link_libraries(linefit_ground_segmentation_test_jf yaml-cpp)
+#target_link_libraries(linefit_ground_segmentation_test_jf -lpcl_filters yaml-cpp)
+
+target_link_libraries(livox_free_space ${LIBRARIES_TO_LINK})
+
+add_definitions(-O2 -pthread)

test_project/livox_free_space/include/FreeSpace.hpp

+#ifndef _FREESPACE_HPP
+#define _FREESPACE_HPP
+
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <Eigen/Core>
+#include <pcl/common/transforms.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/common/common.h>
+#include <Eigen/Dense>
+
+
+#include <vector>
+#include <math.h>
+
+using namespace std;
+
+#define SELF_CALI_FRAMES 20
+
+
+#define GND_IMG_NX 150
+#define GND_IMG_NY 400
+#define GND_IMG_DX 0.2
+#define GND_IMG_DY 0.2
+//#define GND_IMG_OFFX 40
+#define GND_IMG_OFFX 0
+#define GND_IMG_OFFY 40
+
+#define GND_IMG_NX1 24
+#define GND_IMG_NY1 20
+#define GND_IMG_DX1 4
+#define GND_IMG_DY1 4
+//#define GND_IMG_OFFX1 40
+#define GND_IMG_OFFX1 0
+#define GND_IMG_OFFY1 40
+
+#define DN_SAMPLE_IMG_NX 500
+#define DN_SAMPLE_IMG_NY 200
+#define DN_SAMPLE_IMG_NZ 100
+#define DN_SAMPLE_IMG_DX 0.4
+#define DN_SAMPLE_IMG_DY 0.4
+#define DN_SAMPLE_IMG_DZ 0.2
+//#define DN_SAMPLE_IMG_OFFX 50 // -50～150
+#define DN_SAMPLE_IMG_OFFX 0 // m1 不需要偏移，因为所有点都是>0的  结合代码理解
+#define DN_SAMPLE_IMG_OFFY 40 // -40～40
+#define DN_SAMPLE_IMG_OFFZ 10// -10～10
+
+#define DENOISE_IMG_NX 200 
+#define DENOISE_IMG_NY 80 
+#define DENOISE_IMG_NZ 100
+#define DENOISE_IMG_DX 1 
+#define DENOISE_IMG_DY 1 
+#define DENOISE_IMG_DZ 0.2
+
+#define FREE_PI 3.14159265
+
+
+
+class LivoxFreeSpace
+{
+    public:
+    LivoxFreeSpace();
+    // functions
+    int GenerateFreeSpace(float* fPoints1, int pointNum, std::vector<float> & free_space);
+    void FreeSpace(float* fPoints, int n, float* free_space, int free_space_n);
+    void FreeSpaceFilter(float* free_space_small, int n , std::vector<float> & free_space);
+    int GroundSegment(int* pLabel,float *fPoints,int pointNum,float fSearchRadius);
+
+    unsigned char *pVImg;
+
+    ~LivoxFreeSpace();
+
+};
+
+
+#endif

test_project/livox_free_space/src/FreeSpace_node.cpp

+
+// #include "sensor_msgs/PointCloud2.h"
+// #include "geometry_msgs/Point.h"
+// #include "geometry_msgs/Point32.h"
+// #include "geometry_msgs/Polygon.h"
+// #include "geometry_msgs/PolygonStamped.h"
+// #include "nav_msgs/GridCells.h"
+#include "pcl/conversions.h"
+//#include "pcl_conversions/pcl_conversions.h"
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/common.h>
+#include <time.h>
+#include <memory.h>
+#include <fstream>
+#include "pcl/PCLPointCloud2.h"
+#include "FreeSpace.hpp"
+
+
+
+
+
+#define POINTS_TO_FILE  // 定义宏
+LivoxFreeSpace livox_free_space;
+
+
+void writeGroundPointToFile(pcl::PointCloud<pcl::PointXYZ>& free_space, const std::string& filename)
+{   
+    int index = 0;
+
+    std::ofstream file(filename);
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+    std::cout << "File created" << std::endl;
+    file << "cluster " << index << std::endl;
+   
+
+
+    // 遍历点云中的每个点
+    for (const auto& point : free_space.points)
+    {
+      // 以指定格式将点的坐标写入到文件中
+      file << point.x << ", " << point.y << ", " << point.z << endl;
+    }
+    file.close();
+}
+
+void GenerateFreeSpace(pcl::PointCloud<pcl::PointXYZ> & pc,const std::string& filename,std::string& number_str)
+{
+    clock_t t0,t1,t2;
+    t0 = clock();
+
+    int dnum = pc.points.size();
+    std::cout << "Point cloud size: " << dnum << std::endl;
+
+    float *data=(float*)calloc(dnum*3,sizeof(float));
+    std::vector<float> free_space;
+    for (int p=0; p<dnum; ++p)
+    {
+        data[p*3+0] = pc.points[p].x;
+        data[p*3+1] = pc.points[p].y;
+        data[p*3+2] = pc.points[p].z;
+    }       
+    livox_free_space.GenerateFreeSpace(data, dnum, free_space);
+    t1 = clock();
+
+
+    pcl::PointCloud<pcl::PointXYZ> fs;
+    fs.clear();
+    for (int i = 0; i < free_space.size(); i+=3)
+    {   
+        pcl::PointXYZ p;
+        p.x = free_space[i]; p.y = free_space[i + 1]; p.z = 0;
+        fs.points.push_back(p);
+    }
+    #ifdef POINTS_TO_FILE
+    writeGroundPointToFile(fs,filename);
+    #endif
+
+    std::vector<float>().swap(free_space);
+    t2 = clock();
+
+    printf("---------------------------------------------\n\n");
+    free(data);
+
+}
+
+int main(int argc, char **argv)
+{
+
+    typedef pcl::PointXYZ PointT;
+    pcl::PointCloud<pcl::PointXYZ> pc;
+    float height_offset = 1.3; //这个偏移量是lidar和地面距离，为了将地面点z坐标在0附近
+    std::cout << "height offset:\t\t\t" << height_offset << std::endl;
+
+    if(strcmp(argv[1], "pcd") == 0){
+        std::cout<< "read pcd point cloud " << argv[2] << std::endl;
+        std::string file_name = std::string(argv[2]);
+
+        if (pcl::io::loadPCDFile<PointT> (file_name, pc) == -1){
+        // load the file
+        PCL_ERROR ("Couldn't read PCD file \n");
+        return (-1);
+        }
+    }
+    
+    std::string tmp(argv[2]);
+    std::cout << "tmp: " << tmp << "\n";
+    size_t start_pos = tmp.find_last_of("/") + 1;
+    //size_t end_pos = tmp.find(".", start_pos);
+    size_t end_pos = tmp.find_last_of(".");
+    std::string number_str = tmp.substr(start_pos, end_pos - start_pos);
+    std::cout << "number_str: " << number_str << "\n";
+    std::string file_txt = "../result/clusters_" + number_str + ".txt" ;
+
+    printf("init pcd size:%d \n",pc.size());
+    std::vector<int> indices;
+    pcl::removeNaNFromPointCloud(pc, pc, indices);
+    printf("remove NaN  pcd size:%d \n",pc.size());
+
+    for (int i = 0; i < pc.points.size(); i++){
+        pc.points[i].z = pc.points[i].z + height_offset;
+    }
+
+    GenerateFreeSpace(pc, file_txt, number_str);
+    printf("run finished .....");
+    return 0;
+}