计算曲线上一点的曲率（差分法）

// 计算曲线上一点的曲率（差分法）
double calculateCurvature(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in, size_t index)
{
	if (index < 1 || index >= cloud_in->size() - 1)
	{
		return 0.0;  // 边界点曲率设为0
	}

	/*double dx1 = cloud_in->at(index).x - cloud_in->at(index - 1).x;
	double dy1 = cloud_in->at(index).z - cloud_in->at(index - 1).z;
	double dx2 = cloud_in->at(index + 1).x - cloud_in->at(index).x;
	double dy2 = cloud_in->at(index + 1).z - cloud_in->at(index).z;

	double d2x = dx2 - dx1;
	double d2y = dy2 - dy1;

	double curvature = std::abs(dx1 * d2y - dy1 * d2x) / std::pow(dx1 * dx1 + dy1 * dy1, 1.5);
	std::cout << "curvature：" << curvature << std::endl;*/
	double dx1 = cloud_in->at(index + 1).x - cloud_in->at(index - 1).x;
	double dy1 = cloud_in->at(index + 1).z - cloud_in->at(index - 1).z;
	
	double dy2 = cloud_in->at(index + 1).z - 2 * cloud_in->at(index).z + cloud_in->at(index-1).z ;
	double dx2 = std::pow((cloud_in->at(index + 1).x - cloud_in->at(index - 1).x), 2);

	double curvature = std::abs(dy2 / dx2);
	
	//std::cout << "curvature：" << curvature << std::endl;

	return curvature;
}

// 查找拐点
std::vector<size_t> findCornerPoints(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in, double threshold) 
{
	//std::vector<size_t> cornerPointIndices;
	//for (size_t i = 1; i < cloud_in->size() - 1; ++i) 
	//{
	//	double curvature = calculateCurvature(cloud_in, i);
	//	// 简单的局部极大值判定
	//	double prevCurvature = calculateCurvature(cloud_in, i - 1);
	//	double nextCurvature = calculateCurvature(cloud_in, i + 1);

	//	if (curvature > prevCurvature && curvature > nextCurvature && curvature > threshold) {
	//		cornerPointIndices.push_back(i);
	//		std::cout << "curvature：" << curvature << std::endl;

	//	}
	//}

	//计算曲率
	std::vector<double> curvature_vec;
	for (size_t i = 1; i < cloud_in->size() - 1; ++i)
	{
		double curvature = calculateCurvature(cloud_in, i);
		curvature_vec.push_back(curvature);
	}

	//计算曲率最大值和最小值
	auto maxPosition = std::max_element(curvature_vec.begin(), curvature_vec.end());
	auto minPosition = std::min_element(curvature_vec.begin(), curvature_vec.end());

	double max_curvature = *maxPosition;
	size_t max_distance_index = maxPosition - curvature_vec.begin();

	double min_curvature = *minPosition;
	size_t min_distance_index = minPosition - curvature_vec.begin();

	//曲率归一化
	for (int i = 1; i < curvature_vec.size(); ++i)
	{
		curvature_vec[i] = (curvature_vec[i] - min_curvature) / (max_curvature - min_curvature);
		//std::cout << "curvature：" << curvature_vec[i] << std::endl;

	}

	std::vector<size_t> cornerPointIndices;
	for (int i = 1; i < curvature_vec.size() - 1; ++i)
	{
		if (curvature_vec[i] > curvature_vec[i-1] && curvature_vec[i] > curvature_vec[i + 1] && curvature_vec[i] > threshold) {
			cornerPointIndices.push_back(i);
			std::cout << "curvature：" << curvature_vec[i] << std::endl;
		}
	}


	return cornerPointIndices;
}