//
// 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 <opencv2/opencv.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);

#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_