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);

bool resetColor(cv::Mat &mat) {
    if (mat.empty()) {
        return false;
    }

    for (int len_x = 0; len_x < mat.cols; len_x++) {
        for (int len_y = 0; len_y < mat.rows; len_y++) {
            if (mat.at<cv::Vec3b>(len_y, len_x)[0] == 0 &&
                    mat.at<cv::Vec3b>(len_y, len_x)[1] == 255 &&
                    mat.at<cv::Vec3b>(len_y, len_x)[2] == 0 ) {
                mat.at<cv::Vec3b>(len_y, len_x)[0] = 0;
                mat.at<cv::Vec3b>(len_y, len_x)[1] = 0;
                mat.at<cv::Vec3b>(len_y, len_x)[2] = 0;
            }
        }
    }
    return true;
}
/**
 * @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) {
    //离群点过滤
    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);
    cv::Point2f car_body_rect_vertex[4];
    car_body_rect.points(car_body_rect_vertex);
    float minx = 1e8, maxx = -1e8;
    for (auto &point:car_body_rect_vertex) {
        minx = MIN(minx, point.x);
        maxx = MAX(maxx, point.x);
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr a(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr b(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr c(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr d(new pcl::PointCloud<pcl::PointXYZ>);
    car_body_rect.center.y = car_body_rect.center.y + 3.2;
    for (auto &point: clouds->points) {
        point.y = point.y + 3.2;
        if (point.x < car_body_rect.center.x && point.y > car_body_rect.center.y) {
            a->emplace_back(point);
        } else if (point.x > car_body_rect.center.x && point.y > car_body_rect.center.y) {
            b->emplace_back(point);
        } else if (point.x < car_body_rect.center.x && point.y < car_body_rect.center.y) {
            c->emplace_back(point);
        } else {
            d->emplace_back(point);
        }
    }

    // 左前
    for (auto &point: a->points) {
        int mat_x = (point.z + 0.1) * 161;
        int mat_y = (point.y - 3.2) * 161;
        int mat_depth = fabs(car_body_rect.center.x - point.x) * 100 + 0;
        mat_x = MAX(mat_x, 0);
        mat_x = MIN(mat_x, 159);
        mat_y = MAX(mat_y, 0);
        mat_y = MIN(mat_y, 479);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] = MAX(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1] = MIN(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[2] = mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] - mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1];
    }
//    WriteTxtCloud(ETC_PATH"Cloud2Mat/data/a.txt", a);
    // 右前
    for (auto &point: b->points) {
        int mat_x = (point.z + 0.1) * 161 + 160;
        int mat_y = (point.y - 3.2) * 161;
        int mat_depth = fabs(car_body_rect.center.x - point.x) * 100 + 0;
        mat_x = MAX(mat_x, 160);
        mat_x = MIN(mat_x, 319);
        mat_y = MAX(mat_y, 0);
        mat_y = MIN(mat_y, 479);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] = MAX(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1] = MIN(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[2] = mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] - mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1];
    }
//    WriteTxtCloud(ETC_PATH"Cloud2Mat/data/b.txt", b);
    // 左后
    for (auto &point: c->points) {
        int mat_x = (point.z + 0.1) * 161 + 320;
        int mat_y = (point.y) * 161;
        int mat_depth = fabs(car_body_rect.center.x - point.x) * 100 + 0;
        mat_x = MAX(mat_x, 320);
        mat_x = MIN(mat_x, 479);
        mat_y = MAX(mat_y, 0);
        mat_y = MIN(mat_y, 479);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] = MAX(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1] = MIN(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[2] = mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] - mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1];
    }
//    WriteTxtCloud(ETC_PATH"Cloud2Mat/data/c.txt", c);
    // 右后
    for (auto &point: d->points) {
        int mat_x = (point.z + 0.1) * 161 + 480;
        int mat_y = (point.y) * 161;
        int mat_depth = fabs(car_body_rect.center.x - point.x) * 100 + 0;
        mat_x = MAX(mat_x, 480);
        mat_x = MIN(mat_x, 639);
        mat_y = MAX(mat_y, 0);
        mat_y = MIN(mat_y, 479);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] = MAX(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1] = MIN(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[2] = mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] - mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1];
    }
//    WriteTxtCloud(ETC_PATH"Cloud2Mat/data/d.txt", d);
//
//    cv::imwrite(ETC_PATH"Cloud2Mat/data/mat.jpg", mat);
    resetColor(mat);
    return EXIT_SUCCESS;
}

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 = 1e8, maxx = -1e8;
    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 + 0;
        mat_x = MAX(mat_x, 0);
        mat_x = MIN(mat_x, 639);
        mat_y = MAX(mat_y, 240);
        mat_y = MIN(mat_y, 479);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] = MAX(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1] = MIN(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[2] = mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] - mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1];
    }

    // 裁出右侧点云
    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 + 0;
        mat_x = MAX(mat_x, 0);
        mat_x = MIN(mat_x, 639);
        mat_y = MAX(mat_y, 0);
        mat_y = MIN(mat_y, 239);
        mat_depth = MAX(mat_depth, 0);
        mat_depth = MIN(mat_depth, 255);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] = MAX(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1] = MIN(mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1], mat_depth);
        mat.at<cv::Vec3b>(479 - mat_y, mat_x)[2] = mat.at<cv::Vec3b>(479 - mat_y, mat_x)[0] - mat.at<cv::Vec3b>(479 - mat_y, mat_x)[1];
    }
    resetColor(mat);
    return EXIT_SUCCESS;
}

int main(int argc, char* argv[]) {
    ZX::InitGlog("Cloud2Mat", ETC_PATH"Cloud2Mat/Cloud2MatLog/");

    std::string prototxt_path = ETC_PATH"Cloud2Mat/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>);
        //创建多边形区域指针
        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"Cloud2Mat/data/" + std::to_string(m_region.region_id()));
        for (auto &file: file_list) {
            LOG(INFO) << file;
//            file = "/home/zx/Measure/Modules/Cloud2Mat/etc/data/3219/20230918-103314/failture.txt";
            if (file.substr(file.find_last_of('.') + 1) != "txt") {
//                remove(file.c_str());
                continue;
            }

            auto mat_file = file.substr(0, file.length() - 4) + ".jpg";
            auto four_mat_file = file.substr(0, file.length() - 4) + "_four.jpg";

            cv::Mat mat = cv::Mat(MatSmallSize, CV_8UC3, cv::Scalar(0, 255, 0));// 创建双通道黑色图像。
            cv::Mat four_mat = cv::Mat(MatSmallSize, CV_8UC3, cv::Scalar(0, 255, 0));// 创建双通道黑色图像。
            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>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr four_cloud_cut = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            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);
            pcl::copyPointCloud(*cloud_cut, *four_cloud_cut);

//            WriteTxtCloud(lll, cloud_cut);
            Points2Mat(cloud_cut, mat);
            cv::imwrite(mat_file, mat);
            usleep(1000 * 10);

            Points2MatSegmentWheels(four_cloud_cut, four_mat);
            cv::imwrite(four_mat_file, four_mat);
            usleep(1000 * 10);
//            exit(EXIT_FAILURE);
        }

    }

    return EXIT_SUCCESS;
}