svd.lisp

   1 ;;; -*- mode: lisp; coding: utf-8 -*-
   2 ;;;
   3 ;;; Copyright (c) 2008 NANRI <southly@gmail.com>
   4 ;;;
   5 ;;; Permission is hereby granted, free of charge, to any person obtaining a copy
   6 ;;; of this software and associated documentation files (the "Software"), to deal
   7 ;;; in the Software without restriction, including without limitation the rights
   8 ;;; to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   9 ;;; copies of the Software, and to permit persons to whom the Software is
  10 ;;; furnished to do so, subject to the following conditions:
  11 ;;;
  12 ;;; The above copyright notice and this permission notice shall be included in
  13 ;;; all copies or substantial portions of the Software.
  14 ;;;
  15 ;;; THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16 ;;; IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17 ;;; FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18 ;;; AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19 ;;; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20 ;;; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  21 ;;; THE SOFTWARE.
  22
  23 (in-package :cl-jama)
  24
  25 (defclass SVD ()
  26   ((U)
  27    (V)
  28    (s)
  29    (m)
  30    (n)))
  31
  32 (defmethod initialize-instance :after ((obj SVD) &key matrix)
  33   (with-slots (U V s m n) obj
  34     (let (nu e work A (wantu t) (wantv t) nct nrt p pp eps iter)
  35       (setf m (array-dimension matrix 0))
  36       (setf n (array-dimension matrix 1))
  37       (setf nu (min m n))
  38       (setf s (make-array (list (min (1+ m) n))))
  39       (setf U (make-array (list m nu) :initial-element 0.0))
  40       (setf V (make-array (list n n)))
  41       (setf e (make-array (list n)))
  42       (setf work (make-array (list m)))
  43       (setf A (copy-array matrix))
  44       ;;
  45       (setf nct (min (1- m) n))
  46       (setf nrt (max 0 (min (- n 2) m)))
  47       (loop :for k :from 0 :below (max nct nrt)
  48             :do (when (< k nct)
  49                   ;;
  50                   (setf (aref s k) 0)
  51                   (loop :for i :from k :below m
  52                         :do (setf (aref s k) (hypot (aref s k) (aref A i k))))
  53                   (unless (zerop (aref s k))
  54                     (when (minusp (aref A k k))
  55                       (setf (aref s k) (- (aref s k))))
  56                     (loop :for i :from k :below m
  57                           :do (setf (aref A i k) (/ (aref A i k) (aref s k))))
  58                     (incf (aref A k k) 1.0))
  59                   (setf (aref s k) (- (aref s k))))
  60                 (loop :for j :from (1+ k) :below n
  61                       :do (when (and (< k nct) (not (zerop (aref s k))))
  62                             ;;
  63                             (let ((rt 0.0))
  64                               (loop :for i :from k :below m
  65                                     :do (incf rt (* (aref A i k) (aref A i j))))
  66                               (setf rt (/ (- rt) (aref A k k)))
  67                               (loop :for i :from k :below m
  68                                     :do (incf (aref A i j) (* rt (aref A i k))))))
  69                           (setf (aref e j) (aref A k j)))
  70                 (when (and wantu (< k nct))
  71                   (loop :for i :from k :below m
  72                         :do (setf (aref U i k) (aref A i k))))
  73                 (when (< k nrt)
  74                   ;;
  75                   (setf (aref e k) 0)
  76                   (loop :for i :from (1+ k) :below n
  77                         :do (setf (aref e k) (hypot (aref e k) (aref e i))))
  78                   (unless (zerop (aref e k))
  79                     (when (minusp (aref e (1+ k)))
  80                       (setf (aref e k) (- (aref e k))))
  81                     (loop :for i :from (1+ k) :below n
  82                           :do (setf (aref e i) (/ (aref e i) (aref e k))))
  83                     (incf (aref e (1+ k)) 1.0))
  84                   (setf (aref e k) (- (aref e k)))
  85                   (when (and (< (1+ k) m) (not (zerop (aref e k))))
  86                     ;;
  87                     (loop :for i :from (1+ k) :below m
  88                           :do (setf (aref work i) 0.0))
  89                     (loop :for j :from (1+ k) :below n
  90                           :do (loop :for i :from (1+ k) :below m
  91                                     :do (incf (aref work i) (* (aref e j) (aref A i j)))))
  92                     (loop :for j :from (1+ k) :below n
  93                           :do (let ((rt (/ (- (aref e j)) (aref e (1+ k)))))
  94                                 (loop :for i :from (1+ k) :below m
  95                                       :do (incf (aref A i j) (* rt (aref work i)))))))
  96                   (when wantv
  97                     ;;
  98                     (loop :for i :from (1+ k) :below n
  99                           :do (setf (aref V i k) (aref e i))))))
 100       ;;
 101       (setf p (min n (1+ m)))
 102       (when (< nct n)
 103         (setf (aref s nct) (aref A nct nct)))
 104       (when (< m p)
 105         (setf (aref s (1- p)) 0.0))
 106       (when (< (1+ nrt) p)
 107         (setf (aref e nrt) (aref A nrt (1- p))))
 108       (setf (aref e (1- p)) 0.0)
 109       ;;
 110       (when wantu
 111         (loop :for j :from nct :below nu
 112               :do (loop :for i :from 0 :below m
 113                         :do (setf (aref U i j) 0.0))
 114                   (setf (aref U j j) 1.0))
 115         (loop :for k :from (1- nct) :downto 0
 116               :do (cond ((not (zerop (aref s k)))
 117                          (loop :for j :from (1+ k) :below nu
 118                                :do (let ((rt 0.0))
 119                                      (loop :for i :from k :below m
 120                                            :do (incf rt (* (aref U i k) (aref U i j))))
 121                                      (setf rt (/ (- rt) (aref U k k)))
 122                                      (loop :for i :from k :below m
 123                                            :do (incf (aref U i j) (* rt (aref U i k))))))
 124                          (loop :for i :from k :below m
 125                                :do (setf (aref U i k) (- (aref U i k))))
 126                          (setf (aref U k k) (+ 1.0 (aref U k k)))
 127                          (loop :for i :from 0 :below (1- k)
 128                                :do (setf (aref U i k) 0.0)))
 129                         (t
 130                          (loop :for i :from 0 :below m
 131                                :do (setf (aref U i k) 0.0))
 132                          (setf (aref U k k) 1.0)))))
 133       ;;
 134       (when wantv
 135         (loop :for k :from (1- n) :downto 0
 136               :do (when (and (< k nrt) (not (zerop (aref e k))))
 137                     (loop :for j :from (1+ k) :below nu
 138                           :do (let ((rt 0.0))
 139                                 (loop :for i :from (1+ k) :below n
 140                                       :do (incf rt (* (aref V i k) (aref V i j))))
 141                                 (setf rt (/ (- rt) (aref V (1+ k) k)))
 142                                 (loop :for i :from (1+ k) :below n
 143                                       :do (incf (aref V i j) (* rt (aref V i k)))))))
 144                   (loop :for i :from 0 :below n
 145                         :do (setf (aref V i k) 0.0))
 146                   (setf (aref V k k) 1.0)))
 147       ;;
 148       (setf pp (1- p))
 149       (setf iter 0)
 150       (setf eps (expt 2.0 -52.0))
 151       (loop :while (plusp p)
 152             :do (let ((k 0) (kase 0))
 153                   ;;
 154                   (loop :for lk :from (- p 2) :downto -1
 155                         :when (= lk -1)
 156                         :return (setf k lk)
 157                         :end
 158                         :when (<= (abs (aref e lk)) (* eps (+ (abs (aref s lk)) (abs (aref s (1+ lk))))))
 159                         :return (setf (aref e lk) 0.0
 160                                       k lk)
 161                         :end
 162                         :finally (setf k lk)) ; XXX
 163                   (cond ((= k (- p 2))
 164                          (setf kase 4))
 165                         (t
 166                          (let ((ks 0))
 167                            (loop :for lks :from (1- p) :downto k
 168                                  :when (= lks k)
 169                                  :return (setf ks lks)
 170                                  :end
 171                                  :when (<= (abs (aref s lks)) (* eps (+ (if (/= lks p) (abs (aref e lks)) 0.0)
 172                                                                         (if (/= lks (1+ k)) (abs (aref e (1- lks))) 0.0))))
 173                                  :return (setf (aref s lks) 0.0
 174                                                ks lks)
 175                                  :end
 176                                  :finally (setf ks lks)) ; XXX
 177                            (cond ((= ks k)
 178                                   (setf kase 3))
 179                                  ((= ks (1- p))
 180                                   (setf kase 1))
 181                                  (t
 182                                   (setf kase 2)
 183                                   (setf k ks))))))
 184                   (incf k)
 185                   ;;
 186                   ;; switch
 187                   (case kase
 188                     (1 (let ((f (aref e (- p 2))))
 189                          (setf (aref e (- p 2)) 0.0)
 190                          (loop :for j :from (- p 2) :downto k
 191                                :for rt = (hypot (aref s j) f) :then (hypot (aref s j) f)
 192                                :for cs = (/ (aref s j) rt) :then (/ (aref s j) rt)
 193                                :for sn = (/ f rt) :then (/ f rt)
 194                                :do (setf (aref s j) rt)
 195                                    (unless (eql j k)
 196                                      (setf f (* (- sn) (aref e (1- j))))
 197                                      (setf (aref e (1- j)) (* cs (aref e (1- j)))))
 198                                    (when wantv
 199                                      (loop :for i :from 0 :below n
 200                                            :do (setf rt (+ (* cs (aref V i j)) (* sn (aref V i (1- p)))))
 201                                                (setf (aref V i (1- p)) (+ (* (- sn) (aref V i j)) (* cs (aref V i (1- p)))))
 202                                                (setf (aref V i j) rt))))))
 203                     (2 (let ((f (aref e (1- k))))
 204                          (setf (aref e (1- k)) 0.0)
 205                          (loop :for j :from k :below p
 206                                :for rt = (hypot (aref s j) f) :then (hypot (aref s j) f)
 207                                :for cs = (/ (aref s j) rt) :then (/ (aref s j) rt)
 208                                :for sn = (/ f rt) :then (/ f rt)
 209                                :do (setf (aref s j) rt)
 210                                    (setf f (* (- sn) (aref e j)))
 211                                    (setf (aref e j) (* cs (aref e j)))
 212                                    (when wantu
 213                                      (loop :for i :from 0 :below m
 214                                            :do (setf rt (+ (* cs (aref U i j)) (* sn (aref U i (1- k)))))
 215                                                (setf (aref U i (1- k)) (+ (* (- sn) (aref U i j)) (* cs (aref U i (1- k)))))
 216                                                (setf (aref U i j) rt))))))
 217                     (3 (let* ((scale (max (abs (aref s (- p 1))) (abs (aref s (- p 2))) (abs (aref e (- p 2)))
 218                                           (abs (aref s k)) (abs (aref e k))))
 219                               (sp (/ (aref s (1- p)) scale))
 220                               (spm1 (/ (aref s (- p 2)) scale))
 221                               (epm1 (/ (aref e (- p 2)) scale))
 222                               (sk (/ (aref s k) scale))
 223                               (ek (/ (aref e k) scale))
 224                               (b (/ (+ (* (+ spm1 sp) (- spm1 sp)) (* epm1 epm1)) 2.0))
 225                               (c (* sp epm1 sp epm1))
 226                               (shift 0.0)
 227                               f g)
 228                          (when (or (not (zerop b)) (not (zerop c)))
 229                            (setf shift (sqrt (+ (* b b) c)))
 230                            (when (minusp b)
 231                              (setf shift (- shift)))
 232                            (setf shift (/ c (+ b shift))))
 233                          (setf f (+ (* (+ sk sp) (- sk sp)) shift))
 234                          (setf g (* sk ek))
 235                          ;;
 236                          (loop :for j :from k :below (1- p)
 237                                :do (let* ((rt (hypot f g))
 238                                           (cs (/ f rt))
 239                                           (sn (/ g rt)))
 240                                      (when (/= j k)
 241                                        (setf (aref e (1- j)) rt))
 242                                      (setf f (+ (* cs (aref s j)) (* sn (aref e j)))
 243                                            (aref e j) (- (* cs (aref e j)) (* sn (aref s j)))
 244                                            g (* sn (aref s (1+ j)))
 245                                            (aref s (1+ j)) (* cs (aref s (1+ j))))
 246                                      (when wantv
 247                                        (loop :for i :from 0 :below n
 248                                              :do (setf rt (+ (* cs (aref V i j)) (* sn (aref V i (1+ j))))
 249                                                        (aref V i (1+ j)) (+ (* (- sn) (aref V i j)) (* cs (aref V i (1+ j))))
 250                                                        (aref V i j) rt)))
 251                                      (setf rt (hypot f g))
 252                                      (setf cs (/ f rt))
 253                                      (setf sn (/ g rt))
 254                                      (setf (aref s j) rt)
 255                                      (setf f (+ (* cs (aref e j)) (* sn (aref s (1+ j)))))
 256                                      (setf (aref s (1+ j)) (+ (* (- sn) (aref e j)) (* cs (aref s (1+ j)))))
 257                                      (setf g (* sn (aref e (1+ j))))
 258                                      (setf (aref e (1+ j)) (* cs (aref e (1+ j))))
 259                                      (when (and wantu (< j (1- m)))
 260                                        (loop :for i :from 0 :below m
 261                                              :do (setf rt (+ (* cs (aref U i j)) (* sn (aref U i (1+ j))))
 262                                                        (aref U i (1+ j)) (+ (* (- sn) (aref U i j)) (* cs (aref U i (1+ j))))
 263                                                        (aref U i j) rt)))))
 264                          (setf (aref e (- p 2)) f)
 265                          (setf iter (1+ iter))))
 266                     (4 (when (<= (aref s k) 0.0)
 267                          (setf (aref s k) (if (minusp (aref s k)) (- (aref s k)) 0.0))
 268                          (when wantv
 269                            (loop :for i :from 0 :to pp
 270                                  :do (setf (aref V i k) (- (aref V i k))))))
 271                        (loop :while (< k pp)
 272                              :thereis (>= (aref s k) (aref s (1+ k)))
 273                              :do (psetf (aref s k) (aref s (1+ k))
 274                                         (aref s (1+ k)) (aref s k))
 275                              :when (and wantv (< k (1- n)))
 276                              :do (loop :for i :from 0 :below n
 277                                        :do (psetf (aref V i (1+ k)) (aref V i k)
 278                                                   (aref V i k) (aref V i (1+ k))))
 279                              :end
 280                              :when (and wantu (< k (1- m)))
 281                              :do (loop :for i :from 0 :below m
 282                                        :do (psetf (aref U i (1+ k)) (aref U i k)
 283                                                   (aref U i k) (aref U i (1+ k))))
 284                              :do (incf k))
 285                        (setf iter 0)
 286                        (decf p))))))))
 287
 288 (defmethod get-U ((A SVD))
 289   (with-slots (U m n) A
 290     (let* ((minm (min (1+ m) n))
 291            (X (make-array (list m minm))))
 292       (loop :for i :from 0 :below m
 293             :do (loop :for j :from 0 :below minm
 294                       :do (setf (aref X i j) (aref U i j))))
 295       X)))
 296
 297 (defmethod get-V ((A SVD))
 298   (with-slots (V) A
 299     (copy-array V)))
 300
 301 (defmethod get-singular-values ((A SVD))
 302   (with-slots (s) A
 303     (copy-array s)))
 304
 305 (defmethod get-S ((A SVD))
 306   (with-slots (n s) A
 307     (let ((X (make-array (list n n))))
 308       (loop :for i :from 0 :below n
 309             :do (loop :for j :from 0 :below n
 310                       :do (setf (aref X i j) 0.0))
 311                 (setf (aref X i i) (aref s i)))
 312       X)))
 313 (defmethod norm2 ((A SVD))
 314   (with-slots (s) A
 315     (aref s 0)))
 316
 317 (defmethod norm2-of-condition-number ((A SVD))
 318   (with-slots (m n s) A
 319     (/ (aref s 0) (aref s (1- (min m n))))))
 320
 321 (defmethod rank ((A SVD))
 322   (with-slots (m n s) A
 323     (let* ((eps (expt 2.0 -52.0))
 324            (tol (* (max m n) (aref s 0) eps)))
 325       (loop :for i :from 0 :below (array-dimension s 0)
 326             :count (> (aref s 0) tol)))))