include/tool/point3D_tool.hpp

#pragma once

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <vector>
#include <opencv4/opencv2/opencv.hpp>
#include "tool/log.hpp"

class Point3D_tool
{
public:
    static bool WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
        DLOG(INFO) << "begain push point data to " << file;
        std::fstream fileTxtCloud;
        fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
        for (auto &point: *cloud) {
            fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
        }
        fileTxtCloud.close();
        DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
        return true;
    }

    static bool WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
        DLOG(INFO) << "begain push point data to " << file;
        std::fstream fileTxtCloud;
        fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
        for (auto &point: *cloud) {
            fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
        }
        fileTxtCloud.close();
        DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
        return true;
    }

    static bool ReadTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
        DLOG(INFO) << "Load point data form file: " << file;
        std::ifstream fin(file.c_str());
        if (fin.bad()) {
            LOG(WARNING) << "File " << file << " don't exist.";
            return false;
        }
        const int line_length = 64;
        char str[line_length] = {0};
        while (fin.getline(str, line_length)) {
            pcl::PointXYZ point{};
            if (string2PointXYZ(str, point)) {
                cloud->push_back(point);
            }
        }
        DLOG(INFO) << "Load point complete, get point number: " << cloud->size();
        return true;
    }

	template <typename PclPoint>
	static int getMinRect(cv::RotatedRect &rect, pcl::shared_ptr<pcl::PointCloud<PclPoint>> &clouds) {
        if (clouds->points.size() < 4) {
            DLOG(ERROR) << "points to little.";
            return false;
        }
        std::vector<cv::Point2f> cv_clouds;
        for (auto &point: clouds->points) {
            cv_clouds.emplace_back(point.x, point.y);
        }
        rect = cv::minAreaRect(cv_clouds);
        return true;
    }

	// template <typename PclPoint>
	static int getRectCloud(cv::RotatedRect &rect, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &points, float z_value = 0, int numbers = 100) {
        cv::Point2f cv_points[4];
        rect.points(cv_points);
        // PclPoint point;
        pcl::PointXYZ point;
        // 左侧点
        double step_x = (cv_points[1].x - cv_points[0].x) / numbers;
        double step_y = (cv_points[1].y - cv_points[0].y) / numbers;
        for (int i = 0; i < numbers; i++) {
            point.x = cv_points[0].x + i * step_x;
            point.y = cv_points[0].y + i * step_y;
            point.z = z_value;
            points->points.emplace_back(point);
        }

        // 上侧点
        step_x = (cv_points[2].x - cv_points[1].x) / numbers;
        step_y = (cv_points[2].y - cv_points[1].y) / numbers;
        for (int i = 0; i < numbers; i++) {
            point.x = cv_points[1].x + i * step_x;
            point.y = cv_points[1].y + i * step_y;
            point.z = z_value;
            points->points.emplace_back(point);
        }

        // 右侧点
        step_x = (cv_points[3].x - cv_points[2].x) / numbers;
        step_y = (cv_points[3].y - cv_points[2].y) / numbers;
        for (int i = 0; i < numbers; i++) {
            point.x = cv_points[2].x + i * step_x;
            point.y = cv_points[2].y + i * step_y;
            point.z = z_value;
            points->points.emplace_back(point);
        }


        // 下侧点
        step_x = (cv_points[0].x - cv_points[3].x) / numbers;
        step_y = (cv_points[0].y - cv_points[3].y) / numbers;
        for (int i = 0; i < numbers; i++) {
            point.x = cv_points[3].x + i * step_x;
            point.y = cv_points[3].y + i * step_y;
            point.z = z_value;
            points->points.emplace_back(point);
        }

        return points->size();
    }

	template <typename PclPoint>
	static int getRectCloud(pcl::shared_ptr<pcl::PointCloud<PclPoint>> &clouds, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &points, float z_value = 0, int numbers = 100) {
        cv::RotatedRect rect = getMinRect(clouds);
        getRectCloud(rect, points, z_value, numbers);
        return points->size();
    }

	static void getRectWidthlength(cv::RotatedRect &rect, double &width, double &length) {
        cv::Point2f points[4];
        rect.points(points);

        double a = sqrt((points[1].x - points[0].x) * (points[1].x - points[0].x) + (points[1].y - points[0].y) * (points[1].y - points[0].y));
        double b = sqrt((points[2].x - points[1].x) * (points[2].x - points[1].x) + (points[2].y - points[1].y) * (points[2].y - points[1].y));

        width = MIN(a, b);
        length = MAX(a, b);
    }

private:
    static bool string2PointXYZ(const std::string &str, pcl::PointXYZ &point) {
        std::istringstream iss;
        iss.str(str);
        float value;
        float data[3];
        for (int i = 0; i < 3; ++i) {
            if (iss >> value) {
                data[i] = value;
            } else {
                return false;
            }
        }
        point.x = data[0];
        point.y = data[1];
        point.z = data[2];
        return true;
    }
};