public void Test_CameraMatrix_Jacobian()
{
PrepareCameraMatrix();
var points = GenerateCalibrationPoints_Random();
PrepareCalibrator(
AddNoise(points, _varianceReal, _varianceImage));
_calib.HomoPoints();
//_calib.NormalizeImagePoints();
// _calib.NormalizeRealPoints();
_calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
// _calib.FindNormalisedVariances();
_eCM = _calib.CameraMatrix;
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]);
Assert.IsTrue((ip - cp.Img).Length() < 0.4);
}
_calib.DecomposeCameraMatrix();
var miniAlg = _calib._miniAlg;
_calib.PrepareMinimalisationAlg();
miniAlg.Init();
miniAlg.DoComputeJacobianNumerically = false;
Matrix<double> testedJacobian = new DenseMatrix(miniAlg.MeasurementsVector.Count, miniAlg.ParametersVector.Count);
miniAlg.ComputeJacobian(testedJacobian);
miniAlg.DoComputeJacobianNumerically = true;
Matrix<double> numericJacobian = new DenseMatrix(miniAlg.MeasurementsVector.Count, miniAlg.ParametersVector.Count);
miniAlg.ComputeJacobian(numericJacobian);
int size = testedJacobian.RowCount * testedJacobian.ColumnCount;
double jacobian_diff = numericJacobian.PointwiseDivide_NoNaN(testedJacobian).FrobeniusNorm();
Assert.IsTrue(Math.Abs(jacobian_diff - Math.Sqrt(size)) < Math.Sqrt(size) / 100.0 || // 1% diffrence max
(numericJacobian - testedJacobian).FrobeniusNorm() < 1e-6,
"Analitical and numeric jacobians differ");
}