霍夫变换查找直线和一些简单的几何筛选规则

{//*******绘制简单的顿消失点******lee**********************
cv::Mat GrayImage, BlurGrayImage, EdgeImage;
  //********第一步：GetLines
//  if(VPim.channels()<3) //this should be always true
//         cvtColor(VPim,VPim,CV_GRAY2BGR);

// 	// Converting to grayscale
// 	cv::cvtColor(VPim, GrayImage, cv::COLOR_BGR2GRAY);
	// Blurring image to reduce noise.
	cv::GaussianBlur(VPim, BlurGrayImage, cv::Size(5, 5), 1);
	// Generating Edge image
	cv::Canny(BlurGrayImage, EdgeImage, 40, 255);
  
	// Finding Lines in the image
	std::vector<cv::Vec4i> Lines;
	cv::HoughLinesP(EdgeImage,//image为输入图像，要求是单通道，8位图像
	 Lines, //lines为输出参数，4个元素表示，即直线的起始和终止端点的4个坐标（x1,y1）,(x2,y2)
	 1, //rho为距离分辨率，一般为1
	 CV_PI / 180,//heta为角度的分辨率，一般CV_PI/180
	  40, //threshold为阈值，hough变换图像空间值最大点，大于则执行
	  15);//minLineLength为最小直线长度（像素），即如果小于该值，则不被认为是一条直线
	  //maxLineGap为直线间隙最大值，如果两条直线间隙大于该值，则被认为是两条线段，否则是一条。
	
	// Check if lines found and exit if not.
	if (Lines.size() == 0)
	{
		std::cout << "Not enough lines found in the image for Vanishing Point detection." << std::endl;
		// exit(3);
	}
//********第二步： FilterLines
	//Filtering Lines wrt（关于：with regard to） angle
	std::vector<std::vector<double>> FilteredLines;
	std::vector<std::vector<double>> FinalLines;

	for (int i = 0; i < Lines.size(); i++)
	{
		cv::Vec4i Line = Lines[i];
		int x1 = Line[0], y1 = Line[1];
		int x2 = Line[2], y2 = Line[3];
		
		double m, c;

		// Calculating equation of the line : y = mx + c
		if (x1 != x2)
			m = (double)(y2 - y1) / (double)(x2 - x1);
		else
			m = 100000000.0;
		c = y2 - m * x2;
		
		// theta will contain values between - 90 -> + 90.
		double theta = atan(m) * (180.0 / M_PI);

		/*# Rejecting lines of slope near to 0 degree or 90 degree and storing others
        if REJECT_DEGREE_TH <= abs(theta) <= (90 - REJECT_DEGREE_TH):
            l = math.sqrt( (y2 - y1)**2 + (x2 - x1)**2 )    # length of the line
            FinalLines.append([x1, y1, x2, y2, m, c, l])*/
		// Rejecting lines of slope near to 0 degree or 90 degree and storing others
		if (REJECT_DEGREE_TH <= abs(theta) && abs(theta) <= (90.0 - REJECT_DEGREE_TH))
		{
			double l = pow((pow((y2 - y1), 2) + pow((x2 - x1), 2)), 0.5);	// length of the line
			std::vector<double> FinalLine{ (double)x1, (double)y1, (double)x2, (double)y2, m, c, l };
			FinalLines.push_back(FinalLine);
		}
	}

// 	因此，在我们的方法中，我们将从之后获得的所有行中只选取最长的15行
// 在上一步中对它们进行过滤(只有当行数多于15时，否则小于15排队，我们会把他们都带走)。数字“15”只是一个我认为正确的参数
// 我们的计算消失点，但你可以改变它根据你的愿望。为了获得最长的行，我们首先使用
// sort函数，然后我们分割出前15行(最长的15行)。
	if (FinalLines.size() > 15)
	{
		std::sort(FinalLines.begin(), FinalLines.end(), 
				  [](const std::vector< double >& a, 
					 const std::vector< double >& b) 
					{ return a[6] > b[6]; });
	  FilteredLines = std::vector<std::vector<double>>(FinalLines.begin(), FinalLines.begin() + 15);
	}
//********第三步： GetVanishingPoint
// We will apply RANSAC inspired algorithm for this.We will take combination
	// of 2 lines one by one, find their intersection point, and calculate the
	// total error(loss) of that point.Error of the point means root of sum of
	// squares of distance of that point from each line.
	int* VanishingPoint = new int[2];
	VanishingPoint[0] = -1; VanishingPoint[1] = -1;

	double MinError = 1000000000.0;

	for (int i = 0; i < FilteredLines.size(); i++) //遍历lines，一次取2个
	{
		for (int j = i + 1; j < FilteredLines.size(); j++)
		{
			//获取直线的 m，c； y=mx+c
			double m1 = FilteredLines[i][4], c1 = FilteredLines[i][5];
			double m2 = FilteredLines[j][4], c2 = FilteredLines[j][5];

			if (m1 != m2) //如果俩条直线不平行
			{   //直线L1和L2它们的交点是(x0, y0)
				double x0 = (c1 - c2) / (m2 - m1);
				double y0 = m1 * x0 + c1;

				double err = 0;
				// 迭代所有lines进行错误计算
				for (int k = 0; k < FilteredLines.size(); k++)
				{
					double m = FilteredLines[k][4], c = FilteredLines[k][5];
					double m_ = (-1 / m);
					double c_ = y0 - m_ * x0;//过交点(x0, y0)，斜率k=(-1 / m)的直线L_ 
              //求直线L和直线L_的 交点（x_ ， y_）
					double x_ = (c - c_) / (m_ - m);
					double y_ = m_ * x_ + c_;
           //直线L1和直线L2的 交点到L的距离就是交点(x0, y0)到点（x_ ， y_）的距离
					double l = pow((pow((y_ - y0), 2) + pow((x_ - x0), 2)), 0.5);

					err += pow(l, 2);
				}

				err = pow(err, 0.5);

				if (MinError > err)
				{
					MinError = err;
					VanishingPoint[0] = (int)x0;
					VanishingPoint[1] = (int)y0;
                    FD_vpx = VanishingPoint[0];
                    FD_vpy = VanishingPoint[1];
				}
			}
		}
	}
	// if (VanishingPoint[0] == -1 && VanishingPoint[1] == -1)
	// 	{
	// 		// continue;
	// 	}

		// Drawing lines and vanishing point
		for (int i = 0; i < FilteredLines.size(); i++)
		{
			std::vector<double> Line = FilteredLines[i];
			cv::line(VPim, cv::Point((int)Line[0], (int)Line[1]), cv::Point((int)Line[2], (int)Line[3]), cv::Scalar(0, 255, 0), 2);
		}
		cv::circle(VPim, cv::Point(FD_vpx, FD_vpy), 10, cv::Scalar(0, 0, 255), -1);
 cv::imshow("VP on VPim", VPim);
            cv::waitKey(1);
}//*******绘制霍夫变换找到的消失点end******lee**********************
# endif