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


//计算cloud2到cloud1的变换
Eigen::Matrix4f ndt_match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2);
#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_