public void Test_EstimateCameraMatrix_Minimised()
{
PrepareCameraMatrix();
var points = GenerateCalibrationPoints_Random(100);
var noisedPoints = AddNoise(points, _varianceReal, _varianceImage, 200);
PrepareCalibrator(noisedPoints);
_calib.HomoPoints();
_calib.NormalizeImagePoints();
_calib.NormalizeRealPoints();
_calib._pointsNormalised = true;
_calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
// _calib.FindNormalisedVariances();
_calib.DenormaliseCameraMatrix();
_calib._pointsNormalised = false;
_eCM = _calib.CameraMatrix;
double totalDiff = 0.0;
for(int p = 0; p < _pointsCount; ++p)
{
var cp = points[p];
Vector<double> rp = new DenseVector(4);
rp[0] = cp.RealX;
rp[1] = cp.RealY;
rp[2] = cp.RealZ;
rp[3] = 1.0;
var imagePoint = _eCM * rp;
Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
totalDiff += (ip - cp.Img).Length();
Assert.IsTrue((ip - cp.Img).Length() < 1.0,
"Point after linear estimation too far : " + (ip - cp.Img).Length());
}
_calib.HomoPoints();
_calib.NormalizeImagePoints();
_calib.NormalizeRealPoints();
_calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
_calib._pointsNormalised = true;
_calib.FindNormalisedVariances();
_calib.UseCovarianceMatrix = true;
var lecm = _eCM.Clone();
// Disturb camera matrix a little
// _calib.CameraMatrix = AddNoise(_calib.CameraMatrix);
_calib._miniAlg.DoComputeJacobianNumerically = true;
_calib._miniAlg.NumericalDerivativeStep = 1e-4;
_calib.MinimizeError();
// _calib.DenormaliseCameraMatrix();
_calib.DecomposeCameraMatrix();
_eCM = _calib.CameraMatrix;
var errVec = _eCM.PointwiseDivide_NoNaN(lecm);
double err = errVec.L2Norm();
double scaleK = 1.0 / _calib.CameraInternalMatrix[2, 2];
_eCM.MultiplyThis(-scaleK);
var eK = _calib.CameraInternalMatrix.Multiply(scaleK);
var eR = -_calib.CameraRotationMatrix;
var eC = -(_eCM.SubMatrix(0, 3, 0, 3).Inverse() * _eCM.Column(3));
Matrix<double> eExt = new DenseMatrix(3, 4);
eExt.SetSubMatrix(0, 0, eR);
eExt.SetColumn(3, -eR * eC);
var eCM = eK * eExt;
// var errVec = _CM.PointwiseDivide_NoNaN(_eCM);
// double err = errVec.L2Norm();
// Assert.IsTrue(
// Math.Abs(err - Math.Sqrt(12)) < Math.Sqrt(12) / 1000.0 || // max 0.1% diffrence
// (_eCM - _CM).FrobeniusNorm() < 1e-3);
double estDiff = 0;
for(int p = 0; p < _pointsCount; ++p)
{
var cp = points[p];
Vector<double> rp = new DenseVector(4);
rp[0] = cp.RealX;
rp[1] = cp.RealY;
rp[2] = cp.RealZ;
rp[3] = 1.0;
var imagePoint = _eCM * rp;
Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
estDiff += (ip - cp.Img).Length();
Assert.IsTrue((ip - cp.Img).Length() < 1.5,
"Point after error minimalisation too far : " + (ip - cp.Img).Length());
}
var minialg = _calib._miniAlg;
// Test conovergence :
// ||mX-rX|| = ||mX-eX|| + ||rX-eX|| (or squared??)
// rX - real point from 'points'
// mX - measured point, noised
// eX = estimated X from result vector for 3d points and ePeX for image point
double len2_mr = 0;
double len2_me = 0;
double len2_re = 0;
for(int i = 0; i < points.Count; ++i)
{
double rX = points[i].RealX;
double rY = points[i].RealY;
double rZ = points[i].RealZ;
double rx = points[i].ImgX;
double ry = points[i].ImgY;
double mX = noisedPoints[i].RealX;
double mY = noisedPoints[i].RealY;
double mZ = noisedPoints[i].RealZ;
double mx = noisedPoints[i].ImgX;
double my = noisedPoints[i].ImgY;
double eX = minialg.BestResultVector[3 * i + 12];
double eY = minialg.BestResultVector[3 * i + 13];
double eZ = minialg.BestResultVector[3 * i + 14];
Vector<double> rp = new DenseVector(4);
rp[0] = eX;
rp[1] = eY;
rp[2] = eZ;
rp[3] = 1.0;
var imagePoint = _eCM * rp;
double ex = imagePoint[0] / imagePoint[2];
double ey = imagePoint[1] / imagePoint[2];
len2_re += (rX - eX) * (rX - eX);
len2_re += (rY - eY) * (rY - eY);
len2_re += (rZ - eZ) * (rZ - eZ);
len2_re += (rx - ex) * (rx - ex);
len2_re += (ry - ey) * (ry - ey);
len2_me += (mX - eX) * (mX - eX);
len2_me += (mY - eY) * (mY - eY);
len2_me += (mZ - eZ) * (mZ - eZ);
len2_me += (mx - ex) * (mx - ex);
len2_me += (my - ey) * (my - ey);
len2_mr += (rX - mX) * (rX - mX);
len2_mr += (rY - mY) * (rY - mY);
len2_mr += (rZ - mZ) * (rZ - mZ);
len2_mr += (rx - mx) * (rx - mx);
len2_mr += (ry - my) * (ry - my);
}
Assert.IsTrue( Math.Abs(len2_mr - len2_re - len2_me) < len2_mr/100.0 ||
Math.Abs(Math.Sqrt(len2_mr) - Math.Sqrt(len2_re) - Math.Sqrt(len2_me)) < Math.Sqrt(len2_mr) / 100.0,
"Triangle test failed."+" Points distances: LinearDiff = " + totalDiff +
". MiniDiff = " + estDiff);
Assert.IsTrue(estDiff < totalDiff,
"Points after minimalisation are too far. LinearDiff = " + totalDiff +
". MiniDiff = " + estDiff);
}
