RANSAC Spline Fitting

在车道线检测中，通过一系列的点可以拟合出曲线，这儿介绍一下比较常用的随机抽样一致性方法进行三次样条曲线拟合的方法。

三次样条曲线可以通过四个点来定义，分别选取始末点为P0、P3，通过P1、P2控制曲线的形状。如图：

算法步骤如下：

随机抽样一致性算法是最小二乘法的一种改进，通过随机抽样与增加一定的迭代次数，减少了噪声点对结果的干扰。先随机选取n个点，通过离始末点的距离来获取各点t的值，t∈[0,1]。各点t的计算公式如下：

三阶贝塞尔函数曲线可以定义为：

其中，

Q为随机抽样的各点坐标，n*2的矩阵；T为n*4的矩阵，M为4*4的矩阵，P为4*2的矩阵。已知Q、T、M可求得P。

车道线检测步骤中，一般情况检测到直线需要对直线进行kalman filter tracking，跟踪变量为直线方程中 $r$ 和 $\Theta$ 。三阶贝塞尔曲线中可以通过P0、P3获取 $r$ 和 $\Theta$ ，同时加上图1中的 $\Theta_{ 1}$ 和 $\Theta_{ 2}$ ，共跟踪四个变量。

下面贴下拟合三阶贝塞尔函数的代码。曲线可以通过四个点来定义，结构体如下：

typedef struct Spline
{///degree of splineint degree;///points in spline_Point2D32f points[4];///score of splinefloat score;
} Spline;

曲线拟合代码如下：

/** This function fits a Bezier spline to the passed input points
*
* \param points the input points
* \param degree the required spline degree
* \return spline the returned spline
*/
Spline mcvFitBezierSpline(MatrixX2f &points, int degree)
{//set the degreeSpline spline;spline.degree = degree;//get number of pointsint n = points.rows();//float step = 1./(n-1);//sort the pointamcvSortPoints(points, points, 1, 0);//get first point and distance between points_Point2D32f  p0 = _Point2D32f(points(0, 0),points(0, 1));float diff = 0.f;float *us = new float[points.rows()];us[0] = 0;for (int i = 1; i<points.rows(); ++i){float dx = points(i, 0) -points(i - 1, 0);float dy = points(i, 1) -points( i - 1, 1);us[i] = (float)sqrt(dx*dx + dy*dy) + us[i - 1];//     diff += us[i];;}diff = us[points.rows() - 1];//M matrix for BezierMatrix4f M(4, 4);M << -1, 3, -3, 1,3, -6, 3, 0,-3, 3, 0, 0,1, 0, 0, 0 ;//Basis matrixMatrix4Xf B(n,4);//u value for points to create the basis matrixfloat u = 0.f;for (int i = 0; i<B.rows(); i++) //, u+=step){//get u as ratio of y-coordinateu = us[i] / diff;B(i, 3) = 1;  //1B(i, 2) = u;  //uB(i, 1) = u*u;  //u^2B(i, 0) = u*u*u;  //u^2}//multiply B by MB = B*M;//return the required control points by LSMatrixX2f sp(degree+1,2);sp = B.colPivHouseholderQr().solve(points);//put back into splinefor (int j = 0; j < degree + 1; j++){spline.points[j].x = sp(j,0);spline.points[j].y = sp(j,1);}//cleardelete[] us;//returnreturn spline;
}

RANSAC Spline Fitting相关推荐

Udacity机器人软件工程师课程笔记（二十) - 感知 - 校准，过滤，分段， RANSAC
校准,过滤, 分段, RANSAC 首先,我们将讨论传感器校准,也就是说,从几何形状,失真和噪声方面校准相机如何看待周围的世界.了解相机的校准对于了解测量数据如何代表实际物理环境至关重要. 之后,我们 ...
车道线检测（opencv）
بسم الله الرحمن الرحيم MOHAMED ALY California Institute of Technology 1200 E. California Blvd MC 136 ...
Python小白的数学建模课-22.插值方法
Python小白的数学建模课-22.插值方法插值.拟合.回归和预测,都是数学建模中经常提到的概念,也经常被混淆. 插值,是在离散数据的基础上补插连续函数,使得插值函数通过全部给定的离散数据点,多用于 ...
Python小白的数学建模课-23.数据拟合全集
拟合是用一个连续函数(曲线)靠近给定的离散数据,使其与给定的数据相吻合. 数据拟合的算法相对比较简单,但调用不同工具和方法时的函数定义和参数设置有所差异,往往使小白感到困惑. 本文基于 Scipy 工 ...
数学建模中的插值问题
目录 1 概述 1.1 数学建模常见问题 1.2 预测类问题 1.3 插值预测 2 Scipy一维插值方法 2.1 直接法--splrep&splev 代码实现 (包含画图的模板库) 2.2 ...
《Deep Surface Light Fields》论文调研
0<Deep Surface Light Fields> 摘要:A surface light field represents the radiance of rays originat ...
ROS和RRT的一些资料
ROS和RRT结合的示例比较多,之前博文提过两次(1和2),本文做一些汇总和整理,大部分都在roswiki和GitHub上有具体说明.需要认真阅读源码和说明文件,才能使用顺利. 01. RRT for ...
SLAM之特征匹配(二)————RANSAC--------翻译以及经典RANSAC以及其相关的改进的算法小结
本文翻译自维基百科,英文原文地址是:http://en.wikipedia.org/wiki/ransac RANSAC是"RANdom SAmple Consensus(随机抽样一 ...
efficientransac_【泡泡图灵智库】基于图割优化的RANSAC算法（CVPR）
主要贡献本文提出了一种使用局部优化算法改进RANSCA精度的方法,其主要贡献为: 1.提出了一种考虑空间一致性的内点判断方法,从而改进了RANSCA算法的精度: 2.采用图割算法解决内点判断的优化问 ...

RANSAC Spline Fitting

RANSAC Spline Fitting相关推荐

最新文章

热门文章