线结构光中心线提取、拟合（C++ OpenCV）

//提取激光中心线
std::vector<cv::Point2f> lineExtractor(cv::Mat img)
{
    cv::Mat bin;
    cv::adaptiveThreshold(img, bin, 50, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY_INV, 3, 8);
    //cv::namedWindow("bin", cv::WINDOW_NORMAL);
    //cv::imshow("bin", bin);
    //cv::waitKey(0);


    cv::Mat edges;
    cv::Canny(bin, edges, 100, 200);
    //cv::namedWindow("Canny", cv::WINDOW_NORMAL);
    //cv::imshow("Canny", edges);
    //cv::waitKey(0);

    //static bool flag = true;
    //if (flag)
    //    cv::imwrite("canny.png", edges);
    //flag = false;

    std::vector<cv::Point2f> vdata;
    for (int i = 0; i < edges.rows; i++)
    {
        uchar* row_ptr = edges.ptr(i);
        for (int j = 0; j < edges.cols; j++)
        {
            if (row_ptr[j] != 0)
                vdata.push_back(cv::Point2f(j, i));
        }
    }

    cv::Mat tmp(img.size(), CV_8UC3, cv::Scalar::all(0));
    for (auto p : vdata)
        cv::circle(tmp, p, 1, cv::Scalar(255, 255, 255), 1);
    //cv::namedWindow("points", cv::WINDOW_NORMAL);
    //cv::imshow("points", tmp);
    //cv::waitKey(0);

    cv::Vec4f lineParam;
    fitLineRansac(vdata, lineParam, 1000, 10);
    double k = lineParam[1] / lineParam[0];
    double b = lineParam[3] - k * lineParam[2];

    cv::Point p1, p2;
    p1.y = 0;
    p1.x = (p1.y - b) / k;

    p2.y = img.size().height;
    p2.x = (p2.y - b) / k;

    cv::line(tmp, p1, p2, cv::Scalar(0, 0, 255), 2);


    // 显示结果图像
    cv::namedWindow("Structure Line", cv::WINDOW_NORMAL);
    cv::imshow("Structure Line", tmp);
    cv::waitKey(0);

    std::vector<cv::Point2f> ptsLine;
    for (int x = 0; x < img.cols; x++)
        ptsLine.push_back(cv::Point2f(x, k * x + b));

    return ptsLine;
}

// 2d line fitting
void fitLineRansac(const std::vector<cv::Point2f>& points, cv::Vec4f& line, 
    int iterations = 1000, double sigma = 1., double k_min = -7., double k_max = 7.)
{
    unsigned int n = points.size();

    if (n < 2)
    {
        return;
    }

    cv::RNG rng;
    double bestScore = -1.;
    for (int k = 0; k < iterations; k++)
    {
        int i1 = 0, i2 = 0;
        while (i1 == i2)
        {
            i1 = rng(n);
            i2 = rng(n);
        }
        const cv::Point2f& p1 = points[i1];
        const cv::Point2f& p2 = points[i2];

        cv::Point2f dp = p2 - p1;//直线的方向向量
        dp *= 1. / norm(dp);
        double score = 0;

        if (dp.y / dp.x <= k_max && dp.y / dp.x >= k_min)
        {
            for (int i = 0; i < n; i++)
            {
                cv::Point2f v = points[i] - p1;
                double d = v.y * dp.x - v.x * dp.y;//向量a与b叉乘/向量b的摸.||b||=1./norm(dp)
                //score += exp(-0.5*d*d/(sigma*sigma));//误差定义方式的一种
                if (fabs(d) < sigma)
                    score += 1;
            }
        }
        if (score > bestScore)
        {
            line = cv::Vec4f(dp.x, dp.y, p1.x, p1.y);
            bestScore = score;
        }
    }
}