main.cpp

#include <iostream>
#include <string>
#include <vector>
#include <fstream>

// 文件系统支持 (C++14/17)
#if __cplusplus >= 201703L
#include <filesystem>
namespace fs = std::filesystem;
#else
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#endif

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>

#include "./src/lpir_gseg.h" // 确保你的头文件路径正确

// 从 Patchwork++ 借鉴的 .bin 读取函数，适配 PCL 点云结构
void read_bin_to_pcl(const std::string& bin_path, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud) {
    FILE* file = fopen(bin_path.c_str(), "rb");
    if (!file) {
        std::cerr << "\033[1;31mError: failed to load " << bin_path << "\033[0m" << std::endl;
        return;
    }

    std::vector<float> buffer(1000000);
    size_t num_points = fread(reinterpret_cast<char*>(buffer.data()), sizeof(float), buffer.size(), file) / 4;

    cloud->reserve(num_points);
    for (size_t i = 0; i < num_points; i++) {
        pcl::PointXYZI pt;
        pt.x = buffer[i * 4];
        pt.y = buffer[i * 4 + 1];
        pt.z = buffer[i * 4 + 2];
        pt.intensity = buffer[i * 4 + 3];
        cloud->push_back(pt);
    }
    fclose(file);
}

int main(int argc, char** argv) {
    std::string input_cloud_filepath;

    // 支持通过命令行传参，如果不传参则尝试读取默认路径
    if (argc < 2) {
        // 假设你把 .bin 文件放在了 build 目录同级的 data 文件夹中
        input_cloud_filepath = "../data/000000.bin"; 
        std::cout << "\033[1;33mNo point cloud file specified; defaulting to: " << input_cloud_filepath << "\033[0m" << std::endl;
    } else {
        input_cloud_filepath = argv[1];
        std::cout << "\033[1;32mLoading point cloud from: " << input_cloud_filepath << "\033[0m" << std::endl;
    }

    if (!fs::exists(input_cloud_filepath)) {
        std::cerr << "\033[1;31mERROR: File does not exist. Please check the path: " << input_cloud_filepath << "\033[0m" << std::endl;
        return -1;
    }

    // 1. 初始化点云对象
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr nonground_cloud(new pcl::PointCloud<pcl::PointXYZI>);

    // 2. 读取二进制数据
    read_bin_to_pcl(input_cloud_filepath, cloud_in);
    std::cout << "Successfully loaded " << cloud_in->size() << " data points." << std::endl;

    // 3. 配置 LPIR-GSeg 参数 (这里使用论文中的建议值)
    lpir::LPIRParams params;
    params.k_xz = -0.0142; 
    params.k_yz = -0.0181;
    params.sensor_height = 1.723; // SemanticKITTI/Patchwork++ 默认的 Velodyne 高度
    params.h_thr = 0.2;
    
    lpir::LPIRGSeg segmenter(params);

    // 4. 执行地面分割算法并计时
    std::cout << "Starting LPIR-GSeg segmentation..." << std::endl;
    pcl::console::TicToc time;
    time.tic();
    
    segmenter.segmentGround(*cloud_in, *ground_cloud, *nonground_cloud);
    
    double total_time = time.toc();
    std::cout << "\n\033[1;32m=== Execution Time Analysis ===\033[0m" << std::endl;
    std::cout << "1. Preprocessing Time : " << segmenter.getPreProcessTime() << " ms" << std::endl;
    std::cout << "2. Coarse Seg Time    : " << segmenter.getCoarseTime() << " ms" << std::endl;
    std::cout << "3. Fine Seg Time      : " << segmenter.getFineTime() << " ms" << std::endl;
    std::cout << "---------------------------------" << std::endl;
    std::cout << "   Total Time Taken   : " << total_time << " ms\n" << std::endl;

    // 5. 3D 可视化验证
    std::cout << "Opening PCL Visualizer. Close the window to exit." << std::endl;
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("LPIR-GSeg Result"));
    viewer->setBackgroundColor(0.1, 0.1, 0.1); // 深灰色背景

    // 绿色渲染地面点
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> ground_color(ground_cloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZI>(ground_cloud, ground_color, "ground");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "ground");

    // 红色渲染非地面点
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> nonground_color(nonground_cloud, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZI>(nonground_cloud, nonground_color, "nonground");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "nonground");

    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();

    // 保持窗口打开
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
    }

    return 0;
}