通过消失点测量摄像头与平行线夹角
原理：
现实中平行的直线会在照片中交于一点（完全水平的除外)，通过该交点和摄像机位置的连线即可确定摄像头偏离平行线的夹角，可以参考修改为检测小车航线角（偏离车道线的夹角）。
效果图：
第一版程序（有bug）
/**************************************************************************************
通过消失点测量 ：航向角
****************************************************************************************/
#pragma warning(disable:4996)
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include //hog特征的c文件
//自己编写的文件
#include "UART.h"
#include "findline.h"
#include "DrawImage.h"//绘制图形
#include "Manipulator_positioning.h" //棋子挡板识别及定位
#include "Number.h"
#include "Locate.h"
using namespace std;
using namespace cv;
using namespace cv::ml;
RNG rng(12345);
using namespace cv;
using namespace std;
RNG g_rng(12345);
/*函数功能：求两条直线交点*/
/*输入：两条Vec4i类型直线*/
/*返回：Point2f类型的点*/
Point2f getCrossPoint(Vec4i LineA, Vec4i LineB)
{
double ka, kb;
ka = (double)(LineA[3] - LineA[1]) / (double)(LineA[2] - LineA[0]); //求出LineA斜率
kb = (double)(LineB[3] - LineB[1]) / (double)(LineB[2] - LineB[0]); //求出LineB斜率
Point2f crossPoint;
crossPoint.x = (ka*LineA[0] - LineA[1] - kb*LineB[0] + LineB[1]) / (ka - kb);
crossPoint.y = (ka*kb*(LineA[0] - LineB[0]) + ka*LineB[1] - kb*LineA[1]) / (ka - kb);
return crossPoint;
}
//输入一堆直线，返回每条直线与水平直线的角度，
//aaa为0则为弧度，反之则为角度
vector <float> get_lines_arctan(vectordouble)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
lines_arctan.push_back(atan(k));
}
return lines_arctan;
}
else
{
float k = 0; //直线斜率
vector <float> lines_arctan;//直线斜率的反正切值
for (unsigned int i = 0; idouble)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
lines_arctan.push_back((double)atan(k)* (double)180.0 / (double)3.1415926);
}
return lines_arctan;
}
}
//输入一堆直线，返回每条直线的斜率和截距
//Vec2f为2个点的float，参照存储直线的数据结构
vector double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率// -3.1415926/2-----+3.1415926/2
b = (double)lines[i][1] - k * (double)lines[i][0]; //求出直线的斜率
lines_fangcheng.push_back(Point2f(k,b));
}
return lines_fangcheng;
}
//原点到直线的距离
//输入一堆直线，返回直线到坐标原点的距离
vector <float> get_0_distance(vector1; i++)//n个数的数列总共扫描n-1次
{
for (int j = 0; j1; j++)//每一趟扫描到a[n-i-2]与a[n-i-1]比较为止结束
{
if (lines_temp[j]1])//后一位数比前一位数大的话，就交换两个数的位置（降序）
{
t=lines[j + 1];
t1 = lines_temp[j + 1];
lines[j + 1] = lines[j];
lines_temp[j + 1] = lines_temp[j];
lines[j] = t;
lines_temp[j] = t1;
}
}
}
return lines;
}
//-----------------------------------【main( )函数】--------------------------------------------
// 描述：控制台应用程序的入口函数，我们的程序从这里开始
//-----------------------------------------------------------------------------------------------
int main()
{
//【1】载入原始图和Mat变量定义
Mat srcImage = imread("11.jpg"); //工程目录下应该有一张名为1.jpg的素材图
Mat midImage, dstImage;//临时变量和目标图的定义
//彩色转灰度
cv::cvtColor(srcImage, srcImage, CV_BGR2GRAY);
cv::namedWindow("灰度化", 0);
imshow("灰度化", srcImage);
//二值化
threshold(srcImage, srcImage, 150, 255, THRESH_BINARY);
cv::namedWindow("二值化", 0);
imshow("二值化", srcImage);
//模糊
cv::blur(srcImage, srcImage, cv::Size(3, 3));
cv::namedWindow("模糊", 0);
imshow("模糊", srcImage);
/******* 检测直线优化 开始 ****************************************************************/
int cannyThreshold = 80;
float factor = 2.5;
vector lines;//定义一个矢量结构lines用于存放得到的线段矢量集合
//HoughLinesP(midImage, lines, 1, CV_PI / 180, 320, 240, 30);
printf("\n\n\t\t\t 当前使用的OpenCV版本为" CV_VERSION);
cout << "\n共检测到原始 直线" << lines.size() << "条" << endl;
//这里是将检测的线调整到延长至全屏，即射线的效果，其实可以不必这么做
for (unsigned int i = 0; i0] = 0;
lines[i][1] = ((float)v[1] - v[3]) / (v[0] - v[2])* -v[0] + v[1];
lines[i][2] = midImage.cols;
lines[i][3] = ((float)v[1] - v[3]) / (v[0] - v[2])*(midImage.cols - v[2]) + v[3];
}
//【4】依次在图中绘制出每条线段
for (size_t i = 0; i < lines.size(); i++)
{
Vec4i l = lines[i];
//此句代码的OpenCV2版为：
//line( dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(186,88,255), 1, CV_AA);
//此句代码的OpenCV3版为：
line(dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0, 0, 255), 1, LINE_AA);
}
//显示每条直线的角度
vector <float> lines_arctan;//直线斜率的反正切值,即角度
lines_arctan = get_lines_arctan(lines,0);//0为弧度,1为角度
for (unsigned int i = 0; i < lines.size(); i++)
{
cout << "检测到的直线弧度" << lines_arctan[i] << endl;
cout << "检测到的直线角度为：" << lines_arctan[i] *180.0/3.1415926<< endl;
}
//显示每条直线的方程
vector cout << "检测到的直线斜率为:" << lines_arctan1[i]<< " 度"<0]);
lines_2.push_back(lines_1[lines_1.size()-1]);
for (int i = 0; i < lines_2.size(); i++)
{
line(dstImage, Point(lines_2[i][0], lines_2[i][1]), Point(lines_2[i][2], lines_2[i][3]), Scalar(255, 0,0), 2, LINE_AA);
}
/****************确定自己在哪两条直线中间，并把这两条直线找出来 结束 *********************************************************************/
/****************找到消失点 开始 *********************************************************************/
Point2f Xiaoshidian;//线的交点存储
Xiaoshidian = getCrossPoint(lines_2[0], lines_2[1]);//消失点
float H1 = 44.0;//摄像头高度
float Dmin = 25;//最短距离
float Dmax = 250;//最长距离
float D1 = 29.8;//摄像头坐标与水平面线交点差值
float Alpha;//俯仰角Αα：阿尔法 Alpha
float Theta;//垂直视场角 西塔 Theta
Alpha = atan(Dmin / H1);//俯仰角
Theta = atan(Dmax / H1) - Alpha;//垂直视场角
//视场中轴线
line(dstImage, Point(dstImage.cols / 2,0 ), Point(dstImage.cols / 2, dstImage.rows), Scalar(255, 0, 0), 3, LINE_AA);
//与直线平行的地方
line(dstImage, Point(Xiaoshidian.x, Xiaoshidian.y), Point(dstImage.cols / 2, dstImage.rows*(1 + Alpha / Theta)), Scalar(255, 0, 255), 3, LINE_AA);
/****************找到消失点 开始 *********************************************************************/
vectorfor (unsigned int j = i + 1; jif (pt.x >= 0 && pt.y >= 0)
{
corners.push_back(pt);
}
}
}
//【4】依次在图中绘制出角点
for (size_t i = 0; i < corners.size(); i++)
{
circle(dstImage, corners[i], 3, CV_RGB(0, 255, 0), 2);
}
//【5】显示原始图
namedWindow("【原始图】", 0);//参数为零，则可以自由拖动
imshow("【原始图】", srcImage);
//【6】边缘检测后的图
namedWindow("【边缘检测后的图】", 0);//参数为零，则可以自由拖动
imshow("【边缘检测后的图】", midImage);
//【7】显示效果图
namedWindow("【效果图】", 0);//参数为零，则可以自由拖动
imshow("【效果图】", dstImage);
waitKey(0);
}
第二版程序（优化了很多）8月9日写
/**************************************************************************************
该程序功能：找航向角，通过消失点找航向角
****************************************************************************************/
/**************************************************************************************
通过消失点测量 ：航向角
****************************************************************************************/
#pragma warning(disable:4996)
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include //hog特征的c文件
//自己编写的文件
#include "UART.h"
#include "findline.h"
#include "DrawImage.h"//绘制图形
#include "Manipulator_positioning.h" //棋子挡板识别及定位
#include "Number.h"
#include "Locate.h"
using namespace std;
using namespace cv;
using namespace cv::ml;
RNG rng(12345);
using namespace cv;
using namespace std;
RNG g_rng(12345);
/*函数功能：求两条直线交点*/
/*输入：两条Vec4i类型直线*/
/*返回：Point2f类型的点*/
Point2f getCrossPoint(Vec4i LineA, Vec4i LineB)
{
double ka, kb;
Point2f crossPoint;
if ((LineA[2] - LineA[0]) != 0)
{
ka = (double)(LineA[3] - LineA[1]) / (double)(LineA[2] - LineA[0]); //求出LineA斜率
kb = (double)(LineB[3] - LineB[1]) / (double)(LineB[2] - LineB[0]); //求出LineB斜率
crossPoint.x = (ka*LineA[0] - LineA[1] - kb*LineB[0] + LineB[1]) / (ka - kb);
crossPoint.y = (ka*kb*(LineA[0] - LineB[0]) + ka*LineB[1] - kb*LineA[1]) / (ka - kb);
}
return crossPoint;
}
//输入一堆直线，返回每条直线与水平直线的角度，
//aaa为0则为弧度，反之则为角度
//
vector <float> get_lines_arctan(vectorif ((lines[i][2] - lines[i][0]) != 0)
{
k = -(double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
lines_arctan.push_back(atan(k));
}
else
{
lines_arctan.push_back(8000); //8000表示无穷大
}
}
return lines_arctan;
}
else
{
float k = 0; //直线斜率
vector <float> lines_arctan;//直线斜率的反正切值
for (unsigned int i = 0; iif ((lines[i][2] - lines[i][0]) != 0)
{
k = -(double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
lines_arctan.push_back((double)atan(k)* (double)180.0 / (double)3.1415926);
}
else
{
lines_arctan.push_back(90);
}
}
return lines_arctan;
}
}
//输入一堆直线，返回每条直线的斜率和截距
//Vec2f为2个点的float，参照存储直线的数据结构
//如果直线未垂直的话，返回斜率和截距都为8000
vector if ((lines[i][2] - lines[i][0]) != 0)
{
k = (double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率// -3.1415926/2-----+3.1415926/2
b = (double)lines[i][1] - k * (double)lines[i][0]; //求出直线的斜率
lines_fangcheng.push_back(Point2f(k, b));
}
else
{
lines_fangcheng.push_back(Point2f(8000, 8000)); //直线未竖直的，则返回斜率和截距都为8000
}
}
return lines_fangcheng;
}
//原点到直线的距离
//输入一堆直线，返回直线到坐标原点的距离
vector <float> get_0_distance(vector1; i++)//n个数的数列总共扫描n-1次
{
for (int j = 0; j1; j++)//每一趟扫描到a[n-i-2]与a[n-i-1]比较为止结束
{
if (lines_temp[j]1])//后一位数比前一位数大的话，就交换两个数的位置（降序）
{
t = lines[j + 1];
t1 = lines_temp[j + 1];
lines[j + 1] = lines[j];
lines_temp[j + 1] = lines_temp[j];
lines[j] = t;
lines_temp[j] = t1;
}
}
}
return lines;
}
//-----------------------------------【main( )函数】--------------------------------------------
// 描述：控制台应用程序的入口函数，我们的程序从这里开始
//-----------------------------------------------------------------------------------------------
int main()
{
//【1】载入原始图和Mat变量定义
Mat srcImage = imread("14.jpg"); //工程目录下应该有一张名为1.jpg的素材图
Mat midImage, dstImage;//临时变量和目标图的定义
//彩色转灰度
cv::cvtColor(srcImage, srcImage, CV_BGR2GRAY);
cv::namedWindow("灰度化", 0);
imshow("灰度化", srcImage);
//二值化
threshold(srcImage, srcImage, 150, 255, THRESH_BINARY);
cv::namedWindow("二值化", 0);
imshow("二值化", srcImage);
//模糊
cv::blur(srcImage, srcImage, cv::Size(3, 3));
cv::namedWindow("模糊", 0);
imshow("模糊", srcImage);
/******* 检测直线优化 开始 ****************************************************************/
int cannyThreshold = 80;
float factor = 2.5;
vector lines;//定义一个矢量结构lines用于存放得到的线段矢量集合
//HoughLinesP(midImage, lines, 1, CV_PI / 180, 320, 240, 30);
printf("\n\n\t\t\t 当前使用的OpenCV版本为" CV_VERSION);
cout << "\n共检测到原始 直线" << lines.size() << "条" << endl;
//这里是将检测的线调整到延长至全屏，即射线的效果，其实可以不必这么做
//for (unsigned int i = 0; i
//{
// cv::Vec4i v = lines[i];
// lines[i][0] = 0;
// lines[i][1] = ((float)v[1] - v[3]) / (v[0] - v[2])* -v[0] + v[1];
// lines[i][2] = midImage.cols;
// lines[i][3] = ((float)v[1] - v[3]) / (v[0] - v[2])*(midImage.cols - v[2]) + v[3];
//}
//【4】依次在图中绘制出每条线段
for (size_t i = 0; i < lines.size(); i++)
{
Vec4i l = lines[i];
//此句代码的OpenCV2版为：
//line( dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(186,88,255), 1, CV_AA);
//此句代码的OpenCV3版为：
line(dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0, 0, 255), 1, LINE_AA);
}
//显示每条直线的角度
vector <float> lines_arctan;//直线斜率的反正切值,即角度
lines_arctan = get_lines_arctan(lines, 1);//0为弧度,1为角度
for (unsigned int i = 0; i < lines.size(); i++)
{
// cout << "检测到的直线弧度" << lines_arctan[i] << endl;
cout << "检测到的直线角度为：" << lines_arctan[i] << endl;
}
//显示每条直线的方程
//vector lines_fangcheng;//直线斜率的反正切值
//lines_fangcheng = get_lines_fangcheng(lines);
//for (unsigned int i = 0; i < lines.size(); i++)
//{
// cout << "\n" << endl;
// cout << "检测到的直线斜率为" << lines_fangcheng[i].x << endl;
// cout << "检测到的直线截距为：" << lines_fangcheng[i].y << endl;
//}
/****************确定自己在哪两条直线中间，并把这两条直线找出来 开始 *********************************************************************/
vectorcout << "检测到的直线角度为:" << lines_arctan1[i] << " 度" << endl;
if (fabs(lines_arctan1[i])>40)
{
lines_2.push_back(lines_1[i]);
}
}
lines_arctan2 = get_lines_arctan(lines_2, 1);//0为弧度,1为角度
printf("排列好的第二次直线角度是：\n");
for (int i = 0; i < lines_2.size(); i++)
{
cout << "检测到的直线角度为:" << lines_arctan2[i] << " 度" << endl;
}
if (lines_2.size() >= 2)
{
lines_3.push_back(lines_2[0]);
lines_3.push_back(lines_1[lines_2.size() - 1]);
}
else
{
cout << "滤除后的直线个数不够"<< endl;
// continue;
}
//将最后计算用到的两条直线显示出来
for (int i = 0; i < lines_3.size(); i++)
{
line(dstImage, Point(lines_3[i][0], lines_3[i][1]), Point(lines_3[i][2], lines_3[i][3]), Scalar(255, 255, 0), 2, LINE_AA);
}
/****************确定自己在哪两条直线中间，并把这两条直线找出来 结束 *********************************************************************/
/****************找到消失点 开始 *********************************************************************/
Point2f Xiaoshidian;//线的交点存储
Xiaoshidian = getCrossPoint(lines_3[0], lines_3[1]);//消失点
float H1 = 44.0;//摄像头高度
float Dmin = 25;//最短距离
float Dmax = 250;//最长距离
float D1 = 29.8;//摄像头坐标与水平面线交点差值
float Alpha;//俯仰角Αα：阿尔法 Alpha
float Theta;//垂直视场角 西塔 Theta
Alpha = atan(Dmin / H1);//俯仰角
Theta = atan(Dmax / H1) - Alpha;//垂直视场角
//视场中轴线
line(dstImage, Point(dstImage.cols / 2, 0), Point(dstImage.cols / 2, dstImage.rows), Scalar(255, 0, 0), 3, LINE_AA);
//与直线平行的地方
line(dstImage, Point(Xiaoshidian.x, Xiaoshidian.y), Point(dstImage.cols / 2, dstImage.rows*(1 + Alpha / Theta)), Scalar(255, 0, 255), 3, LINE_AA);
/****************找到消失点 结束 *********************************************************************/
//vector corners;//线的交点存储
//for (unsigned int i = 0; i
//{
// for (unsigned int j = i + 1; j
// {
// cv::Point2f pt = getCrossPoint(lines[i], lines[j]);
// if (pt.x >= 0 && pt.y >= 0)
// {
// corners.push_back(pt);
// }
// }
//}
////【4】依次在图中绘制出角点
//for (size_t i = 0; i < corners.size(); i++)
//{
// circle(dstImage, corners[i], 3, CV_RGB(0, 255, 0), 2);
//}
//【5】显示原始图
namedWindow("【原始图】", 0);//参数为零，则可以自由拖动
imshow("【原始图】", srcImage);
//【6】边缘检测后的图
namedWindow("【边缘检测后的图】", 0);//参数为零，则可以自由拖动
imshow("【边缘检测后的图】", midImage);
//【7】显示效果图
namedWindow("【效果图】", 0);//参数为零，则可以自由拖动
imshow("【效果图】", dstImage);
waitKey(0);
}