二维直线的拟合
1、OpenCV实现
使用OpenCV实现二维直线的拟合:
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
using namespace cv;
using namespace std;
int main( )
{
const char* filename = "1.bmp";
Mat src_image = imread(filename,1);
if( src_image.empty() )
{
cout << "Couldn't open image!" << filename;
return 0;
}
int img_width = src_image.cols;
int img_height = src_image.rows;
Mat gray_image,bool_image;
cvtColor(src_image,gray_image,CV_RGB2GRAY);
threshold(gray_image,bool_image,0,255,CV_THRESH_OTSU);
imshow("二值图", bool_image);
//获取二维点集
vector<Point> point_set;
Point point_temp;
for( int i = 0; i < img_height; ++i)
{
for( int j = 0; j < img_width; ++j )
{
if (bool_image.at<unsigned char>(i,j) < 255)
{
point_temp.x = j;
point_temp.y = i;
point_set.push_back(point_temp);
}
}
}
//直线拟合
//拟合结果为一个四元素的容器,比如Vec4f - (vx, vy, x0, y0)
//其中(vx, vy) 是直线的方向向量
//(x0, y0) 是直线上的一个点
Vec4f fitline;
//拟合方法采用最小二乘法
fitLine(point_set,fitline,CV_DIST_L2,0,0.01,0.01);
//求出直线上的两个点
double k_line = fitline[1]/fitline[0];
Point p1(0,k_line*(0 - fitline[2]) + fitline[3]);
Point p2(img_width - 1,k_line*(img_width - 1 - fitline[2]) + fitline[3]);
//显示拟合出的直线方程:y=k_line*(x-fitline[2])+fitline[3]
char text_equation[1024];
sprintf(text_equation,"y-%.2f=%.2f(x-%.2f)",fitline[3],k_line,fitline[2]);
putText(src_image,text_equation,Point(30,50),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(0,0,255),1,8);
//显示拟合出的直线
line(src_image,p1,p2,Scalar(0,0,255),2);
imshow("原图+拟合结果", src_image);
waitKey();
return 0;
}
使用这种方式,只能在数据基本正确时,才能得到较好的拟合结果,当数据中存在噪声数据时,使用RANSAC的方式进行拟合是较好的拟合方式。
2、RANSAC二维直线拟合实现
二维直线的一般方程为:Ax+By+C=0。
若:已知直线上的两点P1(X1,Y1)和P2(X2,Y2), P1和P2两点不重合;
则:
该式子对所有的直线方程均满足。
C++实现主要函数:
//mTrajectoryParams.MaxModelFitIterations=2000
int FitLineRansac(std::vector<cv::Point>& points, cv::Vec3f& line)
{
int PointsNum = 2;
std::vector<std::vector<size_t>> mvSets = std::vector<std::vector<size_t>>(mTrajectoryParams.MaxModelFitIterations, std::vector<size_t>(PointsNum, 0));
//点的对数
const int N = points.size();
//新建一个容器vAllIndices存储点云索引,并预分配空间
std::vector<size_t> vAllIndices;
vAllIndices.reserve(N);
//在RANSAC的某次迭代中,还可以被抽取来作为数据样本的索引,所以这里起的名字叫做可用的索引
std::vector<size_t> vAvailableIndices;
//初始化所有特征点对的索引,索引值0到N-1
for (int i = 0; i < N; i++)
vAllIndices.push_back(i);
//用于进行随机数据样本采样,设置随机数种子
SeedRandOnce(0);
//开始每一次的迭代
for (int it = 0; it < mTrajectoryParams.MaxModelFitIterations; it++)
{
//迭代开始的时候,所有的点都是可用的
vAvailableIndices = vAllIndices;
//选择最小的数据样本集
for (size_t j = 0; j < PointsNum; j++)
{
// 随机产生一对点的id,范围从0到N-1
int randi = RandomInt(0, vAvailableIndices.size() - 1);
// idx表示哪一个索引对应的点对被选中
int idx = vAvailableIndices[randi];
//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中
mvSets[it][j] = idx;
// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,
// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素
// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}//依次提取出6个点
}//迭代mMaxIterations次,选取各自迭代时需要用到的最小数据集
int BestMatch = 0;
std::multiset<float> verror;
for (int it = 0; it < mTrajectoryParams.MaxModelFitIterations; it++)
{
std::vector<cv::Point> pts;
//选择6个点对进行迭代
for (size_t j = 0; j < PointsNum; j++)
{
//从mvSets中获取当前次迭代的某个特征点对的索引信息
int idx = mvSets[it][j];
cv::Point pt = points[idx];
pts.push_back(pt);
}
//Ax+By+C=0
float A = pts[1].y - pts[0].y;
float B = pts[0].x - pts[1].x;
float C = pts[1].x*pts[0].y - pts[0].x*pts[1].y;
int Match = 0;
verror.clear();
for (int i = 0; i < points.size(); i++)
{
float error = (A * points[i].x + B * points[i].y + C) / sqrt(A * A + B * B);
if (abs(error) < 1)
Match++;
verror.insert(error);
}
if (Match > BestMatch)
{
BestMatch = Match;
line = cv::Vec3f(A, B, C);
}
}
return 1;
}
随机处理函数:
//随机处理
static bool m_already_seeded = false;
inline void SeedRand(int seed)
{
srand(seed);
}
inline void SeedRandOnce(int seed)
{
//if (!m_already_seeded)
//{
SeedRand(seed);
m_already_seeded = true;
//}
}
inline int RandomInt(int min, int max)
{
int d = max - min + 1;
return int(((double)rand() / ((double)RAND_MAX + 1.0)) * d) + min;
}
绘制直线的例子:
cv::Vec3f Line;
FitLineRansac(points, Line);
cv::Mat imdraw = im.clone();
cv::cvtColor(imdraw, imdraw, cv::COLOR_GRAY2BGR);
cv::Point pt0, pt1;
if (abs(Line[0] / Line[1]) < 0.067)
{
pt0 = cv::Point(0, -Line[2] / Line[1]);
pt1 = cv::Point(imdraw.cols - 1, -Line[2] / Line[1]);
}
else if (abs(Line[0] / Line[1]) > 15)
{
pt0 = cv::Point(-Line[2] / Line[0], 0);
pt1 = cv::Point(-Line[2] / Line[0], imdraw.rows - 1);
}
else
{
pt0 = cv::Point(0, -Line[2] / Line[1]);
pt1 = cv::Point(-Line[2] / Line[0], 0);
}
cv::line(imdraw, pt0, pt1, cv::Scalar(0, 255, 0), 1, 8);