数学补充
这里会用到线性代数里的一些知识，每次都是用起来看，用完了又忘，这里把一些可能用到的贴出来，用于快速理解算法里用到的公式等。直线一般式
当x1≠x2，y1≠y2时，直线的斜率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 ①
可以发现，当x1=x2或y1=y2时，①式仍然成立。所以直线AX+BY+C=0的一般式方程就是：
A = Y2 - Y1
B = X1 - X2
C = X2*Y1 - X1*Y2
对于一元二次多项式，可以转换为线性方程组求解，我们一般写成矩阵形式 Ax = y。
Ax = y非一致方程和一致方程的求解
一致与非一致方程
一致方程是指Ax = y有至少一个解
非一致方程指Ax = y没有解
Ax = y求解
如果A是满秩的方阵，则x = inv(A）*y
如果A不是方阵，但是是行满秩或者列满秩，那么解为A的伪逆乘以y
如果A是秩亏的，那么A的解为A的广义逆乘以y
实际上广义逆包括逆、伪逆，广义逆又称为：Moore-Penrose逆矩阵，所以Ax = y的解可以统一为A 的Moore-Penrose逆矩阵乘以y，特别的是，对于一致性方程，该解为最小范数解，对于非一致方程，该解为最小范数最小二乘解
Moore-Penrose逆矩阵
RANSAC直线拟合文章来源：https://www.toymoban.com/news/detail-578893.html
C++实现
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();
// 随机选n个点
while (1)
{
unsigned int index = uniform(rng);
selectIndexs.push_back(index);
if(selectIndexs.size() == mSamples) // sample==2
{
break;
}
}
// 模型参数估计
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);
多车道线检测效果图
文章来源地址https://www.toymoban.com/news/detail-578893.html
到了这里，关于C++:RANSAC采样一致性算法拟合一元二次曲线的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
