Measure/Modules/Cloud2Mat/main.cpp

//
// Created by zx on 2023/9/21.
//
#include <dirent.h>
#include <vector>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include "defines.hpp"
#include "error_code/error_code.hpp"
#include "tool/proto_tool.h"
#include "velodyne_config.pb.h"
#include "tool/static_tool.hpp"
#include "tool/point3D_tool.h"

const cv::Size MatSmallSize(640, 480);

/**
 * @brief GetFiles: 获取文件夹内的所有文件名字
 * @param sdir
 * @param bsubdir: true 包含子目录下的文件
 * @return
 */
std::vector<std::string> GetFiles(const std::string& sdir = ".",
                                  bool bsubdir = true) {
    LOG(INFO) << sdir;
    DIR* dp;
    struct dirent* dirp;
    std::vector<std::string> filenames;
    if ((dp = opendir(sdir.c_str())) != NULL) {
        while ((dirp = readdir(dp)) != NULL) {
            if (strcmp(".", dirp->d_name) == 0 || strcmp("..", dirp->d_name) == 0)
                continue;
            if (dirp->d_type != DT_DIR)
                filenames.push_back(sdir + "/" + dirp->d_name);
            if (bsubdir && dirp->d_type == DT_DIR) {
                std::vector<std::string> names = GetFiles(sdir + "/" + dirp->d_name);
                filenames.insert(filenames.begin(), names.begin(), names.end());
            }
        }
    }
    closedir(dp);
    return filenames;
}

int Points2MatSegmentWheels(pcl::PointCloud<pcl::PointXYZ>::Ptr &clouds, cv::Mat &mat) {
    mat.setTo(0);
    cv::imwrite(ETC_PATH"/etc/data/mat.jpg", mat);
}

int Points2Mat(pcl::PointCloud<pcl::PointXYZ>::Ptr &clouds, cv::Mat &mat) {

    //离群点过滤
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(clouds);
    sor.setMeanK(50);            //K近邻搜索点个数
    sor.setStddevMulThresh(0.7); //标准差倍数
    sor.setNegative(false);      //保留未滤波点（内点）
    sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter
    // 获取点云外接矩形
    cv::RotatedRect car_body_rect;
    Point3D_tool::getMinRect(car_body_rect, cloud_filtered);

    // 初始化裁剪参数
    pcl::PointCloud<pcl::PointXYZ>::Ptr left_clouds(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr right_clouds(new pcl::PointCloud<pcl::PointXYZ>);
    cv::Point2f car_body_rect_vertex[4];
    car_body_rect.points(car_body_rect_vertex);
    float minx = 1000000, maxx = 0;
    for (auto &point:car_body_rect_vertex) {
        minx = MIN(minx, point.x);
        maxx = MAX(maxx, point.x);
    }

    // 裁出左侧点云
    pcl::PassThrough<pcl::PointXYZ> cut_pass;
    cut_pass.setFilterFieldName("x");
    cut_pass.setFilterLimits(minx, car_body_rect.center.x);
    cut_pass.setFilterLimitsNegative(false);
    cut_pass.setInputCloud(clouds);
    cut_pass.filter(*left_clouds);

    for (auto &point: left_clouds->points) {
        int mat_x = -(point.y - 3.2) * 100;
        int mat_y = (point.z + 2.4) * 100;
        int mat_depth = (car_body_rect.center.x - point.x) * 100;
        mat_x = MAX(mat_x, 0);
        mat_x = MIN(mat_x, 640);
        mat_y = MAX(mat_y, 241);
        mat_y = MIN(mat_y, 480);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<uchar>(480 - mat_y, mat_x) = MAX(mat.at<uchar>(mat_y, mat_x), mat_depth);
    }

    // 裁出右侧点云
    cut_pass.setFilterFieldName("x");
    cut_pass.setFilterLimits(car_body_rect.center.x, maxx);
    cut_pass.setFilterLimitsNegative(false);
    cut_pass.setInputCloud(clouds);
    cut_pass.filter(*right_clouds);

    for (auto &point: right_clouds->points) {
        int mat_x = -(point.y - 3.2) * 100;
        int mat_y = point.z * 100;
        int mat_depth = (point.x - car_body_rect.center.x) * 100;
        mat_x = MAX(mat_x, 0);
        mat_x = MIN(mat_x, 640);
        mat_y = MAX(mat_y, 0);
        mat_y = MIN(mat_y, 240);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<uchar>(480 - mat_y, mat_x) = MAX(mat.at<uchar>(mat_y, mat_x), mat_depth);
    }
    return EXIT_SUCCESS;
}

int main(int argc, char* argv[]) {
    if (argc == 1) {
        return EXIT_FAILURE;
    }

    ZX::InitGlog("Cloud2Mat", ETC_PATH"/etc/MCloud2MatLog/");

    std::string prototxt_path = ETC_PATH"/etc/velodyne_manager.prototxt";
    velodyne::velodyneManagerParams t_velo_params;
    if (!proto_tool::read_proto_param(prototxt_path, t_velo_params)) {
        LOG(ERROR) << "read_proto_param failed.";
        return EXIT_FAILURE;
    }

    for (auto &m_region: t_velo_params.region()) {
        // 裁剪块
        pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        //创建多边形区域指针
        for (auto &point: m_region.cut_box().cut_points()) {
            LOG(INFO) << point.DebugString();
            boundingbox_ptr->push_back(pcl::PointXYZ(point.x(), point.y(), point.z()));
        }

        std::vector<pcl::Vertices> m_polygons;                    //设置动态数组用于保存凸包顶点
        pcl::PointCloud<pcl::PointXYZ>::Ptr m_surface_hull = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::ConvexHull<pcl::PointXYZ> hull;                    //建立一个凸包对象
        hull.setInputCloud(boundingbox_ptr);                    //设置凸包的维度
        hull.reconstruct(*m_surface_hull, m_polygons); 			//计算凸包结果

        // 找到该区域目录下所有点云文件

        auto file_list = GetFiles(ETC_PATH"/etc/data/" + std::to_string(m_region.region_id()));
        for (auto &file: file_list) {
            LOG(INFO) << file;
            auto mat_file = file.substr(0, file.length() - 4) + ".jpg";
            if (file.substr(file.find_last_of('.') + 1) != "txt") {
                continue;
            } else {
                remove(mat_file.c_str());
            }

            cv::Mat mat = cv::Mat(MatSmallSize, CV_8U, cv::Scalar(0));// 创建单通道黑色图像。
            Points2MatSegmentWheels(cloud, mat);
            exit(EXIT_SUCCESS);
            cloud->clear();
            cloud_cut->clear();
            ReadTxtCloud(file, cloud);

            pcl::CropHull<pcl::PointXYZ> bb_filter;	 		//创建CropHull对象
            bb_filter.setInputCloud(cloud);	                //设置输入点云
            bb_filter.setDim(2);		                    //设置维度
            bb_filter.setHullIndices(m_polygons);	//输入封闭多边形的顶点
            bb_filter.setHullCloud(m_surface_hull);	//输入封闭多边形的形状
            bb_filter.filter(*cloud_cut);

            pcl::PassThrough<pcl::PointXYZ> cut_pass;
            cut_pass.setInputCloud(cloud_cut);
            cut_pass.setFilterFieldName("z");
            cut_pass.setFilterLimits(m_region.minz(), m_region.minz() + 2);
            cut_pass.setFilterLimitsNegative(false);
            cut_pass.filter(*cloud_cut);

//            WriteTxtCloud(lll, cloud_cut);
            Points2Mat(cloud_cut, mat);

            cv::imwrite(mat_file, mat);
        }

    }

    return EXIT_SUCCESS;
}