#ifndef __FUSEWIREDETECTOR_HH__
#define __FUSEWIREDETECTOR_HH__

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>

#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <ceres/ceres.h>
#include <cmath>
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
#include <chrono>
#include <random>

#include <opencv2/opencv.hpp>

using CppAD::AD;
class FG_eval
{
public:
	FG_eval(std::vector<double> n) : norms(n)
	{
		// std::cout<<"input norm size: "<<norms.size()<<std::endl;
	}

	typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
	// minimize sum of (x[0] * px + x[1] * py + x[2]*pz)
	// subject to x[0]^2 + x[1]^2 + x[2]^2 = 1
	void operator()(ADvector &fg, const ADvector &x)
	{
		assert(fg.size() == 2);
		assert(x.size() == 3);
		assert(norms.size() > 0);

		// Fortran style indexing
		AD<double> var_x = x[0];
		AD<double> var_y = x[1];
		AD<double> var_z = x[2];
		AD<double> filter = 0;
		// f(x)
		fg[0] = 0;
		for (int i = 0; i < norms.size(); i += 3)
		{
			AD<double> value = CppAD::pow(var_x * norms[i] + var_y * norms[i + 1] + var_z * norms[i + 2], 2);
			filter = 1 - 1 / (1 + CppAD::exp(-500 * value + 200));
			// filter = 1;
			fg[0] += value * filter;
			// std::cout<<"fg: "<<i<<" "<<fg[0]<<std::endl;
		}

		// g(x)
		fg[1] = CppAD::pow(var_x, 2) + CppAD::pow(var_y, 2) + CppAD::pow(var_z, 2);
		// std::cout<<"fg: "<<fg[0]<<" "<<fg[1]<<std::endl;
		return;
	}

private:
	std::vector<double> norms;
};

class FG_eval_plane
{
public:
	FG_eval_plane(std::vector<double> ps, std::vector<double> p) : points(ps), p1p2(p)
	{
		std::cout << "input point size: " << points.size() << std::endl;
		std::cout << "input p1 p2: " << p1p2.size() << std::endl;
	}

	typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
	// minimize sum of (x[0] * px + x[1] * py + x[2]*pz + x[3])^2 / (x[0]^2 + x[1]^2 + x[2]^2)
	// subject to x[0]^2 + x[1]^2 + x[2]^2 + x[3]^2 > 0
	//			x[0] * px1 + x[1] * py1 + x[2]*pz1 + x[3] = 0
	//			x[0] * px2 + x[1] * py2 + x[2]*pz2 + x[3] = 0
	void operator()(ADvector &fg, const ADvector &x)
	{
		assert(fg.size() == 4);
		assert(x.size() == 4);
		assert(points.size() > 0);
		assert(p1p2.size() == 6);

		// Fortran style indexing
		AD<double> var_a = x[0];
		AD<double> var_b = x[1];
		AD<double> var_c = x[2];
		AD<double> var_d = x[3];
		AD<double> filter = 0;
		// f(x)
		fg[0] = 0;
		for (int i = 0; i < points.size(); i += 3)
		{
			AD<double> value = CppAD::pow(var_a * points[i] + var_b * points[i + 1] + var_c * points[i + 2] + var_d, 2) / (var_a * var_a + var_b * var_b + var_c * var_c);
			// filter = 1 - 1 / (1 + CppAD::exp(-500 * value + 200));
			filter = 1 - 1 / (1 + CppAD::exp(-500 * value + 200));
			fg[0] += value * filter;
			// std::cout<<"fg: "<<i<<" "<<fg[0]<<std::endl;
		}

		// g(x)
		fg[1] = CppAD::pow(var_a, 2) + CppAD::pow(var_b, 2) + CppAD::pow(var_c, 2) + CppAD::pow(var_d, 2);
		fg[2] = var_a * p1p2[0] + var_b * p1p2[1] + var_c * p1p2[2] + var_d;
		fg[3] = var_a * p1p2[3] + var_b * p1p2[4] + var_c * p1p2[5] + var_d;
		// std::cout<<"fg: "<<fg[0]<<" "<<fg[1]<<std::endl;
		return;
	}

private:
	std::vector<double> points;
	std::vector<double> p1p2;
};

class FG_eval_tube
{
public:
	FG_eval_tube(std::vector<double> ps, std::vector<double> p) : points(ps), p1p2(p)
	{
		//std::cout<<"input point size: "<<points.size()<<std::endl;
		//std::cout<<"input p1 p2: "<<p1p2.size()<<std::endl;
	}

	typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
	// minimize sum of (x[0] * px + x[1] * py + x[2]*pz + x[3])^2 / (x[0]^2 + x[1]^2 + x[2]^2)
	// subject to x[0]^2 + x[1]^2 + x[2]^2 + x[3]^2 > 0
	//			x[0] * px1 + x[1] * py1 + x[2]*pz1 + x[3] = 0
	//			x[0] * px2 + x[1] * py2 + x[2]*pz2 + x[3] = 0
	void operator()(ADvector &fg, const ADvector &x)
	{
		assert(fg.size() == 4);
		assert(x.size() == 4);
		assert(points.size() > 0);
		assert(p1p2.size() == 6);

		// Fortran style indexing
		AD<double> var_a = x[0];
		AD<double> var_b = x[1];
		AD<double> var_c = x[2];
		AD<double> var_d = x[3];
		AD<double> filter = 0;
		// f(x)
		fg[0] = 0;
		for (int i = 0; i < points.size(); i += 3)
		{
			AD<double> value = CppAD::pow(var_a * points[i] + var_b * points[i + 1] + var_c * points[i + 2] + var_d, 2) / (var_a * var_a + var_b * var_b + var_c * var_c);
			// filter = 1 - 1 / (1 + CppAD::exp(-500 * value + 200));
			filter = 1 - 1 / (1 + CppAD::exp(-500 * value + 200));
			fg[0] += value * filter;
			// std::cout<<"fg: "<<i<<" "<<fg[0]<<std::endl;
		}

		// g(x)
		fg[1] = CppAD::pow(var_a, 2) + CppAD::pow(var_b, 2) + CppAD::pow(var_c, 2) + CppAD::pow(var_d, 2);
		fg[2] = var_a * p1p2[0] + var_b * p1p2[1] + var_c * p1p2[2] + var_d;
		fg[3] = var_a * p1p2[3] + var_b * p1p2[4] + var_c * p1p2[5] + var_d;
		// std::cout<<"fg: "<<fg[0]<<" "<<fg[1]<<std::endl;
		return;
	}

private:
	std::vector<double> points;
	std::vector<double> p1p2;
};

class FuseWireDetector
{

public:
	//构建代价函数结构体，abc为待优化参数，residual为残差。
	struct VECTOR_FINDING_COST
	{
		VECTOR_FINDING_COST(std::vector<Eigen::Vector3d> points) : _points(points) {}
		template <typename T>
		//使用仿函数重载
		bool operator()(const T *const xyz, T *residual) const
		{
			// normalization
			for (int i = 0; i < _points.size(); i++)
			{
				T euclidean = ceres::sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1] + xyz[2] * xyz[2]) + T(0.00001);
				T px = xyz[0] / euclidean;
				T py = xyz[1] / euclidean;
				T pz = xyz[2] / euclidean;
				residual[i] = T(_points[i].x()) * px + T(_points[i].y()) * py + T(_points[i].z()) * pz;
				std::cout << "print: " << i << " " << px << " " << py << " " << pz << " " << residual[i] << std::endl;
			}
			return true;
		}
		const std::vector<Eigen::Vector3d> _points;
	};

	FuseWireDetector()
	{
	}
	~FuseWireDetector()
	{
	}

	void my_process()
	{
		// // --------------- basic cluster and voxel grid test ---------------
		// for (size_t i = 0; i < 44; i++)
		// {
		// 	std::string filename = "data1_" + boost::to_string(i);
		// 	GetTotalPoints(("data/"+filename+".txt").c_str());
		// 	cloud_clustering(filename);
		// }
		// std::cout<<"聚类结束"<<std::endl;
		// GetTotalPoints("points1.txt");
		// cloud_clustering("points1");

		// get_total_points_from_txt("points1Fuse.txt", ' ');
		// voxelGrid(500);

		// --------------- rgb cloud outlier removal, voxel filter, etc. ---------------
		// read
		// get_total_points_from_txt("points1FuseRGB.txt", ' ', 6);
		get_total_points_from_txt("clouds/newCloud4.txt", ' ', 6);
		// scalling
		for (int i = 0; i < m_origin_cloud_rgb.size(); i++)
		{
			m_origin_cloud_rgb[i].x *= 0.001;
			m_origin_cloud_rgb[i].y *= 0.001;
			m_origin_cloud_rgb[i].z *= 0.001;
		}
		// outlier removal
		outlierRemoval(m_origin_cloud_rgb, m_origin_cloud_rgb);
		// voxel filter 1cm
		voxelFilterRGB(0.01, m_origin_cloud_rgb);
		// if (normalCompute(m_origin_cloud_rgb))
		normalCompute(m_origin_cloud_rgb);
		// find z axis and generate img
		if (findViewingDirection(m_origin_cloud_rgb))
		{
			// find lines according to img, rotate cloud to align tube direction
			findLines(50);

			//normalClustering(0.05, 0.2);

			// find best viewing direction vector
			// pcl::PointCloud<pcl::PointXYZ> norm;
			// // pcl::copyPointCloud(m_norm, norm);// 无法使用
			// for (int i = 0; i < m_norm.size(); i++)
			// {
			// 	norm.push_back(pcl::PointXYZ(m_norm[i].normal_x, m_norm[i].normal_y, m_norm[i].normal_z));
			// }
			// bool result = false;
			// pcl::PointXYZ origin(0, 0, 0), target(0, 0, 0);
			// std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
			// result |= findVecWithCPPAD(norm, target);
			// if (result)
			// 	vec_viewing_direction = target;
			// std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
			// std::cout << " optimization seconds: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() / 1000.0 << std::endl;
			// save_cloud_txt("norm.txt", norm.makeShared());

			//findPole(m_origin_cloud_rgb);

			// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
			// viewer.setBackgroundColor(0.0, 0.0, 0.0);
			// viewer.addPointCloud<pcl::PointXYZRGB>(m_origin_cloud_rgb.makeShared());
			// while (!viewer.wasStopped())
			// {
			// 	usleep(15 * 1000);
			// 	viewer.spinOnce();
			// }

			// findTube(m_rest_cloud);
		}

		// // --------------- ceres debugging --------------- 
		// get_total_points_from_txt("norm.txt", ' ');
		// findVecWithCeres(m_origin_cloud);
		// pcl::PointXYZ p;
		// findVecWithCPPAD(m_origin_cloud, p);

		// // --------------- point cloud read and display ---------------
		// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		// pcl::io::loadPCDFile("tran1.pcd", *cloud);
		// //可视化
		// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
		// viewer.setBackgroundColor(0.0, 0.0, 0.0);
		// viewer.addPointCloud(cloud);
		// // viewer.addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(cloud.makeShared(), cloud_normals);
		// while (!viewer.wasStopped())
		// {
		// 	viewer.spinOnce();
		// }

		// cloud_clustering_rgb("origin");
		// --------------- point cloud split and sample creation ---------------
		// voxelGridRGB(500, 1, 0.001);
		// voxelGridRGB(500, 0.02, 0.001);
		// voxelGridRGB(500, 0.01, 0.001);
		// voxelGridRGB(500, 0.005, 0.001);
		// voxelGridRGB(1000, 1, 0.001);
		// voxelGridRGB(1000, 0.02, 0.001);
		// voxelGridRGB(1000, 0.01, 0.001);
		// voxelGridRGB(1000, 0.005, 0.001);
	}

	void dealWithTop(std::string filename){
		// read cloud
		std::ifstream in(filename);
		std::string linestr;
		pcl::PointCloud<pcl::PointXYZ> cloud;
		while (getline(in, linestr))
		{
			std::vector<std::string> strvec;
			std::string s;
			std::stringstream ss(linestr);
			while (getline(ss, s, ' '))
			{
				strvec.push_back(s);
			}
			if (strvec.size() != 3)
			{
				std::cout << "unsupported data type" << std::endl;
				return;
			}
			pcl::PointXYZ p;
			p.x = stod(strvec[0]);
			p.y = stod(strvec[1]);
			p.z = stod(strvec[2]);
			cloud.push_back(p);
			std::cout << cloud.size() << std::endl;	
		}
		in.close();

		// eulidean clustering
		pcl::PointCloud<pcl::PointXYZ> cloud_needed;
		// pcl::PointCloud<pcl::PointXYZ> cloud_remove;
		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
		kdtree->setInputCloud(cloud.makeShared());

		pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
		// 2cm (small values may cause objects to be divided
		// in several clusters, whereas big values may join objects in a same cluster).
		clustering.setClusterTolerance(20);
		clustering.setMinClusterSize(15);
		clustering.setMaxClusterSize(10000);
		clustering.setSearchMethod(kdtree);
		clustering.setInputCloud(cloud.makeShared());
		std::vector<pcl::PointIndices> clusters;
		clustering.extract(clusters);

		// For every cluster...
		pcl::PointCloud<pcl::PointXYZ> centers;
		pcl::PointXYZ avg_center;
		std::vector<pcl::PointCloud<pcl::PointXYZ>> all_cluster_points;
		int currentClusterNum = 1;
		for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
		{
			pcl::PointXYZ center_point;
			pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
			for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			{
				cluster->points.push_back(cloud[*point]);
				center_point.x += cloud[*point].x;
				center_point.y += cloud[*point].y;
				center_point.z += cloud[*point].z;
			}
			cluster->width = cluster->points.size();
			cluster->height = 1;
			cluster->is_dense = true;
			// find center
			center_point.x /= cluster->size();
			center_point.y /= cluster->size();
			center_point.z /= cluster->size();
			std::cout<<"center: "<<center_point<<std::endl;
			avg_center.x += center_point.x;
			avg_center.y += center_point.y;
			avg_center.z += center_point.z;
			centers.push_back(center_point);
			all_cluster_points.push_back(*cluster);
			
			// // 保存聚类点云，检查用
			// std::string fileName = "tube_cluster" + boost::to_string(currentClusterNum) + ".txt";
			// save_cloud_txt(fileName, cluster);
			// std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
			currentClusterNum++;
		}
		if(centers.size() <= 0){
			std::cout<<"error: with no center point."<<std::endl;
			return;
		}
		avg_center.x /= centers.size();
		avg_center.y /= centers.size();
		avg_center.z /= centers.size();
		// find average distance
		double total_dist = 0;
		std::vector<double> center_dists;
		for (size_t i = 0; i < centers.size(); i++)
		{
			double dist = sqrt(pow(avg_center.x - centers[i].x, 2) + pow(avg_center.y - centers[i].y, 2) + pow(avg_center.z - centers[i].z, 2));
			center_dists.push_back(dist);
			total_dist += dist;
			std::cout<<"calc dist: "<< dist <<std::endl;
		}
		total_dist /= centers.size();
		for (size_t i = 0; i < centers.size(); i++)
		{
			std::cout<<"center dist, total: "<< center_dists[i]<<" "<<total_dist<<std::endl;
			if(center_dists[i] < total_dist){
				cloud_needed.insert(cloud_needed.end(), all_cluster_points[i].begin(), all_cluster_points[i].end());
			}
		}	
		// save_cloud_txt("cloud_needed.txt", cloud_needed.makeShared());
		std::cout << "clustering finished." << std::endl;
	}

	void wholeProcess(std::string filename)
	{
		int index = filename.find_last_of('/');
		if(index >= filename.size()-2) return;
		std::string name = filename.substr(index+1, filename.size()-index-1);
		// read cloud
		std::cout << "******* read file " << filename <<" *******" << std::endl;
		std::ifstream in(filename);
		std::string linestr;
		pcl::PointCloud<pcl::PointXYZ> cloud;
		while (getline(in, linestr))
		{
			std::vector<std::string> strvec;
			std::string s;
			std::stringstream ss(linestr);
			while (getline(ss, s, ' '))
			{
				// std::cout<<"string:"<<s<<"..."<<std::endl;
				if(s != "")
					strvec.push_back(s);
			}
			if (strvec.size() != 3)
			{
				std::cout << "unsupported data type" << std::endl;
				return;
			}
			pcl::PointXYZ p;
			p.x = stod(strvec[0]) /1000.0;
			p.y = stod(strvec[1]) /1000.0;
			p.z = stod(strvec[2]) /1000.0;
			cloud.push_back(p);
		}
		in.close();
		// std::cout << cloud.size() << std::endl;	

		std::cout << "******* voxel filter *******" << std::endl;
		double leafsize = 0.02;
		// voxel grid filter
		pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
		approximate_voxel_filter.setLeafSize(leafsize, leafsize, leafsize);
		approximate_voxel_filter.setInputCloud(cloud.makeShared());
		approximate_voxel_filter.filter(cloud);

		std::cout << "******* viewing direction finding process *******" << std::endl;
		pcl::PointXYZ min_p, max_p, avg_slice1, avg_slice2;
		pcl::getMinMax3D(cloud, min_p, max_p);
		double offset = 0.1, thickness = 0.05;
		pcl::PointCloud<pcl::PointXYZ> slice1, slice2;
		for (size_t i = 0; i < cloud.size(); i++)
		{
			if (cloud[i].z >= min_p.z && cloud[i].z <= min_p.z + thickness)
			{
				slice1.push_back(cloud[i]);
				avg_slice1.x += cloud[i].x;
				avg_slice1.y += cloud[i].y;
				avg_slice1.z += cloud[i].z;
			}
			else if (cloud[i].z >= min_p.z + offset && cloud[i].z <= min_p.z + offset + thickness)
			{
				slice2.push_back(cloud[i]);
				avg_slice2.x += cloud[i].x;
				avg_slice2.y += cloud[i].y;
				avg_slice2.z += cloud[i].z;
			}
		}
		if (slice1.size() <= 0 || slice2.size() <= 0)
		{
			std::cout << "cannot find slices" << std::endl;
			return;
		}
		avg_slice1.x /= slice1.size();
		avg_slice1.y /= slice1.size();
		avg_slice1.z /= slice1.size();
		avg_slice2.x /= slice2.size();
		avg_slice2.y /= slice2.size();
		avg_slice2.z /= slice2.size();
		vec_viewing_direction = pcl::PointXYZ(avg_slice2.x - avg_slice1.x, avg_slice2.y - avg_slice1.y, avg_slice2.z - avg_slice1.z);
		pole_pos = pcl::PointXYZ((avg_slice2.x + avg_slice1.x) / 2.0, (avg_slice2.y + avg_slice1.y) / 2.0, (avg_slice2.z + avg_slice1.z) / 2.0);

		// z 轴校正夹角超过40度， 用另一个算法尝试找电线杆
		if (fabs(atan2(sqrt(pow(vec_viewing_direction.x, 2) + pow(vec_viewing_direction.y, 2)), vec_viewing_direction.z)) > 0.68)
		{
			std::cout << "******* normal computing process *******" << std::endl;
			// std::cout<<"size before normal compute: "<<cloud.size()<<std::endl;
			// save_cloud_txt("filtered_cloud", cloud.makeShared());
			std::chrono::steady_clock::time_point start0 = std::chrono::steady_clock::now();
			//创建法线估计估计向量
			pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
			ne.setInputCloud(cloud.makeShared());
			//创建一个空的KdTree对象，并把它传递给法线估计向量
			//基于给出的输入数据集，KdTree将被建立
			pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
			ne.setSearchMethod(tree);
			//存储输出数据
			pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
			//使用半径在查询点周围5厘米范围内的所有临近元素
			ne.setRadiusSearch(0.05);
			//计算特征值
			ne.compute(*cloud_normals);
			m_norm = *cloud_normals;

			std::cout << "******* pole finding process *******" << std::endl;
			// 根据曲率与位置关系切分点云
			pcl::PointCloud<pcl::PointXYZ> smooth_point;
			pcl::PointCloud<pcl::PointXYZ> sharp_point;
			double min_curv = 1e5, max_curv = 1e-5;
			// 找极值，先根据曲率筛选
			for (size_t i = 0; i < m_norm.size(); i++)
			{
				if (m_norm[i].curvature < min_curv)
					min_curv = m_norm[i].curvature;
				if (m_norm[i].curvature > max_curv)
					max_curv = m_norm[i].curvature;
			}
			for (size_t i = 0; i < m_norm.size(); i++)
			{
				if (m_norm[i].curvature < min_curv + 0.15 * (max_curv - min_curv))
				{
					smooth_point.push_back(cloud[i]);
				}
				else
				{
					sharp_point.push_back(cloud[i]);
				}
			}
			// save_cloud_txt("curv.txt", smooth_point.makeShared());

			// 再根据位置关系筛选出电线杆
			pcl::PointCloud<pcl::PointXYZ> pole_cloud;
			std::vector<pcl::PointXYZ> pole_centers;
			pcl::PointCloud<pcl::PointXYZ> rest_cloud;
			pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
			kdtree->setInputCloud(smooth_point.makeShared());

			pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
			// 2cm (small values may cause objects to be divided
			// in several clusters, whereas big values may join objects in a same cluster).
			clustering.setClusterTolerance(0.02);
			clustering.setMinClusterSize(100);
			clustering.setMaxClusterSize(10000);
			clustering.setSearchMethod(kdtree);
			clustering.setInputCloud(smooth_point.makeShared());
			std::vector<pcl::PointIndices> clusters;
			clustering.extract(clusters);

			std::cout << "start clustering, cloud size, cluster size: " << cloud.size() << " " << clusters.size() << std::endl;
			// For every cluster...
			int currentClusterNum = 1;
			for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
			{
				pcl::PointXYZ center_point;
				pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
				for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
				{
					cluster->points.push_back(smooth_point.points[*point]);
					center_point.x += smooth_point.points[*point].x;
					center_point.y += smooth_point.points[*point].y;
					center_point.z += smooth_point.points[*point].z;
				}
				cluster->width = cluster->points.size();
				cluster->height = 1;
				cluster->is_dense = true;
				// find center
				center_point.x /= cluster->size();
				center_point.y /= cluster->size();
				center_point.z /= cluster->size();
				// calc square error
				pcl::PointXYZ sqe;
				for (size_t i = 0; i < cluster->size(); i++)
				{
					sqe.x += pow(center_point.x - cluster->points[i].x, 2);
					sqe.y += pow(center_point.y - cluster->points[i].y, 2);
					sqe.z += pow(center_point.z - cluster->points[i].z, 2);
				}
				sqe.x /= cluster->size();
				sqe.y /= cluster->size();
				sqe.z /= cluster->size();
				// double dist = sqrt(pow(pole_pos.x - center_point.x, 2) + pow(pole_pos.y - center_point.y, 2));
				// don't limit the euclidean distance to 0.2m (assume pole length 3m, the angle allowance is around 4 degree)
				if (sqe.z > sqe.y && sqe.z > sqe.x)
				{
					pole_cloud.insert(pole_cloud.end(), cluster->begin(), cluster->end());
					pole_centers.push_back(center_point);
					// // 保存聚类点云，检查用
					// std::string fileName = "cluster/pole_cluster" + boost::to_string(currentClusterNum) + ".txt";
					// save_cloud_txt(fileName, cluster);
					// std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
				}
				else
				{
					rest_cloud.insert(rest_cloud.end(), cluster->begin(), cluster->end());
				}
				currentClusterNum++;
			}
			if (pole_centers.size() < 2)
			{
				std::cout << "not enough clusters for pole finding" << std::endl;
				return;
			}
			save_cloud_txt("cluster/pole_" + name, pole_cloud.makeShared());
			vec_viewing_direction = pcl::PointXYZ(pole_centers[1].x - pole_centers[0].x, pole_centers[1].y - pole_centers[0].y, pole_centers[1].z - pole_centers[0].z);
			pole_pos = pcl::PointXYZ((pole_centers[1].x + pole_centers[0].x) / 2.0, (pole_centers[1].y + pole_centers[0].y) / 2.0, (pole_centers[1].z + pole_centers[0].z) / 2.0);
			// pcl::PointXYZ pole_center;
			// for (size_t i = 0; i < pole_centers.size(); i++)
			// {
			// 	pole_center.x += pole_centers[i].x;
			// 	pole_center.y += pole_centers[i].y;
			// 	pole_center.z += pole_centers[i].z;
			// }
			// pole_center.x /= pole_centers.size();
			// pole_center.y /= pole_centers.size();
			// pole_center.z /= pole_centers.size();
		}

		std::cout << "******* cloud projection process *******" << std::endl;
		// transform cloud to x-y plane
		double theta = atan2(vec_viewing_direction.y, vec_viewing_direction.z);
		double phi = atan2(vec_viewing_direction.x, sqrt(pow(vec_viewing_direction.y, 2) + pow(vec_viewing_direction.z, 2)));
		std::cout << "theta, phi: " << theta << " " << phi << std::endl;
		Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
		transform_1.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitX()));
		transform_1.rotate(Eigen::AngleAxisf(-phi, Eigen::Vector3f::UnitY()));
		pcl::transformPointCloud(cloud, cloud, transform_1);

		int width = 500, height = 500;
		double border_ratio = 0.1;
		int border_width = (int)(border_ratio * width);
		int border_height = (int)(border_ratio * height);
		cv::Mat img = cv::Mat::ones(width, height, CV_8UC1);
		// pcl::PointXYZ min_p, max_p;
		pcl::getMinMax3D(cloud, min_p, max_p);
		pixel_width = (max_p.x - min_p.x) / (width - 2 * border_width);
		pixel_height = (max_p.y - min_p.y) / (height - 2 * border_height);
		// 直通滤波器获取顶部0.7m点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_top (new pcl::PointCloud<pcl::PointXYZ>);
		// 创建滤波器对象
		pcl::PassThrough<pcl::PointXYZ> pass;
		pass.setInputCloud (cloud.makeShared());
		pass.setFilterFieldName ("z");
		pass.setFilterLimits (max_p.z - 1, max_p.z);//将x轴的0到1范围内
		pass.setFilterLimitsNegative (false);//保留
		pass.filter (*cloud_top);
		pcl::getMinMax3D(*cloud_top, min_p, max_p);
		// std::cout << "pixel width: " << pixel_width << ", height: " << pixel_height << std::endl;
		for (size_t i = 0; i < cloud_top->size(); i++)
		{
			int col = (int)((cloud_top->points[i].x - min_p.x) / pixel_width) + border_width;
			int row = (int)((cloud_top->points[i].y - min_p.y) / pixel_height) + border_height;
			cv::Scalar pixel_value = img.at<uchar>(row, col);
			pixel_value[0] = (pixel_value[0] >= 254) ? 255 : pixel_value[0] + 1;
			img.at<uchar>(row, col) = pixel_value[0];
			// std::cout<<"row"<<row<<",col"<<col<<",value:"<<int(pixel_value[0])<<std::endl;
		}
		// // histogram equalization
		cv::equalizeHist(img, img);
		// image = img.clone();

		std::cout << "******* line finding process *******" << std::endl;
		// std::cout << "-22222" << std::endl;
		// cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3));
		// cv::morphologyEx(img, img, cv::MORPH_CLOSE, element);
		// cv::threshold(img, img, 235.0, 255.0, cv::ThresholdTypes::THRESH_TRUNC);
		cv::threshold(img, img, 150.0, 255.0, cv::ThresholdTypes::THRESH_BINARY);
		std::vector<cv::Vec4i> lines;
		// std::cout << "-11111" << std::endl;
		cv::HoughLinesP(img, lines, 3.0, 0.034, 100, 120.0, 45.0);
		// std::cout << "line nums before handling: " << lines.size() << std::endl;
		cv::Mat line_img = cv::Mat::zeros(img.size(), CV_8UC1);
		// std::cout << "00000" << std::endl;
		// filter similar lines
		std::vector<cv::Vec3d> norms;		// represent a, b and c param for a line
		std::vector<cv::Vec4i> line_groups; // has same index with norms, lines with different gradient or position will have different index
		for (size_t i = 0; i < lines.size(); i++)
		{
			// normalization
			cv::Vec3d norm = cv::Vec3d(lines[i][1] - lines[i][3], lines[i][2] - lines[i][0], 0);
			double length = sqrt(pow(lines[i][1] - lines[i][3], 2) + pow(lines[i][2] - lines[i][0], 2));
			norm[0] /= length;
			norm[1] /= length;
			norm[2] = -norm[0] * lines[i][0] - norm[1] * lines[i][1];
			bool similar = false;
			for (size_t j = 0; j < norms.size(); j++)
			{
				double mult = fabs(norm[0] * norms[j][0] + norm[1] * norms[j][1]);
				//std::cout<<i<<" mult: "<<mult<<" cos20: "<< cos(20)<<std::endl;
				// angle smaller than 15 degree
				if (mult >= cos(0.262))
				{
					// norm has already normalized, calc the avg dist from two edge points to the line
					double line_dist = fabs((lines[i][0] * norms[j][0] + lines[i][1] * norms[j][1] + norms[j][2]) + (lines[i][2] * norms[j][0] + lines[i][3] * norms[j][1] + norms[j][2])) / 2.0;
					// std::cout<<i<<" line dists: "<<line_dist<<std::endl;
					// near lines can be represented by the existing line
					if (line_dist <= 40)
					{
						similar = true;
						break;
					}
				}
			}
			if (!similar)
			{
				norms.push_back(norm);
				// std::cout << i << " line added " << std::endl;
				line_groups.push_back(lines[i]);
			}
		}
		// std::cout << "11111" << std::endl;
		// std::cout << "line nums after handling: " << line_groups.size() << std::endl;

		// find the sqaure tube direction beside the pole
		std::vector<cv::Vec2i> line_compare(norms.size());// store the num of lines in parallel and perpendicular
		std::vector<int> state(norms.size()); // 0 unknown, 1 maybe wire, 2 maybe tube
		int max_para=0, max_perp=0;
		for (size_t i = 0; i < norms.size(); i++)
		{
			for (size_t j = i+1; j < norms.size(); j++)
			{
				double mult = fabs(norms[j][0] * norms[i][0] + norms[j][1] * norms[i][1]);
				// std::cout<<"mult "<<mult<<" "<<cos(3.14)<<" "<<cos(1.57)<<std::endl;
				// angle larger than 75 degree
				if(mult <= cos(1.308)){
					line_compare[i][1] +=1;
					line_compare[j][1] +=1;
					// std::cout<<"line "<<i<<" and line "<<j<<" perpendicular."<<std::endl;
					if(line_compare[i][1] > max_perp){
						max_perp = line_compare[i][1];
					}
					if(line_compare[j][1] > max_perp){
						max_perp = line_compare[j][1];
					}
				}
				// angle smaller than 20 degree
				if(mult >= cos(0.348)){
					line_compare[i][0] +=1;
					line_compare[j][0] +=1;
					// std::cout<<"line "<<i<<" and line "<<j<<" in parallel."<<std::endl;
					if(line_compare[i][0] > max_para){
						max_para = line_compare[i][0];
					}
					if(line_compare[j][0] > max_para){
						max_para = line_compare[j][0];
					}
				}
			}
		}
		int tube_index[2] = {0};
		double square_tube_length = 1e-5;
		// labeling each filtered line
		for (size_t i = 0; i < line_compare.size(); i++)
		{
			std::cout<<"line "<<i<<" : "<<line_compare[i][0]<<" "<<line_compare[i][1]<<std::endl;
			if (line_compare[i][1] == max_perp)
			{
				state[i] = 2;
				double l_sq = pow(line_groups[i][0] - line_groups[i][2], 2) + pow(line_groups[i][1] - line_groups[i][3], 2);
				if (l_sq > square_tube_length)
				{
					tube_index[0] = i;
					square_tube_length = l_sq;
				}
			}
			// has most parallel lines, or at least 1 parallel and [1,max_perp) perpendicular
			else if (line_compare[i][0] == max_para || ( line_compare[i][0]>0 && line_compare[i][1] < max_perp && line_compare[i][1] >= 1))
			{
				state[i] = 1;
			}
		}
		
		int find_perpendicular_line = 0;
		// for (size_t i = 1; i < norms.size(); i++)
		// {
		// 	double mult = fabs(norms[tube_index[0]][0] * norms[i][0] + norms[tube_index[0]][1] * norms[i][1]);
		// 	// larger than 75 degree
		// 	if (mult <= cos(1.308))
		// 	{
		// 		if (find_perpendicular_line == 0)
		// 		{
		// 			// wait and see
		// 			find_perpendicular_line++;
		// 			tube_index[1] = i;
		// 		}
		// 		else if (find_perpendicular_line < 0)
		// 		{
		// 			// the checked line
		// 			tube_index[0] = i;
		// 			break;
		// 		}
		// 		else
		// 		{
		// 			// current line
		// 			break;
		// 		}
		// 	}
		// 	if (mult >= cos(0.262))
		// 	{
		// 		if (find_perpendicular_line <= 0)
		// 		{
		// 			find_perpendicular_line--;
		// 		}
		// 		else
		// 		{
		// 			tube_index[0] = tube_index[1];
		// 			break;
		// 		}
		// 	}
		// }
		// std::cout << "22222" << std::endl;
		// find cross points
		std::vector<cv::Vec2i> cross_points;
		for (size_t i = 0; i < norms.size(); i++)
		{
			if (norms.size() != 4)
			{
				if (state[i] == 0)
				{
					std::cout << "not core lines" << std::endl;
					continue;
				}
				if (state[i] == 2)
				{
					std::cout << "tube line, don't calc" << std::endl;
					continue;
				}
			}
			cv::Vec3d l_base = norms[tube_index[0]], line = norms[i];
			double mult = fabs(l_base[0] * line[0] + l_base[1] * line[1]);
			double denom = l_base[1] * line[0] - l_base[0] * line[1];
			double numin = l_base[0] * line[2] - l_base[2] * line[0];
			// perpendicular lines, if only 4 lines, calc the 3 cross points
			if (mult <= cos(1.308) || (norms.size()==4 && tube_index[0]!=i))
			{
				if (denom == 0)
				{
					std::cout<<"zero denominator"<<std::endl;
					continue;
				}
				else
				{
					double y_value = (int)(numin / denom);
					double x_value = 0;
					if (l_base[0] != 0)
					{
						x_value = (int)((l_base[1] * y_value + l_base[2]) / (-l_base[0]));
						cross_points.push_back(cv::Vec2i(x_value, y_value));
					}
					else if (line[0] != 0)
					{
						x_value = (int)((line[1] * y_value + line[2]) / (-line[0]));
						cross_points.push_back(cv::Vec2i(int(x_value), int(y_value)));
					}else{
						std::cout<<"a0 ==0 and a1 == 0"<<std::endl;
						continue;
					}
				}
			}
		}
		// std::cout << "33333" << std::endl;
		pcl::PointXYZ point;
		point.x = (line_groups[tube_index[0]][2] - line_groups[tube_index[0]][0]) * pixel_width;
		point.y = (line_groups[tube_index[0]][3] - line_groups[tube_index[0]][1]) * pixel_height;
		point.z = 0;
		// transform cloud so that the tube align with x axis
		theta = atan2(point.y, point.x);
		std::cout<<theta<<std::endl;
		Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
		transform_2.rotate(Eigen::AngleAxisf(-theta, Eigen::Vector3f::UnitZ()));
		pcl::transformPointCloud(cloud, cloud, transform_2);

		for (size_t i = 0; i < line_groups.size(); i++)
		{
			cv::line(img,
					 cv::Point(line_groups[i][0], line_groups[i][1]),
					 cv::Point(line_groups[i][2], line_groups[i][3]),
					 cv::Scalar(150),
					 tube_index[0] == i ? 4 : 2,
					 cv::LINE_8);
		}
		if(cross_points.size() != 3){
			cv::namedWindow("image show", cv::WINDOW_KEEPRATIO);
			cv::imshow("image show", img);
			std::cout<<" cannot find 3 cross points, actually get "<<cross_points.size()<<std::endl;
			cv::waitKey();
			return;
		}else{
			// int compare = 0;
			// if (theta >= M_PI_4)
			// {
			// 	compare = 1;
			// }
			// if (cross_points[0][compare] > cross_points[1][compare])
			// {
			// 	if(cross_points[0][compare] > cross_points[2][compare] && cross_points[1][compare]<cross_points[2])
			// }
			// cross_points[0] = (cross_points[0]+cross_points[1])/2.0;
			// cross_points[1] = (cross_points[1]+cross_points[2])/2.0;
			// cross_points.pop_back();
			for (size_t i = 0; i < 3; i++)
			{
				cv::circle(img,
					cross_points[i],
					10,
					cv::Scalar(150),
					cv::FILLED,
					cv::LINE_8);
			}		
		}
		// std::cout << "44444" << std::endl;
		// 保存投影
		cv::imwrite("image/"+name.substr(0, name.size()-4).append(".jpg"), img);

		// //可视化
		// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
		// viewer.setBackgroundColor(0.0, 0.0, 0.0);
		// viewer.addPointCloud<pcl::PointXYZ>(cloud.makeShared());
		pcl::PointCloud<pcl::PointXYZ> cross_points_3d;
		for (size_t i = 0; i < cross_points.size(); i++)
		{
			cross_points_3d.push_back(pcl::PointXYZ( (cross_points[i][0]-border_width)* pixel_width+min_p.x, (cross_points[i][1]-border_height)*pixel_height+min_p.y, min_p.z));
		}
		pcl::transformPointCloud(cross_points_3d, cross_points_3d, transform_2);
		// for (size_t i = 0; i < cross_points.size(); i++)
		// {
		// 	viewer.addSphere(cross_points_3d[i], 0.05, "sphere"+boost::to_string(i));
		// }
		// while (!viewer.wasStopped())
		// {
		// 	usleep(30*1000);
		// 	viewer.spinOnce();
		// }

		// 点云已对齐到x坐标轴
		std::vector<double> x_values;
		std::vector<pcl::PointCloud<pcl::PointXYZ>> pieces;
		for (size_t i = 0; i < cross_points_3d.size(); i++)
		{
			x_values.push_back(cross_points_3d[i].x);
		}
		// x坐标值排序
		for (size_t i = 0; i < x_values.size(); i++)
		{
			for (size_t j = i; j < x_values.size(); j++)
			{
				if(x_values[j]<x_values[i]){
					double temp = x_values[i];
					x_values[i] = x_values[j];
					x_values[j] = temp;
				}
			}
		}
		x_values[0] = (x_values[0] + x_values[1]) / 2.0;
		x_values[1] = (x_values[1] + x_values[2]) / 2.0;
		x_values.pop_back();
		for (size_t i = 0; i < x_values.size()+1; i++)
		{
			pieces.push_back(pcl::PointCloud<pcl::PointXYZ>());
		}
		// 按x值分类
		for (size_t i = 0; i < cloud.size(); i++)
		{
			if(cloud[i].x>x_values[x_values.size()-1]){
				pieces[pieces.size()-1].push_back(cloud[i]);
			}else{
				for (size_t j = 0; j < x_values.size(); j++)
				{
					if(cloud[i].x < x_values[j]){
						pieces[j].push_back(cloud[i]);
						break;
					}
				}
			}
		}
		// 分别保存txt
		for (size_t i = 0; i < pieces.size(); i++)
		{
			save_cloud_txt("segments/"+name.substr(0, name.size()-4).append("cluster"+boost::to_string(i)+".txt"), pieces[i].makeShared());
		}
		
	}

	// read points from vector
	bool get_total_points(std::vector<pcl::PointXYZ> points)
	{
		if (points.size() == 0)
			return false;
		m_total_points.assign(points.begin(), points.end());
		for (int i = 0; i < m_total_points.size() - 1; i++)
		{
			m_origin_cloud.push_back(m_total_points[i]);
		}
		return true;
	}
	// read points from pcd file
	bool get_total_points(std::string filename)
	{
		pcl::io::loadPCDFile(filename, m_origin_cloud);
		if (m_origin_cloud.size() != 0)
		{
			for (pcl::PointCloud<pcl::PointXYZ>::iterator it = m_origin_cloud.begin(); it != m_origin_cloud.end(); it++)
			{
				m_total_points.push_back(*it);
			}
			return true;
		}
		else
			return false;
	}
	// read points from file
	bool get_total_points_from_txt(std::string filename, const char tag, int point_type = 3)
	{
		m_total_points.clear();
		m_total_points_rgb.clear();
		m_origin_cloud.clear();
		m_origin_cloud_rgb.clear();
		std::ifstream in(filename);
		std::string linestr;
		while (getline(in, linestr))
		{
			std::vector<std::string> strvec;
			std::string s;
			std::stringstream ss(linestr);
			while (getline(ss, s, tag))
			{
				strvec.push_back(s);
			}
			if (strvec.size() != point_type)
			{
				std::cout << "unsupported data type" << std::endl;
				return false;
			}
			if (point_type == 3)
			{
				pcl::PointXYZ p;
				p.x = stod(strvec[0]);
				p.y = stod(strvec[1]);
				p.z = stod(strvec[2]);

				m_total_points.push_back(p);
				std::cout << m_total_points.size() << std::endl;
			}
			else if (point_type == 6)
			{
				pcl::PointXYZRGB p;
				p.x = stod(strvec[0]);
				p.y = stod(strvec[1]);
				p.z = stod(strvec[2]);
				p.r = stod(strvec[3]);
				p.g = stod(strvec[4]);
				p.b = stod(strvec[5]);
				m_total_points_rgb.push_back(p);
				std::cout << m_total_points_rgb.size() << std::endl;
			}
		}
		in.close();
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		if (point_type == 3)
		{
			if (m_total_points.size() <= 0)
			{
				std::cout << "no point at all" << std::endl;
				return false;
			}
			for (int i = 0; i < m_total_points.size() - 1; i++)
			{
				m_origin_cloud.push_back(m_total_points[i]);
			}
			return true;
		}
		else if (point_type == 6)
		{
			if (m_total_points_rgb.size() <= 0)
			{
				std::cout << "no point at all" << std::endl;
				return false;
			}
			for (int i = 0; i < m_total_points_rgb.size() - 1; i++)
			{
				m_origin_cloud_rgb.push_back(m_total_points_rgb[i]);
			}
			return true;
		}
		return false;
	}

	// remove outlier
	void outlierRemoval(std::string filename)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sorfilter(true);
		sorfilter.setInputCloud(m_origin_cloud.makeShared());
		sorfilter.setMeanK(50);
		sorfilter.setStddevMulThresh(1.0);
		sorfilter.filter(m_denoised_cloud);
		pcl::io::savePCDFileASCII(filename, m_denoised_cloud);
	}
	// remove outlier
	void outlierRemoval(pcl::PointCloud<pcl::PointXYZ> cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out)
	{
		pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sorfilter(true);
		sorfilter.setInputCloud(cloud_in.makeShared());
		sorfilter.setMeanK(50);
		sorfilter.setStddevMulThresh(0.1);
		sorfilter.filter(cloud_out);
	}
	void outlierRemoval(pcl::PointCloud<pcl::PointXYZRGB> cloud_in, pcl::PointCloud<pcl::PointXYZRGB> &cloud_out)
	{
		pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sorfilter(true);
		sorfilter.setInputCloud(cloud_in.makeShared());
		sorfilter.setMeanK(50);
		sorfilter.setStddevMulThresh(0.1);
		sorfilter.filter(cloud_out);
	}
	// voxel grid
	void voxelGrid(double width, double scale = 1.0)
	{
		pcl::PointXYZ min_p, max_p, index_p;
		pcl::getMinMax3D(m_origin_cloud, min_p, max_p);
		std::vector<pcl::PointCloud<pcl::PointXYZ>> grids;
		index_p.x = int(ceil((max_p.x - min_p.x + width) / width));
		index_p.y = int(ceil((max_p.y - min_p.y + width) / width));
		index_p.z = int(ceil((max_p.z - min_p.z + width) / width));
		for (int i = 0; i < index_p.x * index_p.y * index_p.z; i++)
		{
			pcl::PointCloud<pcl::PointXYZ> temp;
			grids.push_back(temp);
		}
		for (int i = 0; i < m_origin_cloud.size(); i++)
		{
			int index_x = int(ceil((m_origin_cloud[i].x - min_p.x - width / 2.0) / width));
			int index_y = int(ceil((m_origin_cloud[i].y - min_p.y - width / 2.0) / width));
			int index_z = int(ceil((m_origin_cloud[i].z - min_p.z - width / 2.0) / width));
			int index = index_x * index_p.y * index_p.z + index_y * index_p.z + index_z;
			if (scale != 1 && scale > 0)
			{
				m_origin_cloud[i].x *= scale;
				m_origin_cloud[i].y *= scale;
				m_origin_cloud[i].z *= scale;
			}
			grids[index].push_back(m_origin_cloud[i]);
		}
		int count = 0;
		for (int i = 0; i < grids.size(); i++)
		{
			if (grids[i].size() <= 0)
				continue;
			else
			{
				pcl::PointCloud<pcl::PointXYZ> out;
				outlierRemoval(grids[i], out);
				if (out.size() >= 15)
				{
					save_cloud_txt("segments/seg" + boost::to_string(count) + ".txt", out.makeShared());
					count++;
				}
			}
		}
	}

	// voxel grid followed by outlier removal, normal compute and voxel filter
	void voxelGridRGB(double width, double leafsize = 1, double scale = 1.0)
	{

		pcl::PointXYZRGB min_p, max_p, index_p;
		pcl::PointCloud<pcl::PointXYZRGB> cloud_rgb;
		for (int i = 0; i < m_origin_cloud_rgb.size(); i++)
		{
			cloud_rgb.push_back(m_origin_cloud_rgb[i]);
		}
		pcl::getMinMax3D(cloud_rgb, min_p, max_p);
		std::vector<pcl::PointCloud<pcl::PointXYZRGB>> grids;
		index_p.x = int(ceil((max_p.x - min_p.x + width) / width));
		index_p.y = int(ceil((max_p.y - min_p.y + width) / width));
		index_p.z = int(ceil((max_p.z - min_p.z + width) / width));
		for (int i = 0; i < index_p.x * index_p.y * index_p.z; i++)
		{
			pcl::PointCloud<pcl::PointXYZRGB> temp;
			grids.push_back(temp);
		}
		for (int i = 0; i < cloud_rgb.size(); i++)
		{
			int index_x = int(ceil((cloud_rgb[i].x - min_p.x - width / 2.0) / width));
			int index_y = int(ceil((cloud_rgb[i].y - min_p.y - width / 2.0) / width));
			int index_z = int(ceil((cloud_rgb[i].z - min_p.z - width / 2.0) / width));
			int index = index_x * index_p.y * index_p.z + index_y * index_p.z + index_z;
			if (scale != 1 && scale > 0)
			{
				cloud_rgb[i].x *= scale;
				cloud_rgb[i].y *= scale;
				cloud_rgb[i].z *= scale;
			}
			grids[index].push_back(cloud_rgb[i]);
		}
		int count = 0;
		for (int i = 0; i < grids.size(); i++)
		{
			if (grids[i].size() <= 0)
				continue;
			else
			{

				// outlier removal
				pcl::PointCloud<pcl::PointXYZRGB> out;
				outlierRemoval(grids[i], out);

				// // point cloud transform
				// std::uniform_real_distribution<double> u_a(-1, 1);
				// std::uniform_real_distribution<double> u_angle(-M_PI, M_PI);
				// double transform_data[6] = {u_a(rnd), u_a(rnd), u_a(rnd), u_angle(rnd), u_angle(rnd), u_angle(rnd)};
				// Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
				// transform_1.translation() << transform_data[0], transform_data[1], transform_data[2];
				// transform_1.rotate(Eigen::AngleAxisf(transform_data[3], Eigen::Vector3f::UnitX()));
				// transform_1.rotate(Eigen::AngleAxisf(transform_data[4], Eigen::Vector3f::UnitY()));
				// transform_1.rotate(Eigen::AngleAxisf(transform_data[5], Eigen::Vector3f::UnitZ()));
				// pcl::transformPointCloud(out, out, transform_1);

				normalCompute(out);

				if (leafsize != 1)
				{
					voxelFilterRGB(leafsize, out);
				}
				if (out.size() >= 15)
				{
					save_cloud_txt("segments/seg" + boost::to_string(int(width)) + "_" + boost::to_string(int(leafsize * 1000)) + "_index" + boost::to_string(count) + /*"_r"+ boost::to_string(int(transform_data[0] * 1000)) +*/ ".txt", out.makeShared());
					count++;
				}
			}
		}
	}

	void voxelFilterRGB(double leafsize, pcl::PointCloud<pcl::PointXYZRGB> &out)
	{
		std::cout << "******* voxel filter *******" << std::endl;
		// voxel grid filter
		pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> approximate_voxel_filter;
		approximate_voxel_filter.setLeafSize(leafsize, leafsize, leafsize);
		approximate_voxel_filter.setInputCloud(out.makeShared());
		approximate_voxel_filter.filter(out);
	}

	// compute normal, find viewing direction and visualization
	bool normalCompute(pcl::PointCloud<pcl::PointXYZRGB> cloud)
	{
		std::cout << "******* normal computing process *******" << std::endl;
		// std::cout<<"size before normal compute: "<<cloud.size()<<std::endl;
		// save_cloud_txt("filtered_cloud", cloud.makeShared());
		std::chrono::steady_clock::time_point start0 = std::chrono::steady_clock::now();
		//创建法线估计估计向量
		pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
		ne.setInputCloud(cloud.makeShared());
		//创建一个空的KdTree对象，并把它传递给法线估计向量
		//基于给出的输入数据集，KdTree将被建立
		pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
		ne.setSearchMethod(tree);
		//存储输出数据
		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
		//使用半径在查询点周围5厘米范围内的所有临近元素
		ne.setRadiusSearch(0.05);
		//计算特征值
		ne.compute(*cloud_normals);
		m_norm = *cloud_normals;
		std::chrono::steady_clock::time_point end0 = std::chrono::steady_clock::now();
		std::cout << "normal compute seconds: " << std::chrono::duration_cast<std::chrono::milliseconds>(end0 - start0).count() / 1000.0 << std::endl;

		// //可视化
		// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
		// viewer.setBackgroundColor(0.0, 0.0, 0.0);
		// viewer.addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(cloud.makeShared(), m_norm.makeShared());
		// // viewer.addLine(origin, target);
		// // std::cout<<"normal size: "<<m_norm.size()<<std::endl;
		// while (!viewer.wasStopped())
		// {
		// 	usleep(30*1000);
		// 	viewer.spinOnce();
		// }
		if (m_norm.size() > 0)
			return true;
		else
			return false;
	}

	// find pole according to normals, differences in z axis and distance to pole center calc from "findViewingDirection"
	bool findPole(pcl::PointCloud<pcl::PointXYZRGB> cloud)
	{
		std::cout << "******* pole finding process *******" << std::endl;
		// 根据曲率与位置关系切分点云
		pcl::PointCloud<pcl::PointXYZRGB> smooth_point;
		pcl::PointCloud<pcl::PointXYZRGB> sharp_point;
		double min_curv = 1e5, max_curv = 1e-5;
		// 找极值，先根据曲率筛选
		for (size_t i = 0; i < m_norm.size(); i++)
		{
			if (m_norm[i].curvature < min_curv)
				min_curv = m_norm[i].curvature;
			if (m_norm[i].curvature > max_curv)
				max_curv = m_norm[i].curvature;
		}
		for (size_t i = 0; i < m_norm.size(); i++)
		{
			if (m_norm[i].curvature < min_curv + 0.2 * (max_curv - min_curv))
			{
				smooth_point.push_back(cloud[i]);
			}
			else
			{
				sharp_point.push_back(cloud[i]);
			}
		}
		// save_cloud_txt("curv.txt", smooth_point.makeShared());

		// 再根据位置关系筛选出电线杆
		pcl::PointCloud<pcl::PointXYZRGB> pole_cloud;
		pcl::PointCloud<pcl::PointXYZRGB> rest_cloud;
		pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZRGB>);
		kdtree->setInputCloud(smooth_point.makeShared());

		pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
		// 2cm (small values may cause objects to be divided
		// in several clusters, whereas big values may join objects in a same cluster).
		clustering.setClusterTolerance(0.02);
		clustering.setMinClusterSize(10);
		clustering.setMaxClusterSize(10000);
		clustering.setSearchMethod(kdtree);
		clustering.setInputCloud(smooth_point.makeShared());
		std::vector<pcl::PointIndices> clusters;
		clustering.extract(clusters);

		std::cout << "start clustering, cloud size, cluster size: " << cloud.size() << " " << clusters.size() << std::endl;
		// For every cluster...
		int currentClusterNum = 1;
		for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
		{
			pcl::PointXYZRGB center_point;
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
			for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			{
				cluster->points.push_back(smooth_point.points[*point]);
				center_point.x += smooth_point.points[*point].x;
				center_point.y += smooth_point.points[*point].y;
				center_point.z += smooth_point.points[*point].z;
			}
			cluster->width = cluster->points.size();
			cluster->height = 1;
			cluster->is_dense = true;
			// find center
			center_point.x /= cluster->size();
			center_point.y /= cluster->size();
			center_point.z /= cluster->size();
			// calc square error
			pcl::PointXYZ sqe;
			for (size_t i = 0; i < cluster->size(); i++)
			{
				sqe.x += pow(center_point.x - cluster->points[i].x, 2);
				sqe.y += pow(center_point.y - cluster->points[i].y, 2);
				sqe.z += pow(center_point.z - cluster->points[i].z, 2);
			}
			sqe.x /= cluster->size();
			sqe.y /= cluster->size();
			sqe.z /= cluster->size();
			double dist = sqrt(pow(pole_pos.x - center_point.x, 2) + pow(pole_pos.y - center_point.y, 2));
			// don't limit the euclidean distance to 0.2m (assume pole length 3m, the angle allowance is around 4 degree)
			if (sqe.z > sqe.y && sqe.z > sqe.x && dist < 0.2)
			{
				pole_cloud.insert(pole_cloud.end(), cluster->begin(), cluster->end());
				// // 保存聚类点云，检查用
				// std::string fileName = "cluster/pole_cluster" + boost::to_string(currentClusterNum) + ".txt";
				// save_cloud_txt(fileName, cluster);
				// std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
			}
			else
			{
				rest_cloud.insert(rest_cloud.end(), cluster->begin(), cluster->end());
			}
			currentClusterNum++;
		}
		rest_cloud.insert(rest_cloud.end(), sharp_point.begin(), sharp_point.end());
		// m_origin_cloud_rgb = rest_cloud;
		pcl::copyPointCloud(rest_cloud, m_rest_cloud);
		// save_cloud_txt("cluster/pole.txt", pole_cloud.makeShared());
		std::cout << "clustering finished." << std::endl;
	}

	bool findViewingDirection(pcl::PointCloud<pcl::PointXYZRGB> cloud)
	{
		std::cout << "******* viewing direction finding process *******" << std::endl;
		pcl::PointXYZRGB min_p, max_p, avg_slice1, avg_slice2;
		pcl::getMinMax3D(cloud, min_p, max_p);
		double offset = 0.1, thickness = 0.05;
		pcl::PointCloud<pcl::PointXYZRGB> slice1, slice2;
		for (size_t i = 0; i < cloud.size(); i++)
		{
			if (cloud[i].z >= min_p.z && cloud[i].z <= min_p.z + thickness)
			{
				slice1.push_back(cloud[i]);
				avg_slice1.x += cloud[i].x;
				avg_slice1.y += cloud[i].y;
				avg_slice1.z += cloud[i].z;
			}
			else if (cloud[i].z >= min_p.z + offset && cloud[i].z <= min_p.z + offset + thickness)
			{
				slice2.push_back(cloud[i]);
				avg_slice2.x += cloud[i].x;
				avg_slice2.y += cloud[i].y;
				avg_slice2.z += cloud[i].z;
			}
		}
		if (slice1.size() <= 0 || slice2.size() <= 0)
		{
			std::cout << "cannot find slices" << std::endl;
			return false;
		}
		avg_slice1.x /= slice1.size();
		avg_slice1.y /= slice1.size();
		avg_slice1.z /= slice1.size();
		avg_slice2.x /= slice2.size();
		avg_slice2.y /= slice2.size();
		avg_slice2.z /= slice2.size();
		vec_viewing_direction = pcl::PointXYZ(avg_slice2.x - avg_slice1.x, avg_slice2.y - avg_slice1.y, avg_slice2.z - avg_slice1.z);
		pole_pos = pcl::PointXYZ((avg_slice2.x + avg_slice1.x) / 2.0, (avg_slice2.y + avg_slice1.y) / 2.0, (avg_slice2.z + avg_slice1.z) / 2.0);
		cloudProjection();

		// // find the pole, bad effects
		// pcl::PointXYZRGB s1_min, s1_max, s2_min, s2_max;
		// pcl::getMinMax3D(slice1, s1_min, s1_max);
		// pcl::getMinMax3D(slice2, s2_min, s2_max);
		// s2_min.x = s1_min.x + (s2_min.x-s1_min.x)*(min_p.z - s1_min.z) / (s2_min.z - s1_min.z);
		// s2_min.y = s1_min.y + (s2_min.x-s1_min.x)*(min_p.z - s1_min.z) / (s2_min.z - s1_min.z);
		// s2_max.x = s1_max.x + (s2_max.x-s1_max.x)*(max_p.z - s1_max.z) / (s2_max.z - s1_max.z);
		// s2_max.y = s1_max.y + (s2_max.x-s1_max.x)*(max_p.z - s1_max.z) / (s2_max.z - s1_max.z);
		// // if(s1_min.x > s2_min.x) s1_min.x = s2_min.x;
		// // if(s1_min.y > s2_min.y) s1_min.y = s2_min.y;
		// // if(s1_max.x < s2_max.x) s1_max.x = s2_max.x;
		// // if(s1_max.y < s2_max.y) s1_max.y = s2_max.y;
		// double pole_offset = 0.01;
		// for (size_t i = 0; i < cloud.size(); i++)
		// {
		// 	if (cloud[i].x >= s1_min.x - pole_offset && cloud[i].x <= s1_max.x + pole_offset && cloud[i].y >= s1_min.y - pole_offset && cloud[i].y <= s1_max.y + pole_offset)
		// 	{
		// 		m_pole.push_back(pcl::PointXYZ(cloud[i].x, cloud[i].y, cloud[i].z));
		// 	}
		// }
		// save_cloud_txt("pole.txt",m_pole.makeShared());
		// save_cloud_txt("whole.txt", cloud.makeShared());
		return true;
	}

	void cloudProjection()
	{
		std::cout << "******* cloud projection process *******" << std::endl;
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZRGB>);

		// there's no need to project the cloud points into a plane, just transform the cloud
		// //create a set of planar coefficients with X=Y=0, Z = 1
		// pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
		// coefficients->values.resize(4);
		// coefficients->values[0] = vec_viewing_direction.x;
		// coefficients->values[1] = vec_viewing_direction.y;
		// coefficients->values[2] = vec_viewing_direction.z;
		// coefficients->values[3] = 0;

		// //create the projected object
		// pcl::ProjectInliers<pcl::PointXYZRGB> proj;
		// proj.setModelType(pcl::SACMODEL_PLANE);
		// proj.setInputCloud(m_origin_cloud_rgb.makeShared());
		// proj.setModelCoefficients(coefficients);
		// proj.filter(*cloud_projected);

		// save_cloud_txt("projected.txt", cloud_projected);
		// std::cerr << "cloud after projection: " << cloud_projected->points.size() << std::endl;

		// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
		// viewer.setBackgroundColor(0.0, 0.0, 0.0);
		// viewer.addPointCloud<pcl::PointXYZRGB>(cloud_projected);
		// std::cout<<"normal size: "<<m_norm.size()<<std::endl;
		// while (!viewer.wasStopped())
		// {
		// 	usleep(15 * 1000);
		// 	viewer.spinOnce();
		// }

		// transform cloud to x-y plane
		double theta = atan2(vec_viewing_direction.y, vec_viewing_direction.z);
		double phi = atan2(vec_viewing_direction.x, sqrt(pow(vec_viewing_direction.y, 2) + pow(vec_viewing_direction.z, 2)));
		std::cout << "theta, phi: " << theta << " " << phi << std::endl;
		Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
		transform_1.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitX()));
		transform_1.rotate(Eigen::AngleAxisf(-phi, Eigen::Vector3f::UnitY()));
		// pcl::transformPointCloud(*cloud_projected, curr1, transform_1);
		pcl::transformPointCloud(m_origin_cloud_rgb, m_origin_cloud_rgb, transform_1);
		// for (size_t i = 0; i < m_origin_cloud_rgb.size(); i++)
		// {
		// 	m_origin_cloud_rgb[i].z = 0;
		// }
		// save_cloud_txt("origin.txt", cloud_projected);
		save_cloud_txt("trans.txt", m_origin_cloud_rgb.makeShared());

		int width = 500, height = 500;
		double border_ratio = 0.1;
		int border_width = (int)(border_ratio * width);
		int border_height = (int)(border_ratio * height);
		cv::Mat img = cv::Mat::zeros(width, height, CV_8UC1);
		pcl::PointXYZRGB min_p, max_p;
		pcl::getMinMax3D(m_origin_cloud_rgb, min_p, max_p);
		pixel_width = (max_p.x - min_p.x) / (width - 2 * border_width);
		pixel_height = (max_p.y - min_p.y) / (height - 2 * border_height);
		std::cout << "pixel width: " << pixel_width << ", height: " << pixel_height << std::endl;
		for (size_t i = 0; i < m_origin_cloud_rgb.size(); i++)
		{
			int col = (int)((m_origin_cloud_rgb[i].x - min_p.x) / pixel_width) + border_width;
			int row = (int)((m_origin_cloud_rgb[i].y - min_p.y) / pixel_height) + border_height;
			cv::Scalar pixel_value = img.at<uchar>(row, col);
			pixel_value[0] = (pixel_value[0] >= 254) ? 255 : pixel_value[0] + 1;
			img.at<uchar>(row, col) = pixel_value[0];
			// std::cout<<"row"<<row<<",col"<<col<<",value:"<<int(pixel_value[0])<<std::endl;
		}
		// // cv::namedWindow("img", cv::WINDOW_KEEPRATIO);
		// // cv::imshow("img", img);
		// // histogram equalization
		cv::equalizeHist(img, img);
		// cv::namedWindow("origin", cv::WINDOW_KEEPRATIO);
		// cv::imshow("origin", img);
		image = img.clone();
		// cv::waitKey(0);
	}

	void findLines(double line_width_in_pixel)
	{
		std::cout << "******* line finding process *******" << std::endl;
		// cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5,5));
		// cv::morphologyEx(image, image, cv::MORPH_OPEN, element);
		// cv::namedWindow("image0", cv::WINDOW_KEEPRATIO);
		// cv::imshow("image0", image);
		// cv::Canny(image, image, 50, 150, 3, true);
		// cv::morphologyEx(image, image, cv::MORPH_CLOSE, element);
		cv::threshold(image, image, 150, 255, cv::ThresholdTypes::THRESH_BINARY);
		std::vector<cv::Vec4i> lines;
		cv::HoughLinesP(image, lines, 6, 0.034, 150, 100, 50);
		std::cout << "line nums before handling: " << lines.size() << std::endl;
		cv::Mat line_img = cv::Mat::zeros(image.size(), CV_8UC1);

		// filter similar lines
		std::vector<cv::Vec3d> norms;		// represent a, b and c param for a line
		std::vector<cv::Vec4i> line_groups; // has same index with norms, lines with different gradient or position will have different index
		for (size_t i = 0; i < lines.size(); i++)
		{
			// normalization
			cv::Vec3d norm = cv::Vec3d(lines[i][1] - lines[i][3], lines[i][2] - lines[i][0], 0);
			double length = sqrt(pow(lines[i][1] - lines[i][3], 2) + pow(lines[i][2] - lines[i][0], 2));
			norm[0] /= length;
			norm[1] /= length;
			norm[2] = -norm[0] * lines[i][0] - norm[1] * lines[i][1];
			bool similar = false;
			for (size_t j = 0; j < norms.size(); j++)
			{
				double mult = fabs(norm[0] * norms[j][0] + norm[1] * norms[j][1]);
				//std::cout<<i<<" mult: "<<mult<<" cos20: "<< cos(20)<<std::endl;
				// angle smaller than 10 degree
				if (mult >= cos(0.349))
				{
					// norm has already normalized, calc the avg dist from two edge points to the line
					double line_dist = fabs((lines[i][0] * norms[j][0] + lines[i][1] * norms[j][1] + norms[j][2]) + (lines[i][2] * norms[j][0] + lines[i][3] * norms[j][1] + norms[j][2])) / 2.0;
					// std::cout<<i<<" line dists: "<<line_dist<<std::endl;
					// near lines can be represented by the existing line
					if (line_dist <= line_width_in_pixel)
					{
						similar = true;
						break;
					}
				}
			}
			if (!similar)
			{
				norms.push_back(norm);
				std::cout << i << " line added " << std::endl;
				line_groups.push_back(lines[i]);
			}
		}
		std::cout << "line nums after handling: " << line_groups.size() << std::endl;
		// find the sqaure tube direction beside the pole
		int tube_index[2] = {0};
		int find_perpendicular_line = 0;
		for (size_t i = 1; i < norms.size(); i++)
		{
			double mult = fabs(norms[tube_index[0]][0] * norms[i][0] + norms[tube_index[0]][1] * norms[i][1]);
			// larger than 75 degree
			if (mult <= cos(1.308))
			{
				if (find_perpendicular_line == 0)
				{
					// wait and see
					find_perpendicular_line++;
					tube_index[1] = i;
				}
				else if (find_perpendicular_line < 0)
				{
					// the checked line
					tube_index[0] = i;
					break;
				}
				else
				{
					// current line
					break;
				}
			}
			if (mult >= cos(0.262))
			{
				if (find_perpendicular_line <= 0)
				{
					find_perpendicular_line--;
				}
				else
				{
					tube_index[0] = tube_index[1];
					break;
				}
			}
		}

		for (size_t i = 0; i < line_groups.size(); i++)
		{
			cv::line(line_img,
					 cv::Point(line_groups[i][0], line_groups[i][1]),
					 cv::Point(line_groups[i][2], line_groups[i][3]),
					 cv::Scalar(255),
					 tube_index[0] == i ? 4 : 1,
					 cv::LINE_8);
			// cv::line(line_img,
			// 		cv::Point(lines[i][0], lines[i][1]),
			// 		cv::Point(lines[i][0]+lines[i][1] -lines[i][3], lines[i][1]+lines[i][2]-lines[i][0]), // 垂线
			// 		cv::Scalar(255),
			// 		2,
			// 		cv::LINE_8);
		}
		// cv::namedWindow("image", cv::WINDOW_KEEPRATIO);
		// cv::imshow("image", image);
		// cv::namedWindow("lines", cv::WINDOW_KEEPRATIO);
		// cv::imshow("lines", line_img);
		// cv::waitKey();

		// // find paralleled lines
		// std::vector<cv::Vec4i> parallel_lines;
		// for (size_t i = 0; i < line_groups.size(); i++)
		// {
		// 	double mult = fabs(norms[tube_index[0]][0] * norms[i][0] + norms[tube_index[0]][1] * norms[i][1]);
		// 	if(mult <= cos(1.308)){
		// 		parallel_lines.push_back(line_groups[i]);
		// 	}
		// }
		// if(parallel_lines.size() != 3){
		// 	std::cout<<"find parallel lines failed"<<std::endl;
		// 	return;
		// }
		// // find tube direction in cloud
		// pcl::PointXYZ point;
		// point.x = (line_groups[tube_index[0]][2] - line_groups[tube_index[0]][0]) * pixel_width;
		// point.y = (line_groups[tube_index[0]][3] - line_groups[tube_index[0]][1]) * pixel_height;
		// point.z = 0;
		// // transform cloud so that the tube align with x axis
		// double theta = atan2(point.y, point.x);
		// Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
		// transform_1.rotate(Eigen::AngleAxisf(-theta, Eigen::Vector3f::UnitZ()));
		// pcl::transformPointCloud(m_origin_cloud_rgb, m_origin_cloud_rgb, transform_1);
	}

	void findTube(pcl::PointCloud<pcl::PointXYZRGB> cloud)
	{
		std::cout << "******* tube finding process *******" << std::endl;
		// 再根据位置关系筛选出电线杆
		pcl::PointCloud<pcl::PointXYZRGB> tube_cloud;
		pcl::PointCloud<pcl::PointXYZRGB> rest_cloud;
		pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZRGB>);
		kdtree->setInputCloud(cloud.makeShared());

		pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
		// 5cm (small values may cause objects to be divided
		// in several clusters, whereas big values may join objects in a same cluster).
		clustering.setClusterTolerance(0.05);
		clustering.setMinClusterSize(50);
		clustering.setMaxClusterSize(20000);
		clustering.setSearchMethod(kdtree);
		clustering.setInputCloud(cloud.makeShared());
		std::vector<pcl::PointIndices> clusters;
		clustering.extract(clusters);

		std::cout << "start clustering, cloud size, cluster size: " << cloud.size() << " " << clusters.size() << std::endl;
		// For every cluster...
		int currentClusterNum = 1;
		for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
		{
			pcl::PointXYZRGB center_point;
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
			for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			{
				cluster->points.push_back(cloud.points[*point]);
				center_point.x += cloud.points[*point].x;
				center_point.y += cloud.points[*point].y;
				center_point.z += cloud.points[*point].z;
			}
			cluster->width = cluster->points.size();
			cluster->height = 1;
			cluster->is_dense = true;
			// find center
			center_point.x /= cluster->size();
			center_point.y /= cluster->size();
			center_point.z /= cluster->size();
			// calc square error
			pcl::PointXYZ sqe;
			for (size_t i = 0; i < cluster->size(); i++)
			{
				sqe.x += pow(center_point.x - cluster->points[i].x, 2);
				sqe.y += pow(center_point.y - cluster->points[i].y, 2);
				sqe.z += pow(center_point.z - cluster->points[i].z, 2);
			}
			sqe.x /= cluster->size();
			sqe.y /= cluster->size();
			sqe.z /= cluster->size();
			// currently don't limit the tube to places where has distance in y axis with pole center less than 0.2m
			std::cout << "sqe x,y,z, dist: " << sqe.x << " " << sqe.y << " " << sqe.z << " " << fabs(center_point.y - pole_pos.y) << std::endl;
			if (sqe.x > sqe.y && sqe.x > sqe.z && fabs(center_point.y - pole_pos.y) < 0.2)
			{
				tube_cloud.insert(tube_cloud.end(), cluster->begin(), cluster->end());
				// // 保存聚类点云，检查用
				// std::string fileName = "cluster/curv_cluster" + boost::to_string(currentClusterNum) + ".txt";
				// save_cloud_txt(fileName, cluster);
				// std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
			}
			else
			{
				rest_cloud.insert(rest_cloud.end(), cluster->begin(), cluster->end());
			}
			currentClusterNum++;
		}
		// rest_cloud.insert(rest_cloud.end(), sharp_point.begin(), sharp_point.end());
		// m_origin_cloud_rgb = rest_cloud;
		pcl::copyPointCloud(rest_cloud, m_rest_cloud);
		if (tube_cloud.size() > 0)
		{
			save_cloud_txt("cluster/tube.txt", tube_cloud.makeShared());
		}
		save_cloud_txt("cluster/rest.txt", rest_cloud.makeShared());
		std::cout << "clustering finished." << std::endl;
	}

	// construct kdtree, calc the multiplication between nearest points' normal within the radius
	bool normalClustering(double radius, double ratio = 0.7)
	{
		std::cout << "******* normal clustering process *******" << std::endl;
		pcl::PointCloud<pcl::PointXYZI> norm;
		std::cout << "now, norm size:" << m_norm.size() << std::endl;
		for (int i = 0; i < m_norm.size(); i++)
		{
			if (pcl::isFinite(m_norm[i]))
			{
				pcl::PointXYZI point(m_norm[i].curvature);
				point.x = m_norm[i].normal_x;
				point.y = m_norm[i].normal_y;
				point.z = m_norm[i].normal_z;
				norm.push_back(point);
			}
			else
			{
				std::cout << "find nan normal " << i << " " << m_norm[i] << std::endl;
			}
		}

		// Neighbors within radius search
		pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;

		kdtree.setInputCloud(m_origin_cloud_rgb.makeShared());

		pcl::PointXYZRGB searchPoint;
		std::vector<int> pointIdxRadiusSearch;
		std::vector<float> pointRadiusSquaredDistance;

		// float radius = 0.1;
		int remove_count = 0;
		double min_sqe = 0, max_sqe = 0;
		for (int i = 0; i < m_origin_cloud_rgb.size(); i++)
		{
			searchPoint = m_origin_cloud_rgb[i];
			// std::cout << "Neighbors within radius search at (" << searchPoint.x
			// 		  << " " << searchPoint.y
			// 		  << " " << searchPoint.z
			// 		  << ") with radius=" << radius << std::endl;

			if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
			{
				// calc square error
				double cos_avg = 0.0;
				double sqe_temp = 0.0;
				std::vector<double> cos_values;
				for (size_t j = 0; j < pointIdxRadiusSearch.size(); ++j)
				{
					double value = fabs(norm.points[i].x * norm.points[pointIdxRadiusSearch[j]].x + norm.points[i].y * norm.points[pointIdxRadiusSearch[j]].y + norm.points[i].z * norm.points[pointIdxRadiusSearch[j]].z);
					// // add curvature weights
					// value *= norm.points[i].intensity + norm.points[pointIdxRadiusSearch[j]].intensity;
					cos_values.push_back(value);
					cos_avg += value;
				}
				cos_avg /= pointIdxRadiusSearch.size();
				for (size_t j = 0; j < cos_values.size(); ++j)
				{
					double curr_sqe = pow(cos_values[j] - cos_avg, 2);
					sqe_temp += curr_sqe;
					// std::cout<<"current square error: "<<curr_sqe<<std::endl;
				}
				sqe_temp /= cos_values.size();
				// record min and max
				if (sqe_temp < min_sqe)
					min_sqe = sqe_temp;
				if (sqe_temp > max_sqe)
					max_sqe = sqe_temp;

				// std::cout<<"current square error for this point: "<<sqe_temp<<std::endl;
				if (sqe_temp > (min_sqe + (max_sqe - min_sqe) * ratio))
				{
					filtered_points.push_back(m_origin_cloud_rgb[i - remove_count]);
					m_origin_cloud_rgb.erase(m_origin_cloud_rgb.begin() + i - remove_count);
					m_norm.erase(m_norm.begin() + i - remove_count);
					remove_count++;
				}
			}
		}
		if (filtered_points.size() > 0)
		{
			save_cloud_txt("unstable_points.txt", filtered_points.makeShared());
			save_cloud_txt("stable_points.txt", m_origin_cloud_rgb.makeShared());
		}
	}

	void findVecWithCeres(pcl::PointCloud<pcl::PointXYZ> norm)
	{
		if (norm.size() <= 0)
			return;
		// if(m_norm.size()<=0){
		// 	for (int i = 0; i < norm.size(); i++)
		// 	{
		// 		m_norm.push_back(pcl::Normal(norm[i].x,norm[i].y,norm[i].z));
		// 	}
		// }
		std::vector<Eigen::Vector3d> vec;
		for (int i = 0; i < norm.size(); i++)
		{
			vec.push_back(Eigen::Vector3d(norm[i].x, norm[i].y, norm[i].z));
		}
		std::cout << "size: " << vec.size() << std::endl;

		double outputVec[3] = {0.0, 0.0, 0.0};
		ceres::Problem problem;
		// for (int i = 0; i < m_norm.size(); i++)
		// {
		// std::cout<<i<<": "<<m_norm[i].normal_x<<" "<<m_norm[i].normal_y<<std::endl;
		problem.AddResidualBlock(
			new ceres::AutoDiffCostFunction<VECTOR_FINDING_COST, ceres::DYNAMIC, 3>(
				new VECTOR_FINDING_COST(vec), vec.size()),
			nullptr,
			outputVec);
		// }
		std::cout << "start solve" << std::endl;
		//配置求解器并求解，输出结果
		ceres::Solver::Options options;
		options.linear_solver_type = ceres::DENSE_QR;
		options.minimizer_progress_to_stdout = true;
		options.parameter_tolerance = 1e-9;
		options.function_tolerance = 1e-9;
		ceres::Solver::Summary summary;
		ceres::Solve(options, &problem, &summary);
		cout << "x= " << outputVec[0] << endl;
		cout << "y= " << outputVec[1] << endl;
		cout << "z= " << outputVec[2] << endl;

		// //可视化
		// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
		// viewer.setBackgroundColor(0.0, 0.0, 0.0);
		// viewer.addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(m_origin_cloud_rgb.makeShared(), m_norm.makeShared());
		// pcl::PointXYZ start_point(0, 0, 0), end_point(outputVec[0], outputVec[1], outputVec[2]);
		// viewer.addLine(start_point, end_point);
		// while (!viewer.wasStopped())
		// {
		// 	viewer.spinOnce();
		// }
	}

	bool findVecWithCPPAD(pcl::PointCloud<pcl::PointXYZ> norm, pcl::PointXYZ &p)
	{
		std::cout << "******* viewing vector finding process *******" << std::endl;
		if (norm.size() <= 0)
			return false;
		// std::vector<Eigen::Vector3d> vec;
		std::vector<double> vec;
		int remove_count = 0;
		for (int i = 0; i < norm.size(); i++)
		{
			if (pcl::isFinite(norm[i]))
			{
				// vec.push_back(Eigen::Vector3d(norm[i].x, norm[i].y, norm[i].z));
				vec.push_back(norm[i].x);
				vec.push_back(norm[i].y);
				vec.push_back(norm[i].z);
			}
			else
			{
				// remove point with nan normal
				m_origin_cloud_rgb.erase(m_origin_cloud_rgb.begin() + i - remove_count);
				m_norm.erase(m_norm.begin() + i - remove_count);
				// std::cout<<"remove point:"<<i+1<<std::endl;
				remove_count += 1;
			}
		}
		// std::cout << "vec size: " << vec.size() << std::endl;
		// std::cout << "cloud size: " << m_origin_cloud_rgb.size() << std::endl;

		typedef CPPAD_TESTVECTOR(double) Dvector;
		size_t var_size = 3;
		size_t constrains_size = 1;
		Dvector vars(var_size);
		vars[0] = 1.0;
		vars[1] = 2.0;
		vars[2] = 3.0;

		// lower and upper limits for x
		Dvector xl(var_size), xu(var_size);
		for (size_t i = 0; i < var_size; i++)
		{
			xl[i] = -1.0;
			xu[i] = 1.0;
		}
		// lower and upper limits for g
		Dvector gl(constrains_size), gu(constrains_size);
		gl[0] = 1.0;
		gu[0] = 1.0;

		FG_eval fg_eval(vec);

		// options
		std::string options;
		// turn off any printing
		options += "Integer print_level  0\n";
		options += "String  sb           yes\n";
		// maximum number of iterations
		options += "Integer max_iter     100\n";
		// approximate accuracy in first order necessary conditions;
		// see Mathematical Programming, Volume 106, Number 1,
		// Pages 25-57, Equation (6)
		options += "Numeric tol          1e-6\n";
		// derivative testing
		options += "String  derivative_test            second-order\n";
		// maximum amount of random pertubation; e.g.,
		// when evaluation finite diff
		options += "Numeric point_perturbation_radius  0.\n";

		// place to return solution
		CppAD::ipopt::solve_result<Dvector> solution;

		// solve the problem
		CppAD::ipopt::solve<Dvector, FG_eval>(
			options, vars, xl, xu, gl, gu, fg_eval, solution);

		bool ok = solution.status == CppAD::ipopt::solve_result<Dvector>::success;
		if (ok)
		{
			// std::cout<<"vars: "<<vars[0]<<" "<<vars[1]<<" "<<vars[2]<<std::endl;
			std::cout << "viewing vector: " << solution.x[0] << " " << solution.x[1] << " " << solution.x[2] << std::endl;
			p.x = solution.x[0];
			p.y = solution.x[1];
			p.z = solution.x[2];
			return true;
		}
		return false;
	}

	// fit points to a plane given two points
	bool findPlaneWithCPPAD(std::vector<double> points, std::vector<double> vecPoints)
	{
		// // find plane and its normal vector
		// std::vector<double> points, vecPoints;
		// for (int i = 0; i < m_origin_cloud_rgb.size(); i++)
		// {
		// 	points.push_back(m_origin_cloud_rgb[i].x);
		// 	points.push_back(m_origin_cloud_rgb[i].y);
		// 	points.push_back(m_origin_cloud_rgb[i].z);
		// }
		// int index = 0;
		// std::uniform_int_distribution<int> u_a(0, m_origin_cloud_rgb.size() - 1);
		// index = u_a(rnd) * 3;
		// index = u_a(rnd) * 3;
		// std::cout << "chosen point is: " << index << std::endl;
		// vecPoints.push_back(points[index+0]);
		// vecPoints.push_back(points[index+1]);
		// vecPoints.push_back(points[index+2]);
		// vecPoints.push_back(points[index+0] + vec_viewing_direction.x);
		// vecPoints.push_back(points[index+1] + vec_viewing_direction.y);
		// vecPoints.push_back(points[index+2] + vec_viewing_direction.z);
		// bool plane_success = findPlaneWithCPPAD(points, vecPoints);
		// if (plane_success)
		// {
		// 	//可视化
		// 	pcl::visualization::PCLVisualizer viewer("PCL Viewer");
		// 	viewer.setBackgroundColor(0.0, 0.0, 0.0);
		// 	viewer.addPointCloud<pcl::PointXYZRGB>(m_origin_cloud_rgb.makeShared());
		// 	viewer.addLine(pcl::PointXYZ(vecPoints[0], vecPoints[1], vecPoints[2]), pcl::PointXYZ(vecPoints[3], vecPoints[4], vecPoints[5]), "line1");
		// 	viewer.addLine(pcl::PointXYZ(vecPoints[0], vecPoints[1], vecPoints[2]), pcl::PointXYZ(vecPoints[0] + plane_params[0], vecPoints[1] + plane_params[1], vecPoints[2] + plane_params[2]), "line2");
		// 	// std::cout<<"normal size: "<<m_norm.size()<<std::endl;
		// 	while (!viewer.wasStopped())
		// 	{
		// 		usleep(15 * 1000);
		// 		viewer.spinOnce();
		// 	}
		// }

		if (vecPoints.size() != 6)
			return false;
		if (points.size() <= 0)
			return false;

		typedef CPPAD_TESTVECTOR(double) Dvector;
		size_t var_size = 4;
		size_t constrains_size = 3;
		Dvector vars(var_size);
		vars[0] = 1.0;
		vars[1] = 2.0;
		vars[2] = 3.0;
		vars[3] = 4.0;

		// lower and upper limits for x
		Dvector xl(var_size), xu(var_size);
		for (size_t i = 0; i < var_size; i++)
		{
			xl[i] = -1e5;
			xu[i] = 1e5;
		}
		// lower and upper limits for g
		Dvector gl(constrains_size), gu(constrains_size);
		gl[0] = 1e-10;
		gu[0] = 1e1;
		gl[1] = 0.0;
		gu[1] = 0.0;
		gl[2] = 0.0;
		gu[2] = 0.0;

		FG_eval_plane fg_eval_plane(points, vecPoints);

		// options
		std::string options;
		// turn off any printing
		// options += "Integer print_level  0\n";
		options += "String  sb           yes\n";
		// maximum number of iterations
		options += "Integer max_iter     100\n";
		// approximate accuracy in first order necessary conditions;
		// see Mathematical Programming, Volume 106, Number 1,
		// Pages 25-57, Equation (6)
		options += "Numeric tol          1e-6\n";
		// derivative testing
		options += "String  derivative_test            second-order\n";
		// maximum amount of random pertubation; e.g.,
		// when evaluation finite diff
		options += "Numeric point_perturbation_radius  0.\n";

		// place to return solution
		CppAD::ipopt::solve_result<Dvector> solution;

		// solve the problem
		CppAD::ipopt::solve<Dvector, FG_eval_plane>(
			options, vars, xl, xu, gl, gu, fg_eval_plane, solution);

		bool ok = solution.status == CppAD::ipopt::solve_result<Dvector>::success;
		if (ok)
		{
			// std::cout<<"vars: "<<vars[0]<<" "<<vars[1]<<" "<<vars[2]<<std::endl;
			std::cout << "vars: " << solution.x[0] << " " << solution.x[1] << " " << solution.x[2] << " " << solution.x[3] << std::endl;
			plane_params.clear();
			plane_params.push_back(solution.x[0]);
			plane_params.push_back(solution.x[1]);
			plane_params.push_back(solution.x[2]);
			plane_params.push_back(solution.x[3]);
			return true;
		}
		return false;
	}

	// find tube structure (z value and moved c value) given two points
	bool findTubesWithCPPAD(std::vector<double> points, std::vector<double> vecPoints)
	{
		if (vecPoints.size() != 6)
			return false;
		if (points.size() <= 0)
			return false;

		typedef CPPAD_TESTVECTOR(double) Dvector;
		size_t var_size = 4;
		size_t constrains_size = 3;
		Dvector vars(var_size);
		vars[0] = 1.0;
		vars[1] = 2.0;
		vars[2] = 3.0;
		vars[3] = 4.0;

		// lower and upper limits for x
		Dvector xl(var_size), xu(var_size);
		for (size_t i = 0; i < var_size; i++)
		{
			xl[i] = -1e5;
			xu[i] = 1e5;
		}
		// lower and upper limits for g
		Dvector gl(constrains_size), gu(constrains_size);
		gl[0] = 1e-10;
		gu[0] = 1e1;
		gl[1] = 0.0;
		gu[1] = 0.0;
		gl[2] = 0.0;
		gu[2] = 0.0;

		FG_eval_plane fg_eval_plane(points, vecPoints);

		// options
		std::string options;
		// turn off any printing
		// options += "Integer print_level  0\n";
		options += "String  sb           yes\n";
		// maximum number of iterations
		options += "Integer max_iter     100\n";
		// approximate accuracy in first order necessary conditions;
		// see Mathematical Programming, Volume 106, Number 1,
		// Pages 25-57, Equation (6)
		options += "Numeric tol          1e-6\n";
		// derivative testing
		options += "String  derivative_test            second-order\n";
		// maximum amount of random pertubation; e.g.,
		// when evaluation finite diff
		options += "Numeric point_perturbation_radius  0.\n";

		// place to return solution
		CppAD::ipopt::solve_result<Dvector> solution;

		// solve the problem
		CppAD::ipopt::solve<Dvector, FG_eval_plane>(
			options, vars, xl, xu, gl, gu, fg_eval_plane, solution);

		bool ok = solution.status == CppAD::ipopt::solve_result<Dvector>::success;
		if (ok)
		{
			// std::cout<<"vars: "<<vars[0]<<" "<<vars[1]<<" "<<vars[2]<<std::endl;
			std::cout << "vars: " << solution.x[0] << " " << solution.x[1] << " " << solution.x[2] << " " << solution.x[3] << std::endl;
			return true;
		}
		return false;
	}

	// pcl euclidean clustering
	void cloud_clustering(std::string volumeName)
	{

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		if (m_total_points.size() <= 0)
		{
			std::cout << "no point for clustering" << std::endl;
			return;
		}
		else
		{
			for (size_t i = 0; i < m_total_points.size() - 1; i++)
			{
				cloud->push_back(m_total_points[i]);
			}
		}

		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
		kdtree->setInputCloud(cloud);

		pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
		// 2cm (small values may cause objects to be divided
		// in several clusters, whereas big values may join objects in a same cluster).
		clustering.setClusterTolerance(0.02);
		clustering.setMinClusterSize(100);
		clustering.setMaxClusterSize(50000);
		clustering.setSearchMethod(kdtree);
		clustering.setInputCloud(cloud);
		std::vector<pcl::PointIndices> clusters;
		clustering.extract(clusters);

		// For every cluster...
		int currentClusterNum = 1;
		for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
			for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
				cluster->points.push_back(cloud->points[*point]);
			cluster->width = cluster->points.size();
			cluster->height = 1;
			cluster->is_dense = true;

			std::string fileName = "cluster/" + volumeName + "_cluster" + boost::to_string(currentClusterNum) + ".txt";
			save_cloud_txt(fileName, cluster);

			//if (cluster->points.size() <= 0)
			//	break;
			std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
			//std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
			//pcl::io::savePCDFileASCII(fileName, *cluster);

			currentClusterNum++;
		}
	}
	void cloud_clustering_rgb(std::string volumeName)
	{

		pcl::PointCloud<pcl::PointXYZ> cloud;
		if (m_origin_cloud_rgb.size() <= 0)
		{
			std::cout << "no point for clustering" << std::endl;
			return;
		}
		// outlier removal
		pcl::PointCloud<pcl::PointXYZRGB> out;
		outlierRemoval(m_origin_cloud_rgb, out);
		pcl::copyPointCloud(out, cloud);
		for (int i = 0; i < cloud.size(); i++)
		{
			cloud[i].x *= 0.001;
			cloud[i].y *= 0.001;
			cloud[i].z *= 0.001;
		}

		// else
		// {
		// 	for (size_t i = 0; i < m_total_points.size() - 1; i++)
		// 	{
		// 		cloud->push_back(m_total_points_rgb[i]);
		// 	}
		// }

		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
		kdtree->setInputCloud(cloud.makeShared());

		pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
		// 2cm (small values may cause objects to be divided
		// in several clusters, whereas big values may join objects in a same cluster).
		clustering.setClusterTolerance(0.02);
		clustering.setMinClusterSize(30);
		clustering.setMaxClusterSize(5000);
		clustering.setSearchMethod(kdtree);
		clustering.setInputCloud(cloud.makeShared());
		std::vector<pcl::PointIndices> clusters;
		clustering.extract(clusters);

		std::cout << "start clustering: " << cloud.size() << " " << clusters.size() << std::endl;
		// For every cluster...
		int currentClusterNum = 1;
		for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
		{
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
			for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
				cluster->points.push_back(m_origin_cloud_rgb.points[*point]);
			cluster->width = cluster->points.size();
			cluster->height = 1;
			cluster->is_dense = true;

			std::string fileName = "cluster/" + volumeName + "_cluster" + boost::to_string(currentClusterNum) + ".txt";
			save_cloud_txt(fileName, cluster);

			//if (cluster->points.size() <= 0)
			//	break;
			std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
			//std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
			//pcl::io::savePCDFileASCII(fileName, *cluster);

			currentClusterNum++;
		}
		std::cout << "clustering finished." << std::endl;
	}

	cv::Mat ProSegImage(std::vector<pcl::PointXYZ> &data, pcl::PointXYZ min_p, pcl::PointXYZ max_p)
	{
		int width = max_p.x - min_p.x + 1;
		int height = max_p.y - min_p.y + 1;
		float max_z = max_p.z - 200;

		cv::Mat upImg(cv::Size(width, height), CV_8UC1, cv::Scalar(0));
		for (int i = 0; i < data.size(); ++i)
		{
			if (data[i].z > max_z)
			{
				int x = int(data[i].x - min_p.x);
				int y = int(data[i].y - min_p.y);
				upImg.ptr<uchar>(y)[x] = 255;
			}
		}

		cv::Mat element = getStructuringElement(cv::MORPH_RECT, cv::Size(15, 15));
		cv::Mat closedImg;
		cv::morphologyEx(upImg, closedImg, cv::MORPH_CLOSE, element);

		cv::Mat showImg;
		cv::cvtColor(closedImg, showImg, CV_GRAY2BGR);

		std::vector<cv::Point> cont;
		std::vector<std::vector<cv::Point>> contours;
		cv::findContours(closedImg, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
		for (int i = 0; i < contours.size(); ++i)
		{
			if (contours[i].size() > 100)
			{
				cont.insert(cont.end(), contours[i].begin(), contours[i].end());
			}
		}

		cv::RotatedRect rect = cv::minAreaRect(cv::Mat(cont));
		//cout << "angle = " << rect.angle << endl;
		cv::Point2f pts[4];
		rect.points(pts);
		if (rect.size.width < rect.size.height)
			rect.angle += 90;

		int minID[2];
		int minIndex = 0;
		for (int j = 0; j < 4; j++)
		{
			if (pts[j].x < 500)
			{
				minID[minIndex] = j;
				++minIndex;
			}
			line(showImg, pts[j], pts[(j + 1) % 4], cv::Scalar(0, 255, 0), 2, 8);
		}

		cv::Point2f pt;
		if (pts[minID[0]].y < pts[minID[1]].y)
		{
			pt.x = pts[minID[0]].x;
			pt.y = pts[minID[0]].y;
		}
		else
		{
			pt.x = pts[minID[1]].x;
			pt.y = pts[minID[1]].y;
		}

		// ÇóÖ±Ïß·œ³Ì£º y = kx + b
		float angle = CV_PI / 180.0 * rect.angle;
		float k = tan(angle);
		float b = pt.y - k * pt.x - 300; // pt.y - k * pt.x
		int y1 = k * 0 + b;
		int y2 = k * (width - 1) + b;
		std::vector<std::vector<cv::Point>> cornerPTs(1);
		cornerPTs[0].resize(4);
		cornerPTs[0][0].x = 0;
		cornerPTs[0][0].y = 0;
		cornerPTs[0][1].x = width - 1;
		cornerPTs[0][1].y = 0;
		cornerPTs[0][2].x = width - 1;
		cornerPTs[0][2].y = y2;
		cornerPTs[0][3].x = 0;
		cornerPTs[0][3].y = y1;
		cv::line(showImg, cornerPTs[0][2], cornerPTs[0][3], cv::Scalar(0, 0, 255), 2, 8);

		cv::Mat maskImg(cv::Size(width, height), CV_8UC1, cv::Scalar(0));
		cv::drawContours(maskImg, cornerPTs, -1, cv::Scalar(255), -1, 8);

		return maskImg;
	}

	// save points to file
	void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloud)
	{
		std::ofstream os;
		os.open(txt, std::ios::out);
		for (int i = 0; i < pCloud->points.size(); i++)
		{
			pcl::PointXYZRGB point = pCloud->points[i];
			char buf[255];
			memset(buf, 0, 255);
			sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
			os.write(buf, strlen(buf));
		}
		os.close();
	}
	// save points to file
	void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
	{
		std::ofstream os;
		os.open(txt, std::ios::out);
		for (int i = 0; i < pCloud->points.size(); i++)
		{
			pcl::PointXYZ point = pCloud->points[i];
			char buf[255];
			memset(buf, 0, 255);
			sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
			os.write(buf, strlen(buf));
		}
		os.close();
	}

public:
	std::vector<pcl::PointXYZ> m_total_points; // origin data points
	pcl::PointCloud<pcl::PointXYZ> m_origin_cloud;
	std::vector<pcl::PointXYZRGB> m_total_points_rgb; // origin data points
	pcl::PointCloud<pcl::PointXYZRGB> m_origin_cloud_rgb;
	pcl::PointCloud<pcl::PointXYZRGB> m_rest_cloud;
	pcl::PointCloud<pcl::PointXYZ> m_denoised_cloud;

	pcl::PointCloud<pcl::Normal> m_norm;
	pcl::PointXYZ vec_viewing_direction, pole_pos, vec_tube_direction;
	std::vector<double> plane_params;

	pcl::PointCloud<pcl::PointXYZRGB> filtered_points; // points with unstable normal vectors
	cv::Mat image;

	std::default_random_engine rnd;
	// std::uniform_real_distribution<double> u;
	// std::uniform_int_distribution<int> u2;
	pcl::PointCloud<pcl::PointXYZ> m_pole;

	double pixel_width, pixel_height;
};

#endif
