예제 #1
0
        public double MinimizeOneStep(Matrix parameters)
        {
            // initial value of the function; callee knows the size of the returned vector
            var errorVector = function(parameters);
            var error       = errorVector.Dot(errorVector);

            // Jacobian; callee knows the size of the returned matrix
            var J = jacobianFunction(parameters);

            // J'*J
            var JtJ = new Matrix(parameters.Size, parameters.Size);

            //stopWatch.Restart();
            //JtJ.MultATA(J, J); // this is the big calculation that could be parallelized
            JtJ.MultATAParallel(J, J);
            //Console.WriteLine("JtJ: J size {0}x{1} {2}ms", J.Rows, J.Cols, stopWatch.ElapsedMilliseconds);

            // J'*error
            var JtError = new Matrix(parameters.Size, 1);

            //stopWatch.Restart();
            JtError.MultATA(J, errorVector); // error vector must be a column vector
            //Console.WriteLine("JtError: errorVector size {0}x{1} {2}ms", errorVector.Rows, errorVector.Cols, stopWatch.ElapsedMilliseconds);



            // allocate some space
            var JtJaugmented  = new Matrix(parameters.Size, parameters.Size);
            var JtJinv        = new Matrix(parameters.Size, parameters.Size);
            var delta         = new Matrix(parameters.Size, 1);
            var newParameters = new Matrix(parameters.Size, 1);

            // find a value of lambda that reduces error
            double lambda = initialLambda;

            while (true)
            {
                // augment J'*J: J'*J += lambda*(diag(J))
                JtJaugmented.Copy(JtJ);
                for (int i = 0; i < parameters.Size; i++)
                {
                    JtJaugmented[i, i] = (1.0 + lambda) * JtJ[i, i];
                }

                //WriteMatrixToFile(errorVector, "errorVector");
                //WriteMatrixToFile(J, "J");
                //WriteMatrixToFile(JtJaugmented, "JtJaugmented");
                //WriteMatrixToFile(JtError, "JtError");


                // solve for delta: (J'*J + lambda*(diag(J)))*delta = J'*error
                JtJinv.Inverse(JtJaugmented);
                delta.Mult(JtJinv, JtError);

                // new parameters = parameters - delta [why not add?]
                newParameters.Sub(parameters, delta);

                // evaluate function, compute error
                var    newErrorVector = function(newParameters);
                double newError       = newErrorVector.Dot(newErrorVector);

                // if error is reduced, divide lambda by 10
                bool improvement;
                if (newError < error)
                {
                    lambda     /= lambdaIncrement;
                    improvement = true;
                }
                else // if not, multiply lambda by 10
                {
                    lambda     *= lambdaIncrement;
                    improvement = false;
                }

                // termination criteria:
                // reduction in error is too small
                var diff = new Matrix(errorVector.Size, 1);
                diff.Sub(errorVector, newErrorVector);
                double diffSq     = diff.Dot(diff);
                double errorDelta = Math.Sqrt(diffSq / error);

                if (errorDelta < minimumReduction)
                {
                    state = States.ReductionStepTooSmall;
                }

                // lambda is too big
                if (lambda > maximumLambda)
                {
                    state = States.LambdaTooLarge;
                }

                // change in parameters is too small [not implemented]

                // if we made an improvement, accept the new parameters
                if (improvement)
                {
                    parameters.Copy(newParameters);
                    error = newError;
                    break;
                }

                // if we meet termination criteria, break
                if (state != States.Running)
                {
                    break;
                }
            }

            rmsError = Math.Sqrt(error / errorVector.Size);
            return(rmsError);
        }