數(shù)學(xué)補(bǔ)充

這里會(huì)用到線性代數(shù)里的一些知識(shí)，每次都是用起來(lái)看，用完了又忘，這里把一些可能用到的貼出來(lái)，用于快速理解算法里用到的公式等。
直線一般式

當(dāng)x1≠x2，y1≠y2時(shí)，直線的斜率k=(y2-y1)/(x2-x1)
故直線方程為y-y1=(y2-y1)/(x2-x1)×(x-x1)
即x2y-x1y-x2y1+x1y1=(y2-y1)x-x1(y2-y1)
即(y2-y1)x-(x2-x1)y-x1(y2-y1)+(x2-x1)y1=0
即(y2-y1)x+(x1-x2)y+x2y1-x1y2=0 ①
可以發(fā)現(xiàn)，當(dāng)x1=x2或y1=y2時(shí)，①式仍然成立。所以直線AX+BY+C=0的一般式方程就是：
A = Y2 - Y1
B = X1 - X2
C = X2*Y1 - X1*Y2

對(duì)于一元二次多項(xiàng)式，可以轉(zhuǎn)換為線性方程組求解，我們一般寫成矩陣形式 Ax = y。

Ax = y非一致方程和一致方程的求解
一致與非一致方程

    一致方程是指Ax = y有至少一個(gè)解
    非一致方程指Ax = y沒有解

Ax = y求解

    如果A是滿秩的方陣，則x = inv(A）*y

    如果A不是方陣，但是是行滿秩或者列滿秩，那么解為A的偽逆乘以y

    如果A是秩虧的，那么A的解為A的廣義逆乘以y

實(shí)際上廣義逆包括逆、偽逆，廣義逆又稱為：Moore-Penrose逆矩陣，所以Ax = y的解可以統(tǒng)一為A 的Moore-Penrose逆矩陣乘以y，特別的是，對(duì)于一致性方程，該解為最小范數(shù)解，對(duì)于非一致方程，該解為最小范數(shù)最小二乘解

Moore-Penrose逆矩陣

RANSAC直線擬合

C++實(shí)現(xiàn)

int ransac_curve_fitting(std::vector<float4> &in_cloud, std::vector<double> &best_model, std::vector<float4> &inliers, 
                        int maxiter=200, int mSamples=3, int min_inliers=10, float residual_thres=0.2)
{
    int point_num = in_cloud.size();
    std::default_random_engine rng;
    std::uniform_int_distribution<unsigned> uniform(0, point_num-1);
    rng.seed(10);

    // y = ax^2 + bx + c    <<<--- A_(0,0)=a, A_(1,0)=b, A_(2,0)=c --->>>
    Eigen::MatrixXd X_(3, 3), Y_(3, 1), A_(3, 1), points_x(point_num, 3), points_y(point_num, 1); //  dynamic cols.rows,
    for (int i = 0; i < point_num; ++i)
    {
      points_x(i, 0) = in_cloud[i].x * in_cloud[i].x;
      points_x(i, 1) = in_cloud[i].x;
      points_x(i, 2) = 1;
      points_y(i, 0) = in_cloud[i].y;
    }   

    std::vector<unsigned int> selectIndexs;
    int best_inilers = 0;
    float best_error = 100.0;
    int iter = 0;
    int best_iter = 0;
    float tmp_error = 0.0;
    int num = 0;
    
    while (iter < maxiter)
    {
        selectIndexs.clear();
        inliers.clear();
        // 隨機(jī)選n個(gè)點(diǎn)
        while (1)
        {
            unsigned int index = uniform(rng); 
            selectIndexs.push_back(index);
            if(selectIndexs.size() == mSamples) // sample==2
            {
                break;
            }
        }
        // 模型參數(shù)估計(jì)
        for (size_t i = 0; i < selectIndexs.size(); ++i)
        {
          // std::cerr << selectIndexs[i] << std::endl;
          X_(i, 0) = in_cloud[selectIndexs[i]].x * in_cloud[selectIndexs[i]].x;
          X_(i, 1) = in_cloud[selectIndexs[i]].x;
          X_(i, 2) = 1;
          Y_(i, 0) = in_cloud[selectIndexs[i]].y;
        }
        try
        {
          X_.inverse();
        }
        catch(const std::exception& e)
        {
          std::cerr << e.what() << '\n';
          std::cerr << "Start the next loop..." << '\n';
          continue;
        }
        // X_為可逆方陣
        A_ = X_.inverse() * Y_;  
        Eigen::MatrixXd y_pred = points_x * A_;
        Eigen::MatrixXd residual = points_y - y_pred;

        for (size_t i = 0; i < point_num; ++i)
        {
          if (abs(residual(i, 0)) < residual_thres)
          {
            inliers.push_back(in_cloud[i]);
          }
          
        }
        
        int inlier_num = inliers.size();
        if (inlier_num > best_inilers)
        {
          best_inilers = inlier_num;
          best_model[0] = A_(0,0);
          best_model[1] = A_(1,0);
          best_model[2] = A_(2,0);
          best_iter = iter;
        }

        if (inlier_num > min_inliers)
        {
          Eigen::MatrixXd better_model(3, 1), inliers_x(inlier_num, 3), 
                          inliers_y(inlier_num, 1), y_pred_better(inlier_num, 1);
          for (size_t i = 0; i < inlier_num; ++i)
          {
            inliers_x(i, 0) = inliers[i].x * inliers[i].x;
            inliers_x(i, 1) = inliers[i].x;
            inliers_x(i, 2) = 1;
            inliers_y(i, 0) = inliers[i].y;
          }
          better_model = (inliers_x.transpose() * inliers_x).inverse() * inliers_x.transpose() * inliers_y;
          y_pred_better = inliers_x * better_model;
          float mean_square_error = 0;
          for (size_t i = 0; i < inlier_num; ++i)
          {
            mean_square_error += pow((y_pred_better(i,0) - inliers_y(i,0)), 2);
          }
          mean_square_error = mean_square_error / inlier_num;
          if (mean_square_error < best_error)
          {
            best_error = mean_square_error;
            best_model[0] = better_model(0,0);
            best_model[1] = better_model(1,0);
            best_model[2] = better_model(2,0);
            best_iter = iter;
          }

        }

        if (tmp_error != best_error)
        {
          tmp_error = best_error;
        }
        else
        {
          num += 1;
          if (num > 10)
          {
            break;
          }
        }
        std::cerr << "number of the error is constant: " << num << std::endl;
        std::cerr << "ransac iterations: " << iter << std::endl;
        iter++;
    }
    std::cerr << "best_iter: " << best_iter << "\n" 
              << "best_model[0]: " << best_model[0] << "\n" 
              << "best_error: " << best_error << "\n" 
              << "inliers.size(): " << inliers.size() << std::endl;
    return 0;

}

std::vector<double> coef_line(3); // a, b, c
std::vector<float4> inliers;
ransac_curve_fitting(result, coef_line, inliers);

多車道線檢測(cè)效果圖

文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-578893.html

到了這里，關(guān)于C++:RANSAC采樣一致性算法擬合一元二次曲線的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！