puai_wj_2021/wanji_lidar/region_detect.cpp

//
// Created by zx on 2019/12/6.
//
#include "region_detect.h"

/**
 * 有参构造函数
 * */
Region_detector::Region_detector()
{

}

/**
 * 析构函数
 * */
Region_detector::~Region_detector()
{
}

Error_manager Region_detector::init(Point2D_tool::Point2D_box point2D_box)
{
	m_region_box = point2D_box;
	return Error_code::SUCCESS;
}


/**
 * 预处理，xy直通滤波与离群点过滤
 * 返回 PARAMETER_ERROR，WJ_REGION_EMPTY_CLOUD 或 SUCCESS
 * */
Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
{
	// 1.参数合法性检验
	if (p_cloud->size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
//	std::cout<<"preprocess 000"<<std::endl;
	// 2.直通滤波, 筛选xy
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(p_cloud);
	pass.setFilterFieldName("x");                                       //设置想在哪个坐标轴上操作
	pass.setFilterLimits(m_region_box.x_min, m_region_box.x_max); //将x轴的0到1范围内
	pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
	pass.filter(*p_cloud);                                            //输出到结果指针
	if (p_cloud->size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
//    std::cout<<"preprocess 111 "<<p_cloud->size()<<std::endl;
	pass.setInputCloud(p_cloud);
	pass.setFilterFieldName("y");                                       //设置想在哪个坐标轴上操作
	pass.setFilterLimits(m_region_box.y_min, m_region_box.y_max); //将x轴的0到1范围内
	pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
	pass.filter(*p_cloud);                                            //输出到结果指针
//    std::cout << "cloud size: "<<p_cloud->size()<<","<<m_region_box.x_min<<","<<m_region_box.x_max<<","<<m_region_box.y_min<<","<<m_region_box.y_max<<std::endl;
	if (p_cloud->size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
//    std::cout<<"preprocess 222 "<<p_cloud->size()<<std::endl;
	// 3.离群点过滤
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(p_cloud);
	sor.setMeanK(15);            //K近邻搜索点个数
	sor.setStddevMulThresh(3.0); //标准差倍数
	sor.setNegative(false);      //保留未滤波点（内点）
	sor.filter(*p_cloud);      //保存滤波结果到cloud_filter
//    std::cout<<"preprocess 333 "<<p_cloud->size()<<std::endl;
	if (p_cloud->size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
	else
		return Error_manager(Error_code::SUCCESS);
}

/**
 * 返回二维点之间距离
 * */
double Region_detector::distance(cv::Point2f p1, cv::Point2f p2)
{
	return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}

/**
 * 判断是否符合标准矩形，
 * 传入矩形四个角点，以及是否打印
 * 返回 WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, 
 * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS
 * */
Error_manager Region_detector::isRect(std::vector<cv::Point2f> &points, bool print)
{
	if (points.size() == 4)
	{
		double L[3] = {0.0};
		L[0] = distance(points[0], points[1]);
		L[1] = distance(points[1], points[2]);
		L[2] = distance(points[0], points[2]);
		double max_l = L[0];
		double l1 = L[1];
		double l2 = L[2];
		int max_index = 0;
		cv::Point2f ps = points[0], pt = points[1];
		// 顶点
		cv::Point2f pc = points[2];
		for (int i = 1; i < 3; ++i)
		{
			if (L[i] > max_l)
			{
				max_index = i;
				max_l = L[i];
				l1 = L[abs(i + 1) % 3];
				l2 = L[abs(i + 2) % 3];
				ps = points[i % 3];
				pt = points[(i + 1) % 3];
				pc = points[(i + 2) % 3];
			}
		}
		// 顶角大小
		double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
		// 顶角相对于90度过大或小
		if (fabs(cosa) >= 0.15 /* || std::min(l1, l2) > 2.0 || std::max(l1, l2) > 3.3*/)
		{
			if (print)
			{
				LOG(WARNING) << " angle cos >0.13 =" << cosa << "  i=" << max_index;
				// LOG(WARNING) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
			}
			return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR);
		}

		float width = std::min(l1, l2);
		float length = std::max(l1, l2);
		// 车宽应位于[1.35, 2.0]，车长应位于[2.2, 3.0]
		if (width < 1.350 || width > 2.000 || length > 3.000 || length < 2.200)
		{
			if (print)
			{
				LOG(WARNING) << "\t width<1350 || width >2100 || length >3300 ||length < 2100 "
							 << "  length:" << length << "  width:" << width;
			}
			return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR);
		}

		double d = distance(pc, points[3]);
		cv::Point2f center1 = (ps + pt) * 0.5;
		cv::Point2f center2 = (pc + points[3]) * 0.5;
		// 对角线形变超过0.15倍，或中心点距离偏差0.15m
		if (fabs(d - max_l) > max_l * 0.15 || distance(center1, center2) > 0.150)
		{
			if (print)
			{
				LOG(WARNING) << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2
							 << "  center distance=" << distance(center1, center2);
			}
			return Error_manager(Error_code::WJ_REGION_RECTANGLE_SYMMETRY_ERROR);
		}
		if (print)
		{
			LOG(INFO) << " rectangle verify OK  cos angle=" << cosa << "  length off=" << fabs(d - max_l)
					  << "  center distance=" << distance(center1, center2);
		}
		return Error_manager(Error_code::SUCCESS);
	}
		// 以三个点进行验证
	else if (points.size() == 3)
	{
		double L[3] = {0.0};
		L[0] = distance(points[0], points[1]);
		L[1] = distance(points[1], points[2]);
		L[2] = distance(points[0], points[2]);
		double max_l = L[0];
		double l1 = L[1];
		double l2 = L[2];
		int max_index = 0;
		cv::Point2f ps = points[0], pt = points[1];
		cv::Point2f pc = points[2];
		for (int i = 1; i < 3; ++i)
		{
			if (L[i] > max_l)
			{
				max_index = i;
				max_l = L[i];
				l1 = L[abs(i + 1) % 3];
				l2 = L[abs(i + 2) % 3];
				ps = points[i % 3];
				pt = points[(i + 1) % 3];
				pc = points[(i + 2) % 3];
			}
		}
		double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
		// 顶角应接近90度
		if (fabs(cosa) >= 0.15)
		{
			if (print)
			{
				LOG(WARNING) << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
				LOG(WARNING) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
			}
			return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR);
		}

		double length = std::max(l1, l2);
		double width = std::min(l1, l2);
		// 车宽应位于[1.4, 2.0]，车长应位于[2.2, 3.0]
		if (length > 2.100 && length < 3.000 && width > 1.400 && width < 2.100)
		{
			//生成第四个点
			cv::Point2f vec1 = ps - pc;
			cv::Point2f vec2 = pt - pc;
			cv::Point2f point4 = (vec1 + vec2) + pc;
			points.push_back(point4);
			if (print)
			{
				LOG(WARNING) << "3 wheels rectangle verify OK  cos angle=" << cosa << "  L=" << length
							 << "  w=" << width;
			}
			return Error_manager(Error_code::SUCCESS);
		}
		else
		{
			if (print)
			{
				LOG(WARNING) << "3 wheels rectangle verify Failed  cos angle=" << cosa << "  L=" << length
							 << "  w=" << width;
			}
			return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR);
		}
	}
	else
	{
		return Error_manager(Error_code::WJ_REGION_CLUSTER_SIZE_ERROR);
	}
}

/*
 * 仅仅聚类
 */
Error_manager Region_detector::clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
											   std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print)
{
	// 1.判断参数合法性
	if (cloud_in->size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

	// 2.聚类
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(cloud_in);

	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.5);
	// 设置聚类的小点数和最大点云数
	clustering.setMinClusterSize(10);
	clustering.setMaxClusterSize(500);
	clustering.setSearchMethod(kdtree);
	clustering.setInputCloud(cloud_in);
	std::vector<pcl::PointIndices> clusters;
	clustering.extract(clusters);
	if(clusters.size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

	for (int i = 0; i < clusters.size(); ++i)
	{
		seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>());
	}

	int j = 0;
	for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		//创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
		{
			cloud_cluster->points.push_back(cloud_in->points[*pit]);
			cloud_cluster->width = cloud_cluster->points.size();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;
		}
		seg_clouds[j] = *cloud_cluster;
		j++;
	}

	if (print)
	{
		LOG(INFO) << " cluster classes:" << clusters.size();
	}

	return SUCCESS;
}


/**
 * 聚类并通过矩形判断函数
 * 传入原始点云，传出角点与聚类出的点云
 * 返回 WJ_REGION_EMPTY_CLOUD, WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, 
 * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS
 * */
Error_manager Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f> &corner_points, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print)
{
	// 1.判断参数合法性
	if (cloud_in->size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

	// 2.聚类
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(cloud_in);

	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.2);
	// 设置聚类的小点数和最大点云数
	clustering.setMinClusterSize(10);
	clustering.setMaxClusterSize(500);
	clustering.setSearchMethod(kdtree);
	clustering.setInputCloud(cloud_in);
	std::vector<pcl::PointIndices> clusters;
	clustering.extract(clusters);
	if(clusters.size() <= 0)
		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

	for (int i = 0; i < clusters.size(); ++i)
	{
		seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
	}

	int j = 0;
	for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		//创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
		{
			cloud_cluster->points.push_back(cloud_in->points[*pit]);
			cloud_cluster->width = cloud_cluster->points.size();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;
		}
		seg_clouds[j] = cloud_cluster;
		j++;
	}

	if (print)
	{
		LOG(INFO) << " cluster classes:" << clusters.size();
	}

	// 3.分别计算每团点云几何中心，作为矩形角点
	corner_points.clear();
	for (int i = 0; i < clusters.size(); ++i)
	{
		if (seg_clouds.size() <= i || seg_clouds[i]->size() == 0)
			continue;
		float sumX = 0, sumY = 0;
		for (int j = 0; j < seg_clouds[i]->size(); ++j)
		{
			sumX += seg_clouds[i]->points[j].x;
			sumY += seg_clouds[i]->points[j].y;
		}

		float center_x = sumX / float(seg_clouds[i]->size());
		float center_y = sumY / float(seg_clouds[i]->size());
		corner_points.push_back(cv::Point2f(center_x, center_y));
	}
	/*char buf[255]={0};
	for (int k = 0; k < corner_points.size(); ++k) {
		sprintf(buf+strlen(buf), "point %d: (%.2f, %.2f)__", k, corner_points[k].x, corner_points[k].y);
	}
	LOG(WARNING) << buf;*/

	cv::RotatedRect t_rect=cv::minAreaRect(corner_points);
	//display(t_rect,cv::Scalar(255,0,0));
	return isRect(corner_points, print);
}




// 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
Error_manager Region_detector::detect(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
									  Common_data::Car_wheel_information& car_wheel_information, bool print)
{
	//整个函数都加锁, 这里面非法修改了输入点云, 是为了将筛选点云的结构传出, 然后点云存txt文件
	std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
	Error_manager result = Error_manager(Error_code::SUCCESS);
	pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	*p_cloud_filtered = *p_cloud_in;

	// 1.预处理
	result = preprocess(p_cloud_filtered);
	if (result != SUCCESS)
		return result;

	// 2.聚类计算角点
	std::vector<cv::Point2f> corner_points;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
	result = clustering(p_cloud_filtered, corner_points, seg_clouds, print);
	if (result != SUCCESS)
		return result;
	// 修改原始点云为预处理后点云，并加入角点供查验

	p_cloud_in->clear();
	p_cloud_in->operator+=(*p_cloud_filtered);
	for (size_t i = 0; i < corner_points.size(); i++)
	{
		p_cloud_in->points.push_back(pcl::PointXYZ(corner_points[i].x, corner_points[i].y, 0.0));
	}

	// convert all points after preprocessed into 2d
	std::vector<cv::Point2f> all_points;
	// car center
	for (int j = 0; j < p_cloud_filtered->size(); ++j)
	{
		all_points.push_back(cv::Point2f(p_cloud_filtered->points[j].x, p_cloud_filtered->points[j].y));
	}
	// find bounding rectangle of all wheel points
	cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
	car_wheel_information.car_center_x = wheel_box.center.x;
	car_wheel_information.car_center_y = wheel_box.center.y;
	// add center point to the input cloud for further check
	p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.car_center_x, car_wheel_information.car_center_y, 0.0));



	// 长边向量
	cv::Point2f vec;
	cv::Point2f vertice[4];
	wheel_box.points(vertice);

	float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
	float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
	// 寻找长边，倾角为长边与x轴夹角
	if (len1 > len2)
	{
		vec.x = vertice[0].x - vertice[1].x;
		vec.y = vertice[0].y - vertice[1].y;
	}
	else
	{
		vec.x = vertice[1].x - vertice[2].x;
		vec.y = vertice[1].y - vertice[2].y;
	}
	float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
	car_wheel_information.car_angle = angle_x;

	// get line formula, normal is (cos(theta), sin(theta)), towards the long side
	// the line formula is: nx * x + ny * y + (-(nx*cx+ny*cy)) = 0
	Eigen::Vector2f normal, center_point;
	normal << vec.x / sqrt(vec.x * vec.x + vec.y * vec.y), vec.y / sqrt(vec.x * vec.x + vec.y * vec.y);
	center_point << car_wheel_information.car_center_x, car_wheel_information.car_center_y;
	float line_param_c = -1 * center_point.transpose() * normal;
	// get rotation matrix, get the angle towards normal vector, rather than x axis
	// R = [ cos -sin]
	//     [ sin  cos]
	float rotate_angle = M_PI_2 - acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
	Eigen::Matrix2f rotation_matrix;
	rotation_matrix << cos(rotate_angle), -sin(rotate_angle), sin(rotate_angle), cos(rotate_angle);

	// find min x and max x, separate y values according to the line, calculate difference between mean of y values as wheelbase
	float min_x = 20, max_x = 0;
	float y_values0 = 0, y_values1 = 0;
	int count0 = 0, count1 = 0;
	for (size_t i = 0; i < seg_clouds.size(); i++)
	{
		if (seg_clouds[i]->size() <= 0)
			continue;
		for (size_t j = 0; j < seg_clouds[i]->size(); j++)
		{
			// origin point and the point rotated around the origin
			Eigen::Vector2f vec, vec_rot;
			vec << seg_clouds[i]->points[j].x, seg_clouds[i]->points[j].y;
			vec_rot = rotation_matrix * (vec - center_point) + center_point;
			// find min max x
			if (vec_rot[0] < min_x)
				min_x = vec_rot[0];
			if (vec_rot[0] > max_x)
				max_x = vec_rot[0];
			// separate point as two clusters(front and back), calc y values respectively
			if (normal.transpose() * vec + line_param_c > 0)
			{
				y_values0 += vec_rot[1];
				count0 += 1;
			}
			else
			{
				y_values1 += vec_rot[1];
				count1 += 1;
			}
		}
	}
	// check if front and back both has points
	if (count0 > 0 && count1 > 0)
	{
		y_values0 /= count0;
		y_values1 /= count1;
		car_wheel_information.car_wheel_base = fabs(y_values1 - y_values0);
		car_wheel_information.car_wheel_width = fabs(min_x - max_x);
//        LOG(INFO) << "--------detector find width, wheelbase: " << width << ", " << wheelbase << ", y means: [" << y_values0 << ", " << y_values1 << "] -----";
		p_cloud_in->points.push_back(pcl::PointXYZ(min_x, car_wheel_information.car_center_y, 0.0));
		p_cloud_in->points.push_back(pcl::PointXYZ(max_x, car_wheel_information.car_center_y, 0.0));
		p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.car_center_x, y_values0, 0.0));
		p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.car_center_x, y_values1, 0.0));
	}
	else
	{
		// calculate wheelbase according to corner points
		// wheelbase = std::max(wheel_box.size.width, wheel_box.size.height);
		// LOG(INFO) << "--------detector find border: [" << wheel_box.size.width << ", " << wheel_box.size.height << "] -----";
		cv::RotatedRect wheel_center_box = cv::minAreaRect(corner_points);
		car_wheel_information.car_wheel_base = std::max(wheel_center_box.size.width, wheel_center_box.size.height);
		car_wheel_information.car_wheel_width = std::min(wheel_box.size.width, wheel_box.size.height);
	}

	return Error_manager(Error_code::SUCCESS);
}



//通过ceres检测中心,旋转,轴距,宽度,  新算法, 可以测量前轮旋转
Error_manager Region_detector::detect_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
										 Common_data::Car_wheel_information& car_wheel_information, bool print)
{
	Error_manager result = Error_manager(Error_code::SUCCESS);
	pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	{
		std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
		*p_cloud_filtered = *p_cloud_in;
	}
	// 1.预处理
	result = preprocess(p_cloud_filtered);
	if (result != SUCCESS)
		return result;
//	std::cout << " huli test :::: " << " 111 = " << 111 << std::endl;
	// 2.聚类
	std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
	result = clustering_only(p_cloud_filtered, seg_clouds, print);
	if (result != SUCCESS)
		return result;
//	std::cout << " huli test :::: " << " 112 = " << 112 << std::endl;
	detect_wheel_ceres::Detect_result detect_result;
    std::string t_error_info;
	if(!m_detector_ceres.detect(seg_clouds, detect_result, t_error_info))
	{
		return Error_manager(Error_code::WJ_REGION_CERES_SOLVE_ERROR, MINOR_ERROR, t_error_info.c_str());
	}
	else
	{
		std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
		car_wheel_information.car_center_x = detect_result.cx;
		car_wheel_information.car_center_y = detect_result.cy;
		car_wheel_information.car_angle = detect_result.theta;
		car_wheel_information.car_wheel_base = detect_result.wheel_base;
		car_wheel_information.car_wheel_width = detect_result.width;
		car_wheel_information.car_front_theta = detect_result.front_theta;
		car_wheel_information.correctness = detect_result.success;
	}

	return Error_manager(Error_code::SUCCESS);
}