private static double CalibrateColorCamera()

in Sources/Kinect/Microsoft.Psi.Kinect.Windows/KinectInternalCalibration.cs [328:457]
104 lines of code
6 McCabe index (conditional complexity)

        private static double CalibrateColorCamera(List<Vector<double>> worldPoints, List<System.Drawing.PointF> imagePoints, Matrix<double> cameraMatrix, Vector<double> distCoeffs, Vector<double> rotation, Vector<double> translation, bool silent = true)
        {
            int nPoints = worldPoints.Count;

            {
                Matrix<double> R;
                Vector<double> t;
                DLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, out R, out t);
                var r = MatrixToAxisAngle(R);
                r.CopyTo(rotation);
                t.CopyTo(translation);
            }

            // pack parameters into vector
            // parameters: fx, fy, cx, cy, k1, k2, + 3 for rotation, 3 translation = 12
            int nParameters = 12;
            var parameters = Vector<double>.Build.Dense(nParameters);
            {
                int pi = 0;
                parameters[pi++] = cameraMatrix[0, 0]; // fx
                parameters[pi++] = cameraMatrix[1, 1]; // fy
                parameters[pi++] = cameraMatrix[0, 2]; // cx
                parameters[pi++] = cameraMatrix[1, 2]; // cy
                parameters[pi++] = distCoeffs[0]; // k1
                parameters[pi++] = distCoeffs[1]; // k2
                parameters[pi++] = rotation[0];
                parameters[pi++] = rotation[1];
                parameters[pi++] = rotation[2];
                parameters[pi++] = translation[0];
                parameters[pi++] = translation[1];
                parameters[pi++] = translation[2];

            }

            // size of our error vector
            int nValues = nPoints * 2; // each component (x,y) is a separate entry

            LevenbergMarquardt.Function function = delegate (Vector<double> p)
            {
                var fvec = Vector<double>.Build.Dense(nValues);

                // unpack parameters
                int pi = 0;
                double fx = p[pi++];
                double fy = p[pi++];
                double cx = p[pi++];
                double cy = p[pi++];

                double k1 = p[pi++];
                double k2 = p[pi++];

                var K = Matrix<double>.Build.DenseIdentity(3, 3);
                K[0, 0] = fx;
                K[1, 1] = fy;
                K[0, 2] = cx;
                K[1, 2] = cy;

                var d = Vector<double>.Build.Dense(5, 0);
                d[0] = k1;
                d[1] = k2;

                var r = Vector<double>.Build.Dense(3);
                r[0] = p[pi++];
                r[1] = p[pi++];
                r[2] = p[pi++];

                var t = Vector<double>.Build.Dense(3);
                t[0] = p[pi++];
                t[1] = p[pi++];
                t[2] = p[pi++];

                var R = AxisAngleToMatrix(r);

                int fveci = 0;
                for (int i = 0; i < worldPoints.Count; i++)
                {
                    // transform world point to local camera coordinates
                    var x = R * worldPoints[i];
                    x += t;

                    // fvec_i = y_i - f(x_i)
                    double u, v;
                    KinectInternalCalibration.Project(K, d, x[0], x[1], x[2], out u, out v);

                    var imagePoint = imagePoints[i];
                    fvec[fveci++] = imagePoint.X - u;
                    fvec[fveci++] = imagePoint.Y - v;
                }
                return fvec;
            };

            // optimize
            var calibrate = new LevenbergMarquardt(function);
            while (calibrate.State == LevenbergMarquardt.States.Running)
            {
                var rmsError = calibrate.MinimizeOneStep(parameters);
                if (!silent) Console.WriteLine("rms error = " + rmsError);
            }
            if (!silent)
            {
                for (int i = 0; i < nParameters; i++)
                    Console.WriteLine(parameters[i] + "\t");
                Console.WriteLine();
            }
            // unpack parameters
            {
                int pi = 0;
                double fx = parameters[pi++];
                double fy = parameters[pi++];
                double cx = parameters[pi++];
                double cy = parameters[pi++];
                double k1 = parameters[pi++];
                double k2 = parameters[pi++];
                cameraMatrix[0, 0] = fx;
                cameraMatrix[1, 1] = fy;
                cameraMatrix[0, 2] = cx;
                cameraMatrix[1, 2] = cy;
                distCoeffs[0] = k1;
                distCoeffs[1] = k2;
                rotation[0] = parameters[pi++];
                rotation[1] = parameters[pi++];
                rotation[2] = parameters[pi++];
                translation[0] = parameters[pi++];
                translation[1] = parameters[pi++];
                translation[2] = parameters[pi++];
            }


            return calibrate.RMSError;
        }