线性归一化DLT算法C++，opencv实现

我们使用了opencv2.4.13实现了《计算机视觉中的多视图几何》一书中提到的线性归一化DLT算法，不想说废话，代码如下，且该代码我在VS2012中运行成功：

#include
#include
#include
#include
using namespace cv;
using namespace std;
class DLT
{
private:
	vectorpoint1,point2;
	void norm_point(vector&point,Mat &T);//归一化数据
	Mat A;
	Mat T1,T2;
	Mat _dlt();
public:
	double dlt(vector&_point1,vector&_point2,Mat &H);
                  //H为二维影射变化，返回的值是误差，我将误差定义为|Ah|/|h|
};
void DLT::norm_point(vector&point,Mat &T)
{
	double num=point.size();
	double E[2]={0,0},E2[2]={0,0},D[2]={0,0};
	int i;
	for(i=0;i(0,0)=1.0/sqrt(D[0]);
	T.at(0,2)=-1.0*E[0]/sqrt(D[0]);
	T.at(1,1)=1.0/sqrt(D[1]);
	T.at(1,2)=-1.0*E[1]/sqrt(D[1]);
	T.at(2,2)=1;
	for(i=0;i(0,0)+T.at(0,2);
		point[i].y=point[i].y*T.at(1,1)+T.at(1,2);
	}
}
double DLT::dlt(vector&_point1,vector&_point2,Mat &H)
{
	point1.assign(_point1.begin(),_point1.end());
	point2.assign(_point2.begin(),_point2.end()); 
	H=Mat::ones(3,3,CV_64FC1);
	Mat h=_dlt();
	int i;
	for(i=0;i<9;i++)
		H.at(i/3,i%3)=h.at(0,i);
	H=T2.inv()*H*T1;
	H=H*(1.0/H.at(2,2));
	h=h*(1.0/h.at(0,8));
	double error=norm(A*(h.t()))/norm(h);
	return error;
}
Mat DLT::_dlt()
{
	norm_point(point1,T1);
	norm_point(point2,T2);
	A=Mat::zeros(point1.size()*2,9,CV_64FC1);
	int i;
	for(i=0;i(2*i+0,3)=-point1[i].x;
		A.at(2*i+0,4)=-point1[i].y;
		A.at(2*i+0,5)=-1;
		A.at(2*i+0,6)=point2[i].y*point1[i].x;
		A.at(2*i+0,7)=point2[i].y*point1[i].y;
		A.at(2*i+0,8)=point2[i].y;

		A.at(2*i+1,0)=point1[i].x;
		A.at(2*i+1,1)=point1[i].y;
		A.at(2*i+1,2)=1;
		A.at(2*i+1,6)=-point2[i].x*point1[i].x;
		A.at(2*i+1,7)=-point2[i].x*point1[i].y;
		A.at(2*i+1,8)=-point2[i].x;
	}
	Mat evalues,evector;
	eigen(A.t()*A,evalues,evector);
	Mat ans=evector.row(evector.rows-1);
	return ans;
}