void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
{
Mat X1 = _m1.getMat(), X2 = _m2.getMat(), model = _model.getMat();
const Point2d* x1ptr = X1.ptr<Point2d>();
const Point2d* x2ptr = X2.ptr<Point2d>();
int n = X1.checkVector(2);
Matx33d E(model.ptr<double>());
_err.create(n, 1, CV_32F);
Mat err = _err.getMat();
for (int i = 0; i < n; i++)
{
Vec3d x1(x1ptr[i].x, x1ptr[i].y, 1.);
Vec3d x2(x2ptr[i].x, x2ptr[i].y, 1.);
Vec3d Ex1 = E * x1;//这个理论上是计算的在第二张图上的对极线系数 就是a0x + b0y+c0 = 0的 a0 b0 c0
Vec3d Etx2 = E.t() * x2;//理论上是计算在第一张图中对极线的系数 多视几何P162 有公式 a1x + b1y+c1 = 0的 a1 b1 c1
double x2tEx1 = x2.dot(Ex1);
double a = Ex1[0] * Ex1[0];//a0平方
double b = Ex1[1] * Ex1[1];//b0平方
double c = Etx2[0] * Etx2[0];//a1平方
double d = Etx2[1] * Etx2[1];//b1平方
//下面的这个公式就有点不理解了 呵呵。因为点到直线距离公式应该是
//x2tEx1 / sqrt(a0 * a0 + b0 *b0) + x1.dot(Etx2 )/ sqrt(a1 * a1 + b1 *b1) 才对啊
err.at<float>(i) = (float)(x2tEx1 * x2tEx1 / (a + b + c + d));
}
}