单视图几何Vanish Point（消失点/灭点）计算方法——Robert_T_Collins（罗伯特·柯林斯）算法

算法基本思想如下：

1）将每条直线的端点e1,e2，写成齐次坐标的形式e1=（x1_i,y1_i,w）,e2=（x1_i,y1_i,w）；则点的欧式坐标为（x1_i/w,y1_i/w）,（x2_i/w,y2_i/w）；常数w通常取1，当取0时，即代表该点处于无穷远处，这在欧式坐标里表示需要无穷远的符号。

2）将直线表示为其两个端点的叉乘的齐次坐标向量（说明白点就是，两个点齐次坐标的叉乘为过这两个点直线的齐次坐标方向向量），即一条直线可由两个点的叉乘表示。即直线

L1=(a_i,b_i,c_i)=e1 X e2；结果向量只是通过两个端点的平面直线的方程a_i x + b_i y + c_i = 0的参数

3）如果只有两条直线，L1和L2，两条直线的交点可由直线向量的叉乘表示，即V=L1 X L2

对V进行缩放，使最后一个坐标为1，即(Vx，Vy，1)，Vx和Vy作为图像中的消失点。然而，最好将消失点保留为齐次坐标向量，因为消失点可能离图像很远，甚至在无穷远处(在这种情况下，V的第三个分量是0，当您尝试缩放时，会得到除以0的结果)。

4）但如果有n条直线求交点，L1,L2,...,Ln，按照下面的方法，您可以获得“最佳拟合”消失点。

4-1）形成3×3的“二阶矩”矩阵M为

                       [a_i*a_i        a_i*b_i        a_i*c_i]

        M=sum   [a_i*b_i        b_i*b_i        c_i*b_i]

                       [a_i*c_i        b_i*c_i        c_i*c_i]

其中取i = 1到n的和。注意，M是对称矩阵。

4-2）对M矩阵进行特征值分解。使用雅可比方法执行M的特征分解。

4-3）与最小特征值相关联的特征向量是消失点的齐次坐标向量。

一个建议：如果你只是直接使用图像像素坐标，数值差异过大时，求解出来精度可能会很差。哈特利有一篇关于这个主题的好论文，题目类似“In defense of the Eight-point algorithm”如果你的线条在整个图像中展开，可以通过下面的操作得到他提到的那种条件良好的方法。

第一步：通过(-imageX/2，-imageY/2)进行平移，使(0，0)位于图像的中心[哈特利论文的方法会以线端点的质心为原点]。

第二步：缩放坐标，使所有像点齐次坐标的大小大致为1 [Hartley在他的论文中详细介绍了如何做到这一点]。我曾经通过设置常数w来近似地做到这一点，在这个算法的第一步中提到，它等于图像大小的一半(以像素为单位)。所以如果你有一个256x256的图像，那么你会有w = 128。如果图像的宽度和高度不一样，就取平均值什么的，然后除以二。

下面为代码实现：

Vec3d RobertColins(vector<Point2d> vecLinePoints,int cols,int rows)
{
	Vec3d vanishPoint;
	int size = vecLinePoints.size();
	//防止坐标点差异过大造成求解精度下降，进行偏移
	int offset = (cols + rows) / 4;
	//这里直线端点，以两个点为一组代表一条直线
	//因此size必为偶数
	//得出各条直线的方向向量
	vector <Vec3d> Lines;
	for (int i = 0; i < size/2; i++)
	{
		Vec3d startPoint, endPoint;
		startPoint[0] = (vecLinePoints[2 * i].x-offset)/offset;
		startPoint[1] = (vecLinePoints[2 * i].y-offset)/offset;
		startPoint[2] = 1;
		endPoint[0] = (vecLinePoints[2 * i + 1].x - offset)/offset;
		endPoint[1] = (vecLinePoints[2 * i + 1].y - offset)/offset;
		endPoint[2] = 1;
		//两个点坐标向量的叉乘等于，过该两点的直线齐次坐标向量
		Lines.push_back(startPoint.cross(endPoint));
	}
	/*声明一个Mat M形成3×3的“二阶矩”矩阵M为
	        [a_i*a_i        a_i*b_i        a_i*c_i]
	M = sum [a_i*b_i        b_i*b_i        c_i*b_i]
		    [a_i*c_i        b_i*c_i        c_i*c_i]
	其中取i = 1到n的和。注意，M是对称矩阵。*/
	Mat M = (Mat_<double>(3, 3)<<0,0,0,0,0,0,0,0,0);
	for (int i = 0; i < size / 2; i++)
	{
		Mat tempM = (Mat_<double>(3, 3));
		tempM.at<double>(0, 0) = Lines[i][0] * Lines[i][0];
		tempM.at<double>(0, 1) = Lines[i][0] * Lines[i][1];
		tempM.at<double>(0, 2) = Lines[i][0] * Lines[i][2];

		tempM.at<double>(1, 0) = Lines[i][1] * Lines[i][0];
		tempM.at<double>(1, 1) = Lines[i][1] * Lines[i][1];
		tempM.at<double>(1, 2) = Lines[i][1] * Lines[i][2];

		tempM.at<double>(2, 0) = Lines[i][2] * Lines[i][0];
		tempM.at<double>(2, 1) = Lines[i][2] * Lines[i][1];
		tempM.at<double>(2, 2) = Lines[i][2] * Lines[i][2];

		M = M + tempM;
	}
	//对M矩阵进行特征值分解
	Mat eigenvector = (Mat_<double>(3, 3));
	Mat eigenvalue = (Mat_<double>(3, 1));
	eigen(M, eigenvalue, eigenvector);
	vanishPoint[0] = eigenvector.at<double>(0, 2)*(1 / (eigenvector.at<double>(0, 2))) + offset;
	vanishPoint[1] = eigenvector.at<double>(1, 2)*(1 / (eigenvector.at<double>(0, 2))) + offset;
	vanishPoint[2] = 1;
	return vanishPoint;
}

代码算出来的结果，好像还有点问题，后续接着测试，总体思路没有问题。