学习OpenCV3：计算点到直线的距离并画出垂线（含公式和推导）

---

一、背景

  已知一个点$p_0(x_0,y_0)$和一条直线$l_1$。$l_1$由起点$p_1(x_1,y_1)$和终点$p_2(x_2,y_2)$组成。现希望先计算$p_0$在直线$l_1$上的垂足$p_3(x_3,y_3)$并画出垂线$l_2$，再计算$p_0$到$l_1$的距离$d$。

$$\begin{gathered} 当l_1垂直x轴时：x_1=x_2 \\ 垂足：x_3=x_1{y}_3=y_0 \\ 距离：d=|x_1-x_0| \end{gathered}$$

$$\begin{gathered} 当l_1垂直y轴时：y_1=y_2 \\ 垂足：x_3=x_0{y}_3=y_1 \\ 距离：d=|y_1-y_0| \end{gathered}$$

$$\begin{gathered} l_1与x轴，y轴都不垂直时，l_1的直线方程：\frac{y-y_1}{y_2-y_1}=\frac{x-x_1}{x_2-x_1}\Rightarrow a_{1}x+b_{1}y+c_1=0 \\ {a}_1=-(y_2-y_1)b_1=x_2-x_1{c}_1=(y_2-y_1)x_1-(x_2-x_1)y_1{k}_1=\frac{-a_1}{b_1} \\ 垂线l_2与l_1垂直，则：k_1\ast k_2=-1\Rightarrow k_2=\frac{b_1}{a_1} \\ {l}_2的直线方程：y-y_0=k_2(x-x_0)\Rightarrow a_{2}x+b_{2}y+c_2=0 \\ {a}_2=-b_1{b}_2=a_1{c}_2=b_1{x}_0-a_1{y}_0 \\ {p}_3经过l_1和l_2，则：\{\begin{array}{l}{a}_1{x}_3+b_1{y}_3+c_1=0\\ a_2{x}_3+b_2{y}_3+c_2=0\end{array}\Rightarrow \{\begin{array}{l}{x}_3=\frac{b_1{c}_2-b_2{c}_1}{a_1{b}_2-a_2{b}_1}\\ y_3=\frac{a_1{c}_2-a_2{c}_1}{a_2{b}_1-a_1{b}_2}\end{array}\Rightarrow \{\begin{array}{l}{x}_3=\frac{b_1^2{x}_0-a_1{b}_1{y}_0-a_1{c}_1}{a_1^2+b_1^2}\\ y_3=\frac{a_1^2{y}_0-a_1{b}_1{x}_0-b_1{c}_1}{a_1^2+b_1^2}\end{array} \\ {p}_0到p_3距离：d=\sqrt{(x_3-x_0{)}^2+(y_3-y_0{)}^2}=\frac{|a_1{x}_0+b_1{y}_0+c_1|}{\sqrt{a_1^2+b_1^2}} \end{gathered}$$

二、实现

#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <array>
#include <cmath>
using namespace std;
using namespace cv;

const string g_window_name = "image";
Mat g_image_original;     // 空白图片
array<Point, 3> g_points; // 存放3个点，分别是线外的点，线的起点，线的终点

// 计算垂足的坐标
Point calculate_foot_point(array<Point, 3> &ps)
{
    Point p(0, 0);                // 垂足
    if (ps.at(1).x == ps.at(2).x) // 线与x轴垂直
    {
        p.x = ps.at(1).x;
        p.y = ps.at(0).y;
    }
    else if (ps.at(1).y == ps.at(2).y) // 线与y轴垂直
    {
        p.x = ps.at(0).x;
        p.y = ps.at(1).y;
    }
    else // 线与x轴，y轴都不垂直
    {
        int a1 = -(ps.at(2).y - ps.at(1).y);
        int b1 = ps.at(2).x - ps.at(1).x;
        int c1 = (ps.at(2).y - ps.at(1).y) * ps.at(1).x - (ps.at(2).x - ps.at(1).x) * ps.at(1).y;
        p.x = (b1 * b1 * ps.at(0).x - a1 * b1 * ps.at(0).y - a1 * c1) / (a1 * a1 + b1 * b1);
        p.y = (a1 * a1 * ps.at(0).y - a1 * b1 * ps.at(0).x - b1 * c1) / (a1 * a1 + b1 * b1);
    }
    return p;
}

// 计算点到直线的距离
int calculate_distance(array<Point, 3> &ps)
{
    int d = 0;                    // 距离
    if (ps.at(1).x == ps.at(2).x) // 线与x轴垂直
    {
        d = abs(ps.at(1).x - ps.at(0).x);
    }
    else if (ps.at(1).y == ps.at(2).y) // 线与y轴垂直
    {
        d = abs(ps.at(1).y - ps.at(0).y);
    }
    else // 线与x轴，y轴都不垂直
    {
        int a1 = -(ps.at(2).y - ps.at(1).y);
        int b1 = ps.at(2).x - ps.at(1).x;
        int c1 = (ps.at(2).y - ps.at(1).y) * ps.at(1).x - (ps.at(2).x - ps.at(1).x) * ps.at(1).y;
        d = abs(a1 * ps.at(0).x + b1 * ps.at(0).y + c1) / sqrt(a1 * a1 + b1 * b1);
    }
    return d;
}

// 若垂足不在线上，则画延长的虚线连接垂足
void draw_dotted_line(Mat img, Point2f p1, Point2f p2, Scalar color, int thickness)
{
    float n = 15; // 小虚线的长度
    float w = p2.x - p1.x, h = p2.y - p1.y;
    float l = sqrtf(w * w + h * h);
    // 矫正线长度，使线个数为奇数
    int m = l / n;
    m = m % 2 ? m : m + 1;
    n = l / m;

    circle(img, p1, 1, color, thickness); // 画起点
    circle(img, p2, 1, color, thickness); // 画终点
    // 画中间点
    if (p1.y == p2.y) //水平线：y = m
    {
        float x1 = min(p1.x, p2.x);
        float x2 = max(p1.x, p2.x);
        for (float x = x1, n1 = 2 * n; x < x2; x = x + n1)
            line(img, Point2f(x, p1.y), Point2f(x + n, p1.y), color, thickness);
    }
    else if (p1.x == p2.x) //垂直线, x = m
    {
        float y1 = min(p1.y, p2.y);
        float y2 = max(p1.y, p2.y);
        for (float y = y1, n1 = 2 * n; y < y2; y = y + n1)
            line(img, Point2f(p1.x, y), Point2f(p1.x, y + n), color, thickness);
    }
    else
    {
        // 直线方程的两点式：(y-y1)/(y2-y1)=(x-x1)/(x2-x1) -> y = (y2-y1)*(x-x1)/(x2-x1)+y1
        float n1 = n * abs(w) / l;
        float k = h / w;
        float x1 = min(p1.x, p2.x);
        float x2 = max(p1.x, p2.x);
        for (float x = x1, n2 = 2 * n1; x < x2; x = x + n2)
        {
            Point p3 = Point2f(x, k * (x - p1.x) + p1.y);
            Point p4 = Point2f(x + n1, k * (x + n1 - p1.x) + p1.y);
            line(img, p3, p4, color, thickness);
        }
    }
}

// 画图
void draw(Mat img, array<Point, 3> &ps, size_t step)
{
    Mat img1;
    img.copyTo(img1); // 拷贝空白图片，方便重复画图
    if (ps != array<Point, 3>())
    {
        circle(img1, ps.at(0), 2, Scalar(255, 0, 0), 2);      // 画线外的蓝点
        line(img1, ps.at(1), ps.at(2), Scalar(0, 255, 0), 2); // 画绿线
    }
    int lenght = pow(ps.at(2).y - ps.at(1).y, 2) + pow(ps.at(2).x - ps.at(1).x, 2);
    if (step == 3) // 画线结束时（第2次鼠标左键释放）
    {
        if (ps.at(1) != Point(0, 0) && lenght < 500) // 若线太短，则发出提示
            putText(img1, "line too short!", Point(20, 40), cv::FONT_HERSHEY_COMPLEX, 1, Scalar(0, 0, 255));
        Point p = calculate_foot_point(g_points);      // 计算垂足坐标
        line(img1, p, ps.at(0), Scalar(0, 0, 255), 2); // 画红色的垂线
        // 画淡蓝色的延长线
        Point p1 = ps.at(1).x > ps.at(2).x ? ps.at(2) : ps.at(1);
        Point p2 = ps.at(1).x > ps.at(2).x ? ps.at(1) : ps.at(2);
        if (p.x < p1.x)
        {
            draw_dotted_line(img1, p, p1, Scalar(255, 255, 0), 2);
        }
        else if (p.x > p2.x)
        {
            draw_dotted_line(img1, p, p2, Scalar(255, 255, 0), 2);
        }
        else
        {
            p1 = ps.at(1).y > ps.at(2).y ? ps.at(2) : ps.at(1);
            p2 = ps.at(1).y > ps.at(2).y ? ps.at(1) : ps.at(2);
            if (p.y < p1.y)
            {
                draw_dotted_line(img1, p, p1, Scalar(255, 255, 0), 2);
            }
            else if (p.y > p2.y)
            {
                draw_dotted_line(img1, p, p2, Scalar(255, 255, 0), 2);
            }
        }
        int w = 10, h = 25;
        double sac = 0.5;
        // Point m(10, 10);
        // 文字：点
        string s = "p0: (" + to_string(ps.at(0).x) + "," + to_string(ps.at(0).y) + ")";
        putText(img1, s, Point(w, h), cv::FONT_HERSHEY_COMPLEX, sac, Scalar(0, 0, 255));
        // 文字：线
        s = "p1: (" + to_string(ps.at(1).x) + "," + to_string(ps.at(1).y) + ")";
        putText(img1, s, Point(14 * w, h), cv::FONT_HERSHEY_COMPLEX, sac, Scalar(0, 0, 255));
        s = "p2: (" + to_string(ps.at(2).x) + "," + to_string(ps.at(2).y) + ")";
        putText(img1, s, Point(27 * w, h), cv::FONT_HERSHEY_COMPLEX, sac, Scalar(0, 0, 255));
        // 文字：垂足
        s = "p3: (" + to_string(p.x) + "," + to_string(p.y) + ")";
        putText(img1, s, Point(w, 2 * h), cv::FONT_HERSHEY_COMPLEX, sac, Scalar(0, 0, 255));
        // 文字：距离
        int d = calculate_distance(g_points); // 计算点到直线的距离
        putText(img1, "width: " + to_string(d), Point(14 * w, 2 * h), cv::FONT_HERSHEY_COMPLEX, sac, Scalar(0, 0, 255));
    }
    imshow(g_window_name, img1);
}

// 鼠标回调函数
void mouse_callback(int event, int x, int y, int flags, void *param)
{
    static size_t step = 0; // 控制鼠标操作步骤
    switch (event)
    {
    case cv::EVENT_LBUTTONDOWN: // 鼠标左键点击
        if (step == 0)
        {
            g_points.at(0) = Point(x, y); // 保存第1个点
            step = 1;
        }
        else if (step == 1)
        {
            g_points.at(1) = Point(x, y); // 保存线的起点
            g_points.at(2) = Point(x, y); // 保存线的终点
            step = 2;
        }
        break;
    case cv::EVENT_MOUSEMOVE: // 鼠标移动
        if (step == 2)
            g_points.at(2) = Point(x, y); // 刷新线的终点
        break;
    case cv::EVENT_LBUTTONUP: // 鼠标左键释放
        if (step == 2)
            step = 3;
        break;
    case cv::EVENT_RBUTTONDOWN: // 鼠标右键点击，清除内容
        step = 0;
        g_points = array<Point, 3>();
        break;
    default:
        break;
    }
    draw(g_image_original, g_points, step);
}

// 主函数
int main()
{
    namedWindow(g_window_name, WINDOW_AUTOSIZE);
    int w = 600, h = 400;
    g_image_original = Mat(h, w, CV_8UC3, Scalar(255, 255, 255)); // 空白图片
    cv::imshow(g_window_name, g_image_original);
    cv::setMouseCallback(g_window_name, mouse_callback); // 调用鼠标回调函数
    waitKey();
    return 0;
}

操作方法：
  运行程序，在空白图片上用鼠标左键点击一下并释放，这会画出线外的蓝点。然后再点击鼠标左键并按住移动，由此画出绿线。此次释放鼠标左键后，程序自动根据点和线画出红色的垂线，并输出点、线、垂线和距离。若垂足不在线上，程序自动画对应的蓝色延长线。鼠标右键点击可擦掉已画出的图像，由此可按前面步骤重复画图。按键盘任意键可退出程序。

运行结果：