二维空间点到直线的垂直距离

2、代码

（给定直线上2个点）

double Point2Dline(pcl::PointXYZ p, pcl::PointXYZ Line_1, pcl::PointXYZ Line_2)
{
	double result;
	double x = p.x;
	double y = p.y;

	double dx = Line_2.x - Line_1.x;
	double dy = Line_2.y - Line_1.y;
	if (dx == 0)//斜率不存在的情况
	{
		result = abs(x - Line_2.x);
	}
	else
	{
		//求出直线形式：y=kx+b
		double k = dy / dx;
		double b = Line_1.y - k*Line_1.x;
		//转换成标准形式
		//kx-y+b=0

		double fenzi = abs(k*x - y + b);
		double fenmu = sqrt(1 + k*k);
		result = fenzi / fenmu;
	}
	return result;
}