puai_wj_2021/locate/point_sift_segmentation.cpp

#include "point_sift_segmentation.h"
#include <glog/logging.h>
#include <fstream>

#include <algorithm>
#include <pcl/filters//voxel_grid.h>
#include <pcl/filters/passthrough.h>

#include <time.h>
#include <sys/time.h>
#include <chrono>
#include <thread>

void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& cloud)
{
	std::ofstream os;
	os.open(txt, std::ios::out);
	for (int i = 0; i < cloud.points.size(); i++)
	{
		pcl::PointXYZRGB point = cloud.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();
}

void save_rgb_cloud_txt(std::string txt, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
{
	std::ofstream os;
	os.open(txt, std::ios::out);
	for (int j = 0; j < cloud_vector.size(); ++j)
	{
		for (int i = 0; i < cloud_vector[j]->points.size(); i++)
		{
			pcl::PointXYZRGB point = cloud_vector[j]->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();
}



Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float freq, pcl::PointXYZ minp, pcl::PointXYZ maxp)
:PointSifter(point_size, cls)
{
	m_point_num = point_size;
	m_cls_num = cls;
	m_freq = freq;
	m_minp = minp;
	m_maxp = maxp;
}
Point_sift_segmentation::Point_sift_segmentation(int point_size,int cls,float freq, Cloud_box cloud_box)
:PointSifter(point_size, cls)
{
	m_point_num = point_size;
	m_cls_num = cls;
	m_freq = freq;
	m_cloud_box_limit = cloud_box;
}

Point_sift_segmentation::~Point_sift_segmentation()
{
}





Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
{
	if (!Load(graph, cpkt))
	{
		std::string error_string="pointSIFT Init ERROR:";
		error_string+=LastError();
		return Error_manager(LOCATER_SIFT_INIT_FAILED,MINOR_ERROR,error_string);
	}

	//创建空数据,第一次初始化后空跑
	float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
	float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));

	auto start   = std::chrono::system_clock::now();

	if (false == Predict(cloud_data, output))
	{
		free(cloud_data);
		free(output);
		std::string error_string="pointSIFT int first predict ERROR:";
		error_string+=LastError();
		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,error_string);
	}
	else
	{
		free(cloud_data);
		free(output);

	}
	auto end   = std::chrono::system_clock::now();
	auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
	std::cout <<  "花费了"
		 << double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "秒" << std::endl;


	return SUCCESS;
}



//分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
//cloud:输入点云
//clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
Error_manager Point_sift_segmentation::segmentation(int lidar_id, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
													std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
													bool save_flag, std::string save_dir)
{
	if(p_cloud_in.get()==NULL)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
	}
	if(p_cloud_in->size()<=0)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
	}

	Error_manager t_error;

	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
	//第一步
	// 使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
	t_error = filter_cloud_with_voxel_grid(p_cloud_in);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}
	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;

	//第二步
	// 使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
	t_error = filter_cloud_with_box(p_cloud_in);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}
	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;

	//第三步 /
	// /通过输入点云,更新区域范围, 范围不一定是box的边界,可能比box要小.
	t_error = update_region(p_cloud_in);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}
	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;

	//第四步
	// 抽稀点云,	//自己写的算法, 重新生成新的点云.
	t_error = filter_cloud_with_my_grid(p_cloud_in);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}
	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;

	//第五步
	pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_select(new pcl::PointCloud<pcl::PointXYZ>);
	//从点云中选出固定数量的随机点, 默认8192个点
	t_error = filter_cloud_with_select(p_cloud_in, p_cloud_select);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}
	std::cout << "p_cloud_select = "<<p_cloud_select->size() << std::endl;




	/*
	//第六步
	//把pcl点云转化为float, 因为TensorFlow算法只能识别float
	//提前分配内存, 后续记得回收....
	float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float));				//8192*3 float, 8192个三维点
	float* p_data_type = (float*)malloc(m_point_num*m_cls_num*sizeof(float));		//8192*2 float, 8192个类别百分比(车轮和车身2类)
	memset(p_data_point, 0, m_point_num * 3 * sizeof(float));
	memset(p_data_type, 0, m_point_num*m_cls_num * sizeof(float));
	//将点云数据转换到float*
	t_error = translate_cloud_to_float(p_cloud_select, p_data_point);
	if ( t_error != Error_code::SUCCESS )
	{
		free(p_data_point);
		free(p_data_type);
		return t_error;
	}

	//第七步
	//tensonflow预测点云, 计算出每个三维点的类别百分比,
	auto start = system_clock::now();
	if (!Predict(p_data_point, p_data_type))
	{
		free(p_data_point);
		free(p_data_type);
		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,"pointSIFT predict ERROR");
	}
	auto end   = system_clock::now();
	auto duration = duration_cast<microseconds>(end - start);
	cout <<  "花费了"
		 << double(duration.count()) * microseconds::period::num / microseconds::period::den   << "秒" << endl;
	cout <<  double(duration.count()) << " "
		 <<   microseconds::period::num << " " << microseconds::period::den   << "秒" << endl;
*/

	//重写6和7步, 将TensorFlow算法放在服务器上, 使用socket通信去发送点云并获取结果
	//第六步
	//把pcl点云转化为float, 因为TensorFlow算法只能识别float
	//提前分配内存, 后续记得回收....
	float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float));				//8192*3 float, 8192个三维点
	int* p_data_type = (int*)malloc(m_point_num*sizeof(int));		//8192*2 float, 8192个类别百分比(车轮和车身2类)
	memset(p_data_point, 0, m_point_num * 3 * sizeof(float));
	memset(p_data_type, 0, m_point_num * sizeof(int));
	//将点云数据转换到float*
	t_error = translate_cloud_to_float(p_cloud_select, p_data_point);
	if ( t_error != Error_code::SUCCESS )
	{
		free(p_data_point);
		free(p_data_type);
		return t_error;
	}

	//第七步
	//tensonflow预测点云, 计算出每个三维点的类别百分比,
	auto start = std::chrono::system_clock::now();
	//预测, 发送socket通信, 在服务器上进行点云算法
	t_error = m_predict_with_network.predict_for_send(lidar_id, p_data_point);
	if ( t_error != Error_code::SUCCESS )
	{
		free(p_data_point);
		free(p_data_type);
		return t_error;
	}

	//循环接受, 超时等待5000ms, (后续可以在这里加上强制取消)
	while (std::chrono::system_clock::now() - start < std::chrono::milliseconds(5000))
	{
		t_error = m_predict_with_network.predict_for_receive(lidar_id, p_data_type);
		if ( t_error == Error_code::SUCCESS )
		{
			//算法成功, 直接退出循环即可
			break;
		}
		else if ( t_error == Error_code::NODATA )
		{
			//继续循环, 延迟1us
			std::this_thread::yield();
			std::this_thread::sleep_for(std::chrono::microseconds(1));
			continue;
		}
		else
		{
			free(p_data_point);
			free(p_data_type);
			return t_error;
		}
	}
	if ( std::chrono::system_clock::now() - start >= std::chrono::milliseconds(5000) )
	{
		free(p_data_point);
		free(p_data_type);
		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_TIMEOUT, Error_level::MINOR_ERROR,
							 " m_predict_with_network.predict_for_receive error ");
	}


	auto end   = std::chrono::system_clock::now();
	auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
	std::cout <<  "花费了"
		 << double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "秒" << std::endl;
	std::cout <<  double(duration.count()) << " "
		 <<   std::chrono::microseconds::period::num << " " << std::chrono::microseconds::period::den   << "秒" << std::endl;



	//第八步
	//恢复点云,并填充到cloud_vector
	t_error = recovery_float_to_cloud(p_data_type, p_data_point, cloud_vector);
	if ( t_error != Error_code::SUCCESS )
	{
		free(p_data_point);
		free(p_data_type);
		return t_error;
	}

	//第九步
	//保存点云数据
	if ( save_flag )
	{
		t_error = save_cloud(cloud_vector,save_dir);
		if ( t_error != Error_code::SUCCESS )
		{
			free(p_data_point);
			free(p_data_type);
			return t_error;
		}
	}


	//第十步
	free(p_data_point);
	free(p_data_type);

	return Error_code::SUCCESS;
}


Predict_with_network& Point_sift_segmentation::get_predict_with_network()
{
	return m_predict_with_network;
}


//使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
Error_manager Point_sift_segmentation::filter_cloud_with_voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
{
	if(p_cloud.get()==0)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
	}
	if(p_cloud->size()<=0)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
	}
	//体素网格 下采样
	pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
	vgfilter.setInputCloud(p_cloud);
	vgfilter.setLeafSize(0.02f, 0.02f, 0.02f);
	vgfilter.filter(*p_cloud);
	return Error_code::SUCCESS;
}
//使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
Error_manager Point_sift_segmentation::filter_cloud_with_box(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
{
	if(p_cloud.get()==0)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
	}
	if(p_cloud->size()<=0)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
	}
	//限制 x y
	pcl::PassThrough<pcl::PointXYZ> passX;
	pcl::PassThrough<pcl::PointXYZ> passY;
	pcl::PassThrough<pcl::PointXYZ> passZ;
	passX.setInputCloud(p_cloud);
	passX.setFilterFieldName("x");
	passX.setFilterLimits(m_cloud_box_limit.x_min, m_cloud_box_limit.x_max);
	passX.filter(*p_cloud);

	passY.setInputCloud(p_cloud);
	passY.setFilterFieldName("y");
	passY.setFilterLimits(m_cloud_box_limit.y_min, m_cloud_box_limit.y_max);
	passY.filter(*p_cloud);

	passZ.setInputCloud(p_cloud);
	passZ.setFilterFieldName("z");
	passZ.setFilterLimits(m_cloud_box_limit.z_min, m_cloud_box_limit.z_max);
	passZ.filter(*p_cloud);
	return Error_code::SUCCESS;
}
//通过输入点云,更新边界区域范围
Error_manager Point_sift_segmentation::update_region(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
{
	pcl::PointXYZ min_point,max_point;
	pcl::getMinMax3D(*p_cloud,min_point,max_point);

	if(max_point.x<=min_point.x || max_point.y<=min_point.y)
	{
		return Error_manager(LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,MINOR_ERROR,
							 "Point sift update_region errror :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y ");
	}
	m_minp = min_point;
	m_maxp = max_point;
	return Error_code::SUCCESS;
}
//抽稀点云,	//自己写的算法, 重新生成新的点云.
Error_manager Point_sift_segmentation::filter_cloud_with_my_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
{

	if(p_cloud.get()==NULL)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
	}
	if(p_cloud->size()<=0)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
	}
	Error_manager t_error;

	//可以按照输入点云的数量, 然后动态修改 m_freq , 让其在抽稀之后,点云数量接近8192
	//以后在写.

	//按照m_freq的分辨率, 将点云拆分为很多网格
	//网格大小 m_freq 的3次方, 网格总数 grid_number
	int t_grid_length = int((m_maxp.x - m_minp.x) / m_freq) + 1;
	int t_grid_width = int((m_maxp.y - m_minp.y) / m_freq) + 1;
	int t_grid_height = int((m_maxp.z - m_minp.z) / m_freq) + 1;
	int t_grid_number = t_grid_length * t_grid_width * t_grid_height;
//	std::cout << " t_grid_length = "<<t_grid_length << " t_grid_width = "<<t_grid_width<< " t_grid_height = "<<t_grid_height<< std::endl;
//	std::cout << "---------------------------"<< std::endl;

	//创建网格->三维点的map, 每个网格里面只保留一个点,
	std::map<int, pcl::PointXYZ> grid_point_map;
	//遍历输入点云, 将每个点的索引编号写入对应的网格.
	for (int i = 0; i < p_cloud->size(); ++i)
	{
		//找到对应的网格.
		pcl::PointXYZ point = p_cloud->points[i];
		int id_x = (point.x - m_minp.x) / m_freq;
		int id_y = (point.y - m_minp.y) / m_freq;
		int id_z = (point.z - m_minp.z) / m_freq;

//		std::cout << "------------point:"<< point << std::endl;
//		std::cout << "------------m_minp:"<< m_minp << std::endl;
//		std::cout << "------------m_freq:"<< m_freq << std::endl;
//		std::cout << " id_x = "<<id_x << " id_y = "<<id_y<< " id_y = "<<id_y<< std::endl;

		if (id_x < 0 || id_x >= t_grid_length || id_y < 0 || id_y >= t_grid_width || id_z < 0 || id_z >= t_grid_height)
		{
			//无效点.不要.
			continue;
		}
		grid_point_map[id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width] = point;
		//注:如果有多个点属于同一个网格, 那个后者就会覆盖前者, 最终保留一个点.
	}

	//检查map
	if ( grid_point_map.size() >0 )
	{
		//清空点云, 然后将map里面的点 重新加入p_cloud
		p_cloud->clear();
		for ( auto iter = grid_point_map.begin(); iter != grid_point_map.end(); iter++  )
		{
			p_cloud->push_back(iter->second);
		}
	}
	else
	{
		return Error_manager(Error_code::LOCATER_SIFT_GRID_ERROR, Error_level::MINOR_ERROR,
							 " filter_cloud_with_my_grid  error ");
	}

/*
	//创建网格的数据, 内存大小为t_grid_number, 储存内容为是否存在点云的标记位.
	bool* tp_grid = (bool*)malloc(t_grid_number*sizeof(bool));
	memset(tp_grid, 0, t_grid_number*sizeof(bool));
	//遍历输入点云, 将每个点映射到输入网格, 每个网格最多保留一个点,多余的删除.
	for(pcl::PointCloud<pcl::PointXYZ>::iterator iter = p_cloud->begin(); iter !=p_cloud->end(); )
	{
		//找到对应的网格.
		int id_x = (iter->x - m_minp.x) / m_freq;
		int id_y = (iter->y - m_minp.y) / m_freq;
		int id_z = (iter->z - m_minp.z) / m_freq;
		if (id_x < 0 || id_x >= t_grid_length || id_y < 0 || id_y >= t_grid_width || id_z < 0 || id_z >= t_grid_height)
		{
			//无效点, 直接删除
			iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
		}
		else if(*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) == true)
		{
			//标记位为true,就表示这个网格已经有点了, 那么直接删除后者,保留前者
			iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
		}
		else
		{
			*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) = true;
			iter++;
		}
	}
	free(tp_grid);
	*/

	return Error_code::SUCCESS;
}

//从点云中选出固定数量的随机点, 默认8192个点
Error_manager Point_sift_segmentation::filter_cloud_with_select(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
{
	if(p_cloud_in.get()==NULL || p_cloud_out.get()==NULL)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
	}
	if(p_cloud_in->size()<=0)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
	}
	Error_manager t_error;

	//从输入点云 p_cloud_in 里面选出 m_point_num 个点, 然后加入 p_cloud_out
	//输入个数大于8192
	if (p_cloud_in->size() > m_point_num)
	{
		//将点云随机打乱
		std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
		//选出输入点云前面8192个点.
		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + m_point_num);
	}
	//输入个数小鱼8192
	else if (p_cloud_in->size() < m_point_num)
	{
		int add = m_point_num - p_cloud_in->size();
		//注意了, 输入点云不能少于4096, 否则点云分布太少, 会严重影响到TensorFlow的图像识别.
		if (add > p_cloud_in->size())
		{
			return Error_manager(Error_code::LOCATER_SIFT_CLOUD_VERY_LITTLE, Error_level::MINOR_ERROR,
								" filter_cloud_with_select  p_cloud_in->size() very little ");
		}
		//将点云随机打乱
		std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
		//选出输入点云8192个点, 不够的在加一次,
		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + add);
		p_cloud_out->points.insert(p_cloud_out->points.begin(), p_cloud_in->points.begin(), p_cloud_in->points.end());
	}
	//输入个数等于8192
	else
	{
		//此时就不需要打乱了, 直接全部复制.
		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.end());
	}

	if (p_cloud_out->points.size() != m_point_num)
	{
		return Error_manager(Error_code::LOCATER_SIFT_SELECT_ERROR, Error_level::MINOR_ERROR,
							" Point_sift_segmentation::filter_cloud_with_select  error ");
	}
	return Error_code::SUCCESS;
}

//将点云数据转换到float*
Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, float* p_data_out)
{
	if(p_cloud_in.get()==NULL)
	{
		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
	}
	if(p_cloud_in->size() != m_point_num)
	{
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							" Point_sift_segmentation::translate_cloud_to_float p_cloud_in->size() error ");
	}

	if ( p_data_out == NULL )
	{
	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
	    					" translate_cloud_to_float  p_data_out POINTER_IS_NULL ");
	}

	for (int i = 0; i < m_point_num; ++i)
	{
		pcl::PointXYZ point = p_cloud_in->points[i];
		*(p_data_out + i * 3) = point.x;
		*(p_data_out + i * 3+1) = point.y;
		*(p_data_out + i * 3+2) = point.z ;
		//注:三维点云的数据 不用做缩放和平移,
		// 这个在雷达扫描时就进行了标定和转化, 雷达传输过来的坐标就已经是公共坐标系了, (单位米)
	}
	return Error_code::SUCCESS;
}

//恢复点云, 将float*转换成点云
//p_data_type:输入点云对应的类别,  大小为  点数*m_cls_num*float
//p_data_point:输入点云数据(xyz)
//cloud_vector::输出带颜色的点云vector
Error_manager Point_sift_segmentation::recovery_float_to_cloud(float* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
{
	if ( p_data_type == NULL || p_data_point == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " recovery_float_to_cloud  p_data_type or  p_data_point  POINTER_IS_NULL ");
	}

	//为cloud_vector 添加m_cls_num个点云, 提前分配内存
	for (int k = 0; k < m_cls_num; ++k)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
		cloud_vector.push_back(cloud_rgb);
	}

	//遍历data数据,
	for (int i = 0; i < m_point_num; ++i)
	{
		pcl::PointXYZRGB t_point;
		t_point.x = *(p_data_point + i * 3);
		t_point.y = *(p_data_point + i * 3 + 1);
		t_point.z = *(p_data_point + i * 3 + 2);
		if (t_point.x > m_maxp.x || t_point.x<m_minp.x
			|| t_point.y>m_maxp.y || t_point.y<m_minp.y
			|| t_point.z>m_maxp.z || t_point.z < m_minp.z)
		{
			continue;
		}

		//当前点 的类别百分比的指针
		float* tp_type_percent = p_data_type + i*m_cls_num;
		//当前点 的类别, 初始化为0
		int t_cls = 0;
		//当前点 的类别, 百分比的最大值
		float t_type_percent_max = tp_type_percent[0];
		//循环比较, 找出百分比最大的类别
		for (int j = 1; j < m_cls_num; j++) //前面已经把第0个用来初始化了, 后续从第下一个开始比较
		{
			if (tp_type_percent[j] > t_type_percent_max)
			{
				t_type_percent_max = tp_type_percent[j];
				t_cls = j;
			}
		}

		//根据点的类别, 添加颜色, 并且分别加入到各自的点云里面
		switch ( t_cls )
		{
		    case 0: //第0类, 绿色, 轮胎
				t_point.r = 0;
				t_point.g = 255;
				t_point.b = 0;
		        break;
		    case 1://第1类, 白色, 车身
				t_point.r = 255;
				t_point.g = 255;
				t_point.b = 255;
				break;
//			case 2:
//				;
//				break;
			default:

		        break;
		}
		cloud_vector[t_cls]->push_back(t_point);
	}

	//校验点云的数量, 要求size>0
	if(cloud_vector[0]->size() <=0)
	{
		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_WHEEL_POINT, Error_level::MINOR_ERROR,
							" recovery_float_to_cloud NO_WHEEL_POINT ");
	}
	if(cloud_vector[1]->size() <=0)
	{
		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_CAR_POINT, Error_level::MINOR_ERROR,
							 " recovery_float_to_cloud NO_CAR_POINT ");
	}


	return Error_code::SUCCESS;
}

//恢复点云, 将float*转换成点云
//p_data_type:输入点云对应的类别(取值 0 ~ m_cls_num-1 ),大小为  点数*int
//p_data_point:输入点云数据(xyz)
//cloud_vector::输出带颜色的点云vector
Error_manager Point_sift_segmentation::recovery_float_to_cloud(int* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
{
	if ( p_data_type == NULL || p_data_point == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " recovery_float_to_cloud  p_data_type or  p_data_point  POINTER_IS_NULL ");
	}

	//为cloud_vector 添加m_cls_num个点云, 提前分配内存
	for (int k = 0; k < m_cls_num; ++k)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
		cloud_vector.push_back(cloud_rgb);
	}

	//遍历data数据,
	for (int i = 0; i < m_point_num; ++i)
	{
		pcl::PointXYZRGB t_point;
		t_point.x = *(p_data_point + i * 3);
		t_point.y = *(p_data_point + i * 3 + 1);
		t_point.z = *(p_data_point + i * 3 + 2);
		if (t_point.x > m_maxp.x || t_point.x<m_minp.x
			|| t_point.y>m_maxp.y || t_point.y<m_minp.y
			|| t_point.z>m_maxp.z || t_point.z < m_minp.z)
		{
			continue;
		}

		//当前点 的类别, 初始化为0
		int t_cls = p_data_type[i];

		//根据点的类别, 添加颜色, 并且分别加入到各自的点云里面
		switch ( t_cls )
		{
			case 0: //第0类, 绿色, 轮胎
				t_point.r = 0;
				t_point.g = 255;
				t_point.b = 0;
				break;
			case 1://第1类, 白色, 车身
				t_point.r = 255;
				t_point.g = 255;
				t_point.b = 255;
				break;
//			case 2:
//				;
//				break;
			default:

				break;
		}
		cloud_vector[t_cls]->push_back(t_point);
	}

	//校验点云的数量, 要求size>0
	if(cloud_vector[0]->size() <=0)
	{
		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_WHEEL_POINT, Error_level::MINOR_ERROR,
							 " recovery_float_to_cloud NO_WHEEL_POINT ");
	}
	if(cloud_vector[1]->size() <=0)
	{
		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_CAR_POINT, Error_level::MINOR_ERROR,
							 " recovery_float_to_cloud NO_CAR_POINT ");
	}


	return Error_code::SUCCESS;
}



//保存点云数据
Error_manager Point_sift_segmentation::save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,std::string save_dir)
{
	if(cloud_vector.size()>0) {
		std::string sift_in = save_dir + "/SIFT_cls_0.txt";
		save_rgb_cloud_txt(sift_in, *cloud_vector[0]);
	}
	if(cloud_vector.size()>1) {
		std::string sift_in = save_dir + "/SIFT_cls_1.txt";
		save_rgb_cloud_txt(sift_in, *cloud_vector[1]);
	}

	if(cloud_vector.size()>2) {
		std::string sift_in = save_dir + "/SIFT_cls_2.txt";
		save_rgb_cloud_txt(sift_in, *cloud_vector[2]);
	}

	std::string sift_in = save_dir + "/SIFT_TOTAL.txt";
	save_rgb_cloud_txt(sift_in, cloud_vector);

	return Error_code::SUCCESS;
}