Rename *ll* and *ul* to ll and ul in defint

[maxima.git] / share / nelder_mead / neldermead.lisp

(in-package :neldermead)

(defclass cached-simplex-data ()
  ((pseudopivot :initform nil)
   (q-factor :initform nil)
   (side-vectors :initform nil)
   (r-factor :initform nil)
   (dv :initform nil
       :initarg :dv
       :accessor dv)
   (dmin :initform nil
         :initarg :dmin
         :accessor dmin)
   (best-before-reshape :initform nil
                        :initarg :best-before-reshape
                        :accessor best-before-reshape)))

(defclass nm-simplex ()
  ((x :accessor x
      :initarg :x)
   (fx :accessor fx
       :initarg :fx)
   (pmap :accessor pmap
         :initarg :pmap)
   (data :accessor data
         :initform nil)))

(defclass grid ()
  ((z :accessor grid-z :initarg :z)
   (delta :accessor delta :initarg :delta)))

(defvar *verbose-level* 1)

(defmethod print-object ((s nm-simplex) stream)
  (format stream "#<NM-SIMPLEX")
  (case *verbose-level*
    (2 (let* ((fx (fx s))
              (pmap (pmap s))
              (fmt "~12,3,3E")
              (n (length pmap)))

         (dotimes (i n)
           (terpri stream)
           (if fx (format stream fmt (fk s i))
               (format stream " -- "))

           (format stream " :")
           (dotimes (j (- n 1))
             (format stream fmt (aref (xk s i) j))))))
    (1 (format stream " D=~A Best=~A " (dimension s)
               (if (fx s)
                   (format nil "~16,7,3E" (fk s 0))
                   "--"))))
  (format stream ">"))

(defmethod xk ((s nm-simplex) k)
  (aref (x s) (aref (pmap s) k)))

(defmethod (setf xk) (nv (s nm-simplex) k)
  (setf (aref (x s) (aref (pmap s) k)) nv))

(defmethod fk ((s nm-simplex) k)
  (aref (fx s) (aref (pmap s) k)))

(defmethod (setf fk) (nv (s nm-simplex) k)
  (setf (aref (fx s) (aref (pmap s) k)) nv))

(defmethod sort-simplex ((s nm-simplex))
  (sort (pmap s) #'< :key #'(lambda (k) (aref (fx s) k)))
  s)

(defmethod dimension ((s nm-simplex))
  (- (length (pmap s)) 1))

;; simple additive simplex generator
(defun initial-simplex (x0
                        &key (displace 0.1d0))
  (let* ((n (length x0))
         (x (make-array (+ n 1)))
         (pmap (make-array (+ n 1) :element-type 'fixnum)))

    (setf (aref x 0) x0
          (aref pmap 0) 0)

    (dotimes (k n)
      (let ((xk (copy-seq x0)))

        (incf (aref xk k)
              (if (numberp displace) displace
                  (aref displace k)))
        (setf (aref x (+ k 1)) xk
              (aref pmap (+ k 1)) (+ k 1))))

    (make-instance 'nm-simplex
                   :x x :fx nil :pmap pmap)))

;; The simplex generator from the article, more or less.
(defun default-initial-simplex (x0)
  (initial-simplex x0
                   :displace
                   (map 'vector
                        #'(lambda (v)
                            (max 0.00025d0
                                 (abs (* v 0.05d0))))
                        x0)))

(defmethod maybe-fill-simplex ((s nm-simplex) f)
  (if (fx s) s
      (let* ((n (length (x s)))
             (fx (make-array n :element-type 'double-float)))
        (loop :for i :from 0 :below n :do
          (setf (aref fx i) (funcall f (xk s i))))

        (setf (fx s) fx)
        s)))

;; substitutes the worst point with x/fx. Assumes simplex is sorted.
(defmethod improve ((s nm-simplex) x fx)
  (let ((last (length x)))
    (setf (xk s last) x
          (fk s last) fx

          (data s) nil)
          
    (sort-simplex s)))

(defmethod cached-slot ((s nm-simplex) slot computer)
  (if (and (data s) (slot-value (data s) slot))
      (slot-value (data s) slot)
      (setf (data s) (if (data s) (data s)
                         (make-instance 'cached-simplex-data))
            (slot-value (data s) slot) (funcall computer))))

(defmethod pseudopivot ((s nm-simplex))
  (cached-slot
   s 'pseudopivot
               
   #'(lambda ()
       (let* ((n (- (length (pmap s)) 1))
              (xbar (make-array n
                                :element-type 'double-float
                                :initial-element 0.0d0)))
         
         (dotimes (i n)
           (setf xbar (v+w*c xbar (xk s (+ i 1)) (/ 1.0d0 n))))

         xbar))))

(defmethod side-vectors ((s nm-simplex))
  (cached-slot
   s 'side-vectors

   #'(lambda ()
       (let* ((n (- (length (pmap s)) 1))
              (sv (make-array n)))

         (dotimes (i n)
           (setf (aref sv i) (v+w*c (xk s (+ i 1)) (xk s 0) -1)))

         sv))))

(defun simplex-qr-thing (sidev)
  (let* ((n (length sidev))
         (norms (make-array n :element-type 'double-float))
         (pmap (make-array n :element-type 'fixnum))
         (mat (make-array (list n n) :element-type 'double-float)))

    (dotimes (i n)
      (setf (aref pmap i) i
            (aref norms i) (norm (aref sidev i))))

    (sort pmap #'> :key #'(lambda (k) (aref norms k)))

    (dotimes (i n)
      (dotimes (j n)
        (setf (aref mat i j)
              (aref (aref sidev (aref pmap j)) i))))

    (multiple-value-bind (r q) (qr-factorization mat)
      (values q r pmap))))

(defun qrthing-closure (s n)
  #'(lambda ()
      (multiple-value-bind (q r p)
          (simplex-qr-thing (side-vectors s))
        (setf (slot-value (data s) 'q-factor) q
              (slot-value (data s) 'r-factor) r)

        (elt (list q r p) n))))

(defmethod q-factor ((s nm-simplex))
  (cached-slot s 'q-factor

               (qrthing-closure s 0)))

(defmethod r-factor ((s nm-simplex))
  (cached-slot s 'r-factor
               (qrthing-closure s 1)))

;; A Nelder-Mead iteration.
(defun nm-iteration
    (simplex f &key
     verbose
     (gamma_reflect 1.0d0)
     (gamma_expand 2.0d0)
     (gamma_outer_contraction 0.5d0)
     (gamma_inner_contraction -0.5d0)
     (gamma_shrink 0.5d0))


  (let* ((n (- (length (x simplex)) 1))

         (x_cb (make-array n
                           :element-type 'double-float
                           :initial-element 0.0d0))
         (x_cb-x_n (make-array n
                               :element-type 'double-float
                               :initial-element 0.0d0)))

    (labels ((newpoint (gamma)
               (let ((np (v+w*c x_cb x_cb-x_n gamma)))
                 (values np (funcall f np))))
               
             (accept (xx ff)
               (improve simplex xx ff))

             (shrink ()
               (let ((x0 (xk simplex 0)))
                 (loop for i from 1 to n do
                       (let* ((newx (v+w*c (v*c x0
                                                (- 1.0d0 gamma_shrink))
                                           (xk simplex i) gamma_shrink))
                              (newf (funcall f newx)))

                         (setf (xk simplex i) newx
                               (fk simplex i) newf)

                         (sort-simplex simplex))))))

      ;; compute centroid
      (dotimes (i n)
        (setf x_cb (v+w*c x_cb (xk simplex i) (/ 1.0d0 n))))

      (setf x_cb-x_n (v+w*c x_cb (xk simplex n) -1.0d0))

      ;; 2. Reflect
      (multiple-value-bind (xr fr)
          (newpoint gamma_reflect)
        (if (and (<= (fk simplex 0) fr) (< fr (fk simplex (- n 1))))
            (accept xr fr)
            ;; 3. expand
            (if (< fr (fk simplex 0))
                (multiple-value-bind (xe fe)
                    (newpoint gamma_expand)
                  (if (< fe fr)
                      (accept xe fe)
                      (accept xr fr)))
                ;; 4. contract or shrink
                (if (<= (fk simplex (- n 1)) fr)
                    (if (< fr (fk simplex n))
                        ;;outer contraction
                        (multiple-value-bind (xc fc)
                            (newpoint gamma_outer_contraction)
                          (if (or (= n 2)
                                  (<= fc fr)) ;; 1D shrink is
                              ;; equivalent to
                              ;; acceptance of this
                              ;; point
                              (accept xc fc)
                              (shrink)))
                        ;; inner contraction
                        (multiple-value-bind (xcc fcc)
                            (newpoint gamma_inner_contraction)
                          (if (< fcc (fk simplex n)) (accept xcc fcc)
                              (shrink))))

                    (shrink)))))
      
      (when verbose (format t "~S~%" simplex))
      simplex)))

;; The test returns true if the volume of the paralelepiped spanned by
;; the vertices of the simplex is smaller than that of an n-cube of
;; side cside.
(defun pp-volume-test (cside)
  #'(lambda (simplex)
      (let* ((mat (r-factor simplex)))

        (let ((det 1.0d0))
          (dotimes (i (dimension simplex))
            (setf det (* det (aref mat i i))))

          (< (abs det) (expt cside (dimension simplex)))))))

(defun nm-optimize (objective-function initial-guess &key
                    (max-function-calls 100000)
                    (convergence-p (burmen-et-al-convergence-test))
                    verbose)

  (let ((simplex (if (typep initial-guess 'nm-simplex)
                     initial-guess
                     (default-initial-simplex initial-guess)))
        (fvcount 0))
    (labels ((rigged-f (v)
               (incf fvcount)
               (funcall objective-function v))
             (converged-p (s)
               (or (funcall convergence-p s)
                   (> fvcount max-function-calls))))

      (when verbose
        (format t "Initial simplex: ~%~A~%---~%" simplex))

      (maybe-fill-simplex simplex #'rigged-f)

      (loop :until (converged-p simplex)
            :do
            (nm-iteration simplex #'rigged-f :verbose verbose))

      (values (xk simplex 0) (fk simplex 0) simplex fvcount))))

(defmethod restrict ((grid grid) point)
  (let ((new (copy-seq point))
        (n (length point))
        (delta (delta grid))
        (z (grid-z grid)))

    (dotimes (i n)
      (setf (aref new i)
            (+ (* (aref delta i)
                  (floor
                   (+ (/ (- (aref new i)
                            (aref z i))
                         (aref delta i))
                      0.5d0)))
               (aref z i))))

    new))


;; Some parameters. Look at Burmen et al for further details.
(defparameter *psi* 1.0d-6)
(defparameter *biglambda* (/ 0.5d0 double-float-epsilon))
(defparameter *tau-r* (* 2.0d0 double-float-epsilon))
(defparameter *tau-a* (expt least-positive-double-float (/ 1.0d0 3.0d0)))
(defparameter *smalllambda* 2)

(defvar *breakdown*)

(defmethod maybe-reshape ((s nm-simplex) (g grid) ff &key force)
  (let ((n (length (side-vectors s)))
        (biglambda *biglambda*)
        (smalllambda *smalllambda*)
        (reshaped-p nil))
    
    (labels ((degenerate-p ()
               (let* ((r (r-factor s))
                      (ax (loop for i from 0 below n
                                minimizing (abs (aref r i i)))))
                       
                 (< ax
                    (/ (* *psi*
                          (norm (delta g))
                          (sqrt (float n 1.0d0)))
                       2)))))

      (when (or force (degenerate-p))

        (setf reshaped-p t)
        
        (let ((r (r-factor s))
              (q (q-factor s))
              (|det| 1.0d0)
              (dv (make-array (+ n 1)))
              (dmin-norm nil)
              (dmin nil)
              (|Delta| (norm (delta g))))

          (setf (aref dv 0) (xk s 0))

          (dotimes (i n)
            (let* ((di (make-array n :element-type 'double-float))
                   (rii (aref r i i))
                   (sgn[rii] (if (>= rii 0) 1 -1))
                   (|rii| (abs rii))
                   (quot (* (sqrt (float n 0.0d0))
                            |Delta|
                            0.5d0))
                   (minc (min |rii|
                              (* biglambda quot)))
                   (maxc (max (* smalllambda quot) minc))
                   (dfkt (* sgn[rii] maxc)))

              (setf |det| (* |det| |rii|))

              (dotimes (j n)
                (setf (aref di j)
                      (* dfkt (aref q j i))))
              
              (when (or (not dmin-norm) (< (abs dfkt) dmin-norm))
                (setf dmin-norm (abs dfkt)
                      dmin di))

              (setf (aref dv (+ i 1)) di)

              (let* ((nxi (restrict g (v+w*c (xk s 0) di 1.0d0)))
                     (fxi (funcall ff nxi)))

                (setf (xk s (+ i 1)) nxi
                      (fk s (+ i 1)) fxi))))

          ;; Looks like a very extreme situation, but can actually
          ;; happen.
          (when (= |det| 0.0d0)
            (setf *breakdown* t))
          
          ;; Most cached data is invalid, but better keep the reshape
          ;; data, which might be needed for shrinking the simplex.
          (setf (data s)
                (make-instance 'cached-simplex-data
                               :dv dv
                               :dmin dmin
                               :best-before-reshape (fk s 0)))
          
          (sort-simplex s))))
    
    reshaped-p))

;;; Nelder Mead iteration variant from Burmen et al.  Does not shrink,
;;; "failing" instead. Acceptance criteria for the contraction points
;;; are stricter.
;;;
;;; Iterates are restricted to a grid.
(defun nm-iteration-burmen-et-al
    (simplex f grid &key
     verbose
     (gamma_reflect 1.0d0)
     (gamma_expand 2.0d0)
     (gamma_outer_contraction 0.5d0)
     (gamma_inner_contraction -0.5d0))
     

  (let ((n (- (length (x simplex)) 1))
        (failure t))

    (let ((x_cb (make-array n
                            :element-type 'double-float
                            :initial-element 0.0d0))
          (x_cb-x_n (make-array n
                                :element-type 'double-float
                                :initial-element 0.0d0)))

      (labels ((newpoint (gamma)
                 (let ((np (restrict grid
                                     (v+w*c x_cb x_cb-x_n gamma))))
                   (values np (funcall f np))))
               
               (accept (xx ff)
                 (improve simplex xx ff)
                 (setf failure nil)))

        ;; compute centroid
        (dotimes (i n)
          (setf x_cb (v+w*c x_cb (xk simplex i) (/ 1.0d0 n))))

        (setf x_cb-x_n (v+w*c x_cb (xk simplex n) -1.0d0))

        ;; 2. Reflect
        (multiple-value-bind (xr fr)
            (newpoint gamma_reflect)
          (if (and (<= (fk simplex 0) fr) (< fr (fk simplex (- n 1))))
              (accept xr fr)
              ;; 3. expand
              (if (< fr (fk simplex 0))
                  (multiple-value-bind (xe fe)
                      (newpoint gamma_expand)
                    (if (< fe fr)
                        (accept xe fe)
                        (accept xr fr)))
                  ;; 4. contract or fail ABurmen & al have swapped
                  ;; inner and outers - maybe a typo. (?) No, just a
                  ;; different variant.
                  (if (<= (fk simplex (- n 1)) fr)
                      (if (< fr (fk simplex n))
                          ;;outer contraction
                          (multiple-value-bind (xc fc)
                              (newpoint gamma_outer_contraction)
                            (if (<= fc (fk simplex (- n 1)))
                                (accept xc fc)
                                ))
                          ;; inner contraction
                          (multiple-value-bind (xcc fcc)
                              (newpoint gamma_inner_contraction)
                            (if (< fcc (fk simplex (- n 1)))
                                (accept xcc fcc))))))))))

    (sort-simplex simplex)
    (when verbose (format t "~S~%" simplex))
    (values simplex failure)))

;; Convergence test used in the article
(defun burmen-et-al-convergence-test (&key
                                      (tol-x 1.0d-8)
                                      (tol-f 1.0d-15)
                                      (rel 1.0d-15))

  #'(lambda (s)
      (let* ((sv (side-vectors s))
             (fdiff (abs (- (fk s 0) (fk s (dimension s)))))
             (vijmax (loop for v across sv maximizing
                           (loop for x across v maximizing
                                 (abs x))))
             (xbest (xk s 0))
             (|xi|max (loop for z across xbest maximizing (abs z))))

        (and (< fdiff (max tol-f (* rel (fk s 0))))
             (< vijmax (max tol-x (* rel |xi|max)))))))
      
      

(defun regrid (g x1 dmin fkt)
  (let* ((dmin (v*c dmin fkt))
         (n (length dmin))
         (|dmin| (norm dmin))
         (delta (delta g))
         (newd (copy-seq delta)))
    
    (setf (grid-z g) x1)
    
    (dotimes (i (length dmin))
      (setf (aref newd i)
            
            (max (min (max (/ (abs (aref dmin i))
                              (* *smalllambda* 250 n))

                           (/ |dmin|
                              (* *smalllambda* 250 (expt n (/ 3.0d0 2.0d0)))))
                      (aref delta i))
                 (* *tau-r* (aref x1 i))

                 *tau-a*)))

    (setf (delta g) newd)))

(defun deep-shrink (f s g gamma_s convergence-p verbose)
  (let ((bbr (fk s 0)))
  (unless (dv (data s))
    (maybe-reshape s g f :force t))

  (let* ((dv (dv (data s)))
         (n (dimension s))
         (dmin (dmin (data s)))
         (|dmin| (norm dmin))
         (converged-p nil)
         (l 1)) ;; This does the same as in the article

    (loop :until (or (< (fk s 0) bbr)
                     (setf converged-p (funcall convergence-p s)))
          :do (let ((fkt (* (expt gamma_s (floor l 2))
                            (expt -1 l))))

                (when verbose
                  (format t "Shrinking by factor: ~A~%" fkt))

                (when (= 0.0d0 fkt)
                  ;; You've got no simplex anymore
                  (setf *breakdown* t))

                (when (< (* (abs fkt) |dmin|)
                         (* (/ *smalllambda* 2)
                            (sqrt n)
                            (norm (delta g))))

                  (regrid g (xk s 0) dmin fkt))

                (setf (xk s 0) (aref dv 0)
                      (fk s 0) bbr)

                (loop for k from 1 below (length (x s)) do
                      (setf (xk s k) (restrict g
                                               (v+w*c (aref dv 0)
                                                             (aref dv k) fkt))
                            (fk s k) (funcall f (xk s k))))

                (sort-simplex s)
                (incf l)))

    (setf (data s) nil)

    converged-p)))
      
(defun grnm-optimize (objective-function initial-guess &key
                      max-function-calls
                      (convergence-p (burmen-et-al-convergence-test))
                      verbose)

  (let ((simplex (if (typep initial-guess 'nm-simplex)
                     initial-guess
                     (default-initial-simplex initial-guess)))
        (fvcount 0)
        (gamma_s 0.5d0)
        (*breakdown* nil))
    
    (labels ((rigged-f (v)
               (incf fvcount)
               (funcall objective-function v))
             (converged-p (s)
               (or (funcall convergence-p s)
                   *breakdown*
                   (and max-function-calls (> fvcount max-function-calls)))))
      (when verbose
        (format t "Initial simplex: ~%~A~%---~%" simplex))
      
      (maybe-fill-simplex simplex #'rigged-f)
      
  (prog (failure xbest fbest pbest reshaped-p
 
         (grid (make-instance 'grid
                              :z (xk simplex 0)
                              :delta
                              (make-array (dimension simplex)
                                          :initial-element
                                          (/ (loop :for i :from 1 :to
                                                   (dimension simplex)
                                                   :minimizing
                                                   (norm
                                                    (v+w*c
                                                     (xk simplex 0)
                                                     (xk simplex i) -1.0d0)))
                                             10.0d0)))))
     

   iterate ;; 1.
   (setf (values simplex failure)
         (nm-iteration-burmen-et-al simplex #'rigged-f grid
                                    :verbose verbose))

   ;; Small variation. We test for convergence here too. Depending on
   ;; the convergence criterion, this might be spurious, so we have to
   ;; reshape, etc.
   (unless (or failure (converged-p simplex))
     (go iterate))

   ;; 2.
   (setf xbest (xk simplex 0)
         fbest (fk simplex 0)
         pbest (aref (pmap simplex) 0)
   
         reshaped-p (maybe-reshape simplex grid #'rigged-f))
        

   ;; 4. Here we look at the pseudo expand point
   (let* ((pep (pseudopivot simplex))
          (xx (v+w*c pep
                     (v+w*c pep (xk simplex 0) -1.0d0)
                     1.0d0)) ;; To do: clear this up
          (fpep (rigged-f xx)))

     (if (<= fbest (min (fk simplex 0) fpep))
         (go deep-shrink)
         (when (< fpep fbest)
           ;; There is something subtle here. The pseudo-expand point
           ;; is supposed to substitute the old (xk 0), which might
           ;; have changed *position* during reshape, because of the
           ;; simplex being sorted.
           ;;
           ;; So it is not
           ;;(improve simplex xx fpep)
           ;; here, but instead

           ;; Step 5.
           (setf (aref (x simplex) pbest) xx
                 (aref (fx simplex) pbest) fpep
                 (data simplex) nil)

           (sort-simplex simplex)

           ;; Step 6.
           (go iterate))))
   
   deep-shrink

   (unless
       (deep-shrink #'rigged-f simplex grid

                    ;;(+ 0.1d0 (random 0.8d0))
                    gamma_s

                    #'converged-p verbose)
     (go iterate))

   end)
  (values (xk simplex 0) (fk simplex 0) simplex fvcount))))


;; Some sample functions to play around.

(defun standard-quadratic (v)
  (loop for i from 0 below (length v)
        summing (expt (aref v i) 2)))

(defun rosenbrock (v)
  (let ((x (aref v 0))
        (y (aref v 1)))

    (+ (expt (- 1 x) 2)
       (* 100 (expt (- y (expt x 2)) 2)))))