Measure/include/tool/point_tool.h

//
// Created by zx on 2021/5/11.
//

#ifndef ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_
#define ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <opencv2/opencv.hpp>
#include "static_tool.hpp"

//bool string2point(std::string str, pcl::PointXYZ &point);
//pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file);

void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string file);
void save_cloud_txt(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string file);

//double distance(cv::Point2f p1, cv::Point2f p2);
bool isRect(std::vector<cv::Point2f> &points);
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);

void rpy_to_rotation_matrix();
void rotation_matrix_to_rpy();
void reverse_test();

bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);

class pclTool {
public:
    static pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> ComplementSymmetryPoints(pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &in) {
        pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> out(new pcl::PointCloud<pcl::PointXYZ>);
        ComplementSymmetryPoints(in, out);
        return out;
    }

    static void ComplementSymmetryPoints(pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &in, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &out) {
        cv::Point2f centroid;
        float radius;
        std::vector<cv::Point2f> cv_in;
        for (auto &point:in->points) {
            cv_in.emplace_back(point.x, point.y);
        }
        cv::minEnclosingCircle(cv_in, centroid, radius);
        pcl::copyPointCloud(*in, *out);

        for (auto &point: in->points) {
            out->emplace_back(SymmetryPoint(point, centroid));
        }
    }

    static pcl::PointXYZ SymmetryPoint(pcl::PointXYZ &in, cv::Point2f &centroid) {
        pcl::PointXYZ sp;
        sp.x = 2 * centroid.x - in.x;
        sp.y = 2 * centroid.y - in.y;
        sp.z = in.z;
        return sp;
    }
};
#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_