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"); }