最近做车道线检测,要检测弯道的曲线,于是采用Bezier3次曲线拟合的方式去拟合弯道曲线。
首先,要知道什么Bezier 3次曲线:对于二次抛物线,使用3个点就可以确定这条抛物线,而且抛物线的参数方程最高次为2,这种拟合抛物线就叫Bezier 的2次曲线拟合,对于3次曲线拟合,需要参数方程的最高次为3,也就是会有4个点来确定曲线,所以叫Bezier的3次曲线拟合。总的来说,由n个点确定的直线,就叫Bezier的n-1次拟合(个人想法,不同理解可以进行讨论)。
接下来来具体分析Beizier的3次拟合过程以及代码:
有这几点需要学习:
1.RanSac算法在拟合过程中的使用;
2.迭代过程中,怎样保证每次产生的随机数不同;
3.怎样用代码实现矩阵相乘,矩阵求逆(将数组传入到Mat矩阵中);
4.投票得分在概率估计中的代码实现;
5.曲线拟合的控制点出来后怎么去画成连续的直线;
6.下面图片的代码实现:(累加公式的欧式距离的计算)
下面一条一条学习:
1. RanSac在拟合算法中的使用
首先看一段核心迭代代码
/**
* 对输入的点使用 RANSAC 算法进行匹配,并返回一个有四个点的 vector<Point3f>
* 如果匹配失败,则返回一个空的 vector<Point3f>
**/
vector<Point3f> fit(int _iterNum, Mat _image)
{
int i;
vector<Point3f> spline, bestSpline;
float score, bestScore = -1;
vector <Point3f> samples;
vector <Point3f> samples1;
for (i = 0; i < _iterNum; i++)
{
count++;//这里的count是一个成员变量,用于产生随机数的时候初始种子的选定
samples= this->getSample(50);//这个入口函数就实现了50次迭代每次都会产生不完全相同的随机数。
cout<<"sample: "<<samples[20]<<","<<samples[9]<<":"<<i <<endl;
if (samples.size() < 4)
{
continue;
}
spline = this->fitSpline(samples);
// cout<<"spline: "<<spline.size() <<endl;
sort(spline.begin() ,spline.end() , Comp);
score = this->computeScore(spline, _image);
if (score > bestScore)
{
bestSpline = spline;
bestScore = score;
swap(samples1,samples);
}
vector<Point3f>().swap( samples ); //将空间大小也进行初始化
}
Mat ground1(500,500,CV_8UC1,Scalar::all(0));
for(int i=0;i<50;i++)
{
circle(ground1,Point(samples1[i].x , samples1[i].y),2,Scalar(255),1,8);
}
imshow("g" , ground1);
// waitKey(0);
return bestSpline;
//return spline;
}
- 迭代过程中,怎样保证每次产生的随机数不同;
/**
* 采样,返回 n 个点
*/
vector<Point3f> getSample(int n)//获取n个样点
{
vector <Point3f> ret;
int i, k;
vector <int> order;
srand(count);//这个就是关键代码,让rand()每次的迭代初始值都不同
for (i = 0; i < n; i++)
{
float r = rand() % 200 ;
order.push_back(r) ;
}
sort(order.begin() , order.end());
//cout<<order[5]<<":"<<order[20]<<endl;
for (i = 0; i < n; i++)
{
//cout<<"pos::" <<pos[k]<<endl;
cout<<order[i] <<endl;
}
for (i = 0; i < n; i++)
{
ret.push_back(ps[order[i]]);
}
//cout<<"拟合结束,得分:"<<bestScore<<",:最佳线条点:"<<bestSpline<<"\n"<<endl;
//cout<<"采样点:"<<ret<<endl;
return ret;
}
3.怎样用代码实现矩阵相乘,矩阵求逆
/// 构建矩阵 T, M, Q
Mat M;
this->getM(M);//4行4列
// cout << M<<endl;
Mat T(ps.size(), 4, CV_32F, Scalar::all(1));//100行4列
for (i = 0; i < ps.size(); i++) //T矩阵
{
float tt = t[i];
T.at<float>(i, 2) = tt;
tt *= t[i];
T.at<float>(i, 1) = tt;
tt *= t[i];
T.at<float>(i, 0) = tt;
}
//cout<< T <<endl;
float datQ[ps.size()][2];//100行2列//这里ps数据就是随机采样的50个样本点的数据坐标点
for (i = 0; i < ps.size(); i++)
{
Point3f p = ps.at(i);
datQ[i][0] = p.x;
datQ[i][1] = p.y;
}
Mat Q(ps.size(), 2, CV_32F, datQ);//将数组元素传入到Mat矩阵中,这点也要学习
Mat P = (T * M).inv(DECOMP_SVD) * Q;//本来是Q=T*M*P,得到的P为4行2列
// Mat P = (T * M).inv() * Q;
//cout<<"P: " <<P.size() <<endl;
// cout<<"控制点坐标:"<<P<<endl;
for (k = 0; k < P.rows; k++)
{
pc.push_back( Point3f(P.at<float>(k, 0) , P.at<float>(k, 1), 0) );
}
//cout<<"pc: "<<pc.size() <<endl;
4.投票得分在概率估计中的代码实现
/**
* 对线条进行评分
*
* @param vector<Point3f> 线条的 4 个控制点
* @return float 线条得分,越大越好
*/
float computeScore(vector<Point3f> spline, Mat _image)
{
float score = 0;
float x = 0, y = 0;
int xx, yy;
float t3, t2, t1, t;
/// 计算原始分数
// for (t = 0; t <= 1; t += 0.01)
for (t = 0; t <= 1; t += 0.001)
{
t1 = t;
t2 = t1 * t;
t3 = t2 * t;
x += (-1 * t3 + 3 * t2 - 3 * t1 + 1) * spline.at(0).x;
x += (3 * t3 - 6 * t2 + 3 * t1) * spline.at(1).x;
x += (-3 * t3 + 3 * t2) * spline.at(2).x;
x += t3 * spline.at(3).x;
y += (-1 * t3 + 3 * t2 - 3 * t1 + 1) * spline.at(0).y;
y += (3 * t3 - 6 * t2 + 3 * t1) * spline.at(1).y;
y += (-3 * t3 + 3 * t2) * spline.at(2).y;
y += t3 * spline.at(3).y;
xx = round(x);
yy = round(y);
//cout<< xx<<" "<<yy<<endl;
if (xx > 0 && xx < _image.cols && yy > 0&& yy < _image.rows)
{
for(int a=-10;a<10;a++)//这里的10是左右举例为10的相关点区域
{
score+= _image.at<uchar>(yy,xx+a);
}
}
x=0;
y=0;
//cout<<"score:"<<score<<endl;
}
return score;
}
5.曲线拟合的控制点出来后怎么去画成连续的直线;
Point p , pOld;//这里psSPline里面就存储了控制线条的4个点
if( psSpline.size() > 0)
{
pOld = ransacFit.getPoint(0, psSpline);//这是计算第一个点,为了让变量pOld有初值
for (t = 0.01; t <= 1; t += 0.001)//画出1000个点
{
Point p = ransacFit.getPoint(t, psSpline);
line(ground, pOld, p, Scalar(255));
pOld = p;
}
}
else
{
cout<<"没有拟合线条"<<endl;
}
在给出getpoint函数实现
/**
* 根据参数 t 和控制点,返回一个直角坐标系上的点
*/
Point getPoint(float t, vector<Point3f> spline)
{
float x = 0, y = 0;
float t3, t2, t1;
t1 = t;
t2 = t1 * t;
t3 = t2 * t;
x += (-1 * t3 + 3 * t2 - 3 * t1 + 1) * spline.at(0).x;
x += (3 * t3 - 6 * t2 + 3 * t1) * spline.at(1).x;
x += (-3 * t3 + 3 * t2) * spline.at(2).x;
x += t3 * spline.at(3).x;
y += (-1 * t3 + 3 * t2 - 3 * t1 + 1) * spline.at(0).y;
y += (3 * t3 - 6 * t2 + 3 * t1) * spline.at(1).y;
y += (-3 * t3 + 3 * t2) * spline.at(2).y;
y += t3 * spline.at(3).y;
//cout<<x<<":"<<y<<endl;
return Point(x, y);
}
6.累加公式的欧式距离的计算
/// 计算 t_{i}(计算点的时候t的步进不能按画点时候的平均步进来,要按累加求欧式距离来,就把前面的点的影响关联到了后面的点)
t[0] = 0;
t[ps.size() - 1] = 1;//头尾为1
/// t_{i} = \frac{tA}{tB}
for (i = 1; i < ps.size(); i++)
{
Point3f p2, p1;
float d;
p2 = ps.at(i);
p1 = ps.at(i - 1);
d = sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
tB += d;
}
for (i = 1; i < ps.size() - 1; i++)
{
Point3f p2, p1;
float d;
p2 = ps.at(i);
p1 = ps.at(i - 1);
d = sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
tA += d;
t[i] = tA / tB;
// cout<<"t:"<<i<<":"<<t[i] <<endl;
}
如果还有问题不理解,可以QQ联系我:454933136