vol.lisp

   1 (defpackage :vol
   2   (:use :cl :sb-alien :sb-c-call)
   3   (:export
   4    #:fftshift3
   5    #:ft3
   6    #:ift3
   7    #:read-pgm
   8    #:write-pgm
   9    #:histogram
  10    #:get-linear-array
  11    #:read-stack
  12    #:square
  13    #:linear-regression
  14    #:clamp
  15    #:interp1
  16    #:interpolate2
  17
  18    #:+forward+
  19    #:+backward+
  20    #:+estimate+
  21
  22    #:do-rectangle
  23    #:do-box
  24    #:with-slice))
  25
  26 ;; for i in `cat vol.lisp|grep defconst|cut -d " " -f 2`;do echo \#:$i ;done
  27
  28 (in-package :vol)
  29
  30 #+nil (declaim (optimize (speed 3) (debug 1) (safety 1)))
  31 (declaim (optimize (speed 2) (debug 3) (safety 3)))
  32
  33 (load-shared-object "/usr/lib/libfftw3.so.3")
  34
  35 (define-alien-routine ("fftw_plan_dft_3d" plan-dft-3d)
  36     (* int)
  37   (n0 int)
  38   (n1 int)
  39   (n2 int)
  40   (in (* double-float))
  41   (out (* double-float))
  42   (sign int)
  43   (flags unsigned-int))
  44
  45 ;; C-standard "row-major" order, so that the last dimension has the
  46 ;; fastest-varying index in the array.
  47
  48
  49
  50 (defmacro do-rectangle ((j i ymin ymax xmin xmax) &body body)
  51   "Loop through 2d points in ymin .. ymax-1. e.g. call with 0 y 0 x to
  52 visit every point."
  53   `(loop for ,j from ,ymin below ,ymax do
  54         (loop for ,i from ,xmin below ,xmax do
  55              (progn ,@body))))
  56
  57 (defmacro do-box ((k j i zmin zmax ymin ymax xmin xmax) &body body)
  58   "Loop through 3d points e.g. call with 0 z 0 y 0 x to visit every
  59 point."
  60   `(loop for ,k from ,zmin below ,zmax do
  61         (loop for ,j from ,ymin below ,ymax do
  62              (loop for ,i from ,xmin below ,xmax do
  63                   (progn ,@body)))))
  64
  65
  66 ;; fftshift3 /home/martin/usb/y2009/1123/1.lisp
  67 (declaim (ftype (function ((simple-array (complex double-float) (* * *)))
  68                           (values (simple-array (complex double-float) (* * *))
  69                                   &optional))
  70                 fftshift3))
  71 (defun fftshift3 (in)
  72   (let ((out (make-array (array-dimensions in)
  73                          :element-type '(complex double-float))))
  74    (destructuring-bind (w2 w1 w0)
  75        (array-dimensions in)
  76      (dotimes (k w2)
  77        (dotimes (j w1)
  78          (dotimes (i w0)
  79            (let* ((ii (if (> i (/ w0 2))
  80                           (+ w0 (/ w0 2) (- i))
  81                           (- (/ w0 2) i)))
  82                   (jj (if (> j (/ w1 2))
  83                           (+ w1 (/ w1 2) (- j))
  84                           (- (/ w1 2) j)))
  85                   (kk (if (> k (/ w2 2))
  86                           (+ w2 (/ w2 2) (- k))
  87                           (- (/ w2 2) k))))
  88              (setf (aref out k j i)
  89                    (aref in kk jj ii))
  90              nil)))))
  91    out))
  92
  93 (define-alien-routine ("fftw_execute" execute)
  94     void
  95   (plan (* int)))
  96
  97 (defconstant +forward+ 1)
  98 (defconstant +backward+ -1)
  99 (defconstant +estimate+ (ash 1 6))
 100
 101 (declaim (ftype (function ((simple-array (complex double-float) (* * *))
 102                            &key (:forward boolean))
 103                           (values (simple-array (complex double-float) (* * *))
 104                                   &optional))
 105                 ft3))
 106 (defun ft3 (in &key (forward t))
 107   (let ((dims (array-dimensions in)))
 108    (destructuring-bind (z y x)
 109        dims
 110      (let* ((out (make-array dims :element-type '(complex double-float))))
 111        (sb-sys:with-pinned-objects (in out)
 112          (let ((p (plan-dft-3d z y x
 113                                (sb-sys:vector-sap
 114                                 (sb-ext:array-storage-vector in))
 115                                (sb-sys:vector-sap
 116                                 (sb-ext:array-storage-vector out))
 117                                (if forward
 118                                    +forward+
 119                                    +backward+)
 120                                +estimate+)))
 121            (execute p)))
 122        (when forward ;; normalize if forward
 123          (let ((1/n (/ 1d0 (* x y z))))
 124           (do-box (k j i 0 z 0 y 0 x)
 125             (setf (aref out k j i) (* 1/n (aref out k j i))))))
 126        out))))
 127
 128 (defmacro ift3 (in)
 129   `(ft3 ,in :forward nil))
 130
 131 (declaim
 132  (type (function
 133         (string)
 134         (values (simple-array (unsigned-byte 8) (* *))
 135                 &optional))
 136        read-pgm))
 137 (defun read-pgm (filename)
 138   (with-open-file (s filename)
 139     (unless (equal (symbol-name (read s)) "P5")
 140       (error "no PGM file"))
 141     (let* ((w (read s))
 142            (h (read s))
 143            (grays (read s))
 144            (pos (file-position s))
 145            (data (make-array
 146                   (list h w)
 147                   :element-type '(unsigned-byte 8)))
 148            (data-1d (make-array
 149                      (* h w)
 150                      :element-type '(unsigned-byte 8)
 151                      :displaced-to data)))
 152       (declare ((simple-array (unsigned-byte 8) (* *)) data)
 153                ((array (unsigned-byte 8) (*)) data-1d)
 154                ((integer 0 65535) grays w h))
 155       (unless (= grays 255)
 156         (error "image has wrong bitdepth"))
 157       (with-open-file (s filename
 158                          :element-type '(unsigned-byte 8))
 159         (file-position s pos)
 160         (read-sequence data-1d s))
 161       data)))
 162
 163 (declaim
 164  (ftype (function
 165          ((array (unsigned-byte 8) 2)
 166           string)
 167          (values null &optional))
 168         write-pgm))
 169 (defun write-pgm (img filename)
 170   (destructuring-bind (h w)
 171       (array-dimensions img)
 172     (declare ((integer 0 65535) w h))
 173     (with-open-file (s filename
 174                        :direction :output
 175                        :if-exists :supersede
 176                        :if-does-not-exist :create)
 177       (declare (stream s))
 178       (format s "P5~%~D ~D~%255~%" w h))
 179     (with-open-file (s filename
 180                        :element-type '(unsigned-byte 8)
 181                        :direction :output
 182                        :if-exists :append)
 183       (let ((data-1d (make-array
 184                       (* h w)
 185                       :element-type '(unsigned-byte 8)
 186                       :displaced-to img)))
 187         (write-sequence data-1d s)))
 188     nil))
 189
 190 (declaim
 191  (ftype (function
 192          ((array (unsigned-byte 8) (*)) &optional fixnum)
 193          (values (simple-array fixnum (*)) (unsigned-byte 8) (unsigned-byte 8) (unsigned-byte 8)
 194                  &optional))
 195         histogram))
 196 (defun histogram (img &optional (bins 30))
 197   (let* ((maxval (aref img 0))
 198          (minval maxval)
 199          (w (length img)))
 200     (dotimes (i w)
 201       (let ((v (aref img i)))
 202         (when (< v minval)
 203           (setf minval v))
 204         (when (< maxval v)
 205           (setf maxval v))))
 206     (let* ((len (1+ (- maxval minval)))
 207            (result (make-array len :element-type 'fixnum))
 208            (binsm (min (- maxval minval) bins)))
 209       (when (eq maxval minval)
 210         #+nil (error "data is too boring.")
 211         (return-from histogram (values result minval maxval binsm)))
 212       (dotimes (i w)
 213         (incf (aref result (floor (* binsm
 214                                      (- (aref img i)
 215                                         minval))
 216                                   (- maxval minval)))))
 217       (values result minval maxval binsm))))
 218
 219 (declaim (ftype (function ((array * (* *)))
 220                           (values (simple-array * (*)) &optional))
 221                 get-linear-array))
 222 (defun get-linear-array (img)
 223   (sb-ext:array-storage-vector img)
 224   #+nil (make-array (* (array-dimension img 0)
 225                  (array-dimension img 1))
 226               :element-type (array-element-type img)
 227               :displaced-to img))
 228
 229
 230 (declaim (ftype (function (string)
 231                           (values (simple-array (unsigned-byte 8) (* * *))
 232                                   &optional))
 233                 read-stack))
 234 (defun read-stack (fn)
 235   (let* ((files (directory fn))
 236          (slices (length files))
 237          (a (read-pgm (first files))))
 238     (destructuring-bind (h w)
 239         (array-dimensions a)
 240       (let* ((result (make-array (list slices h w)
 241                                  :element-type '(unsigned-byte 8))))
 242         (dotimes (k slices)
 243           (let* ((a (read-pgm (elt files k))))
 244             (dotimes (j h)
 245               (dotimes (i w)
 246                 (setf (aref result k j i)
 247                       (aref a j i))))))
 248         result))))
 249
 250 (defmacro with-slice ((slice-array array slice-nr) &body body)
 251   "Returns SLICE-NRth slice of ARRAY as the 2D SLICE-ARRAY."
 252   (let ((x (gensym))
 253         (y (gensym))
 254         (z (gensym)))
 255     `(destructuring-bind (,z ,y ,x)
 256         (array-dimensions ,array)
 257       (when (or (< ,slice-nr 0) (<= ,z ,slice-nr))
 258         (error "slice-nr=~d out of range [0,~d]" ,slice-nr (1- ,z)))
 259       (let* ((,slice-array (make-array (list ,y ,x)
 260                                        :element-type '(unsigned-byte 8)
 261                                        :displaced-to ,array
 262                                        :displaced-index-offset (* ,slice-nr ,x ,y))))
 263         ,@body))))
 264
 265 (declaim (ftype (function (double-float) (values double-float &optional))
 266                 square))
 267 (defun square (x)
 268   (* x x))
 269
 270 (declaim (ftype (function ((array double-float *)
 271                            &optional
 272                            (array double-float *))
 273                           (values double-float double-float
 274                                   double-float double-float &optional))
 275                 linear-regression))
 276 ;; chernov/book.pdf p. 20
 277 (defun linear-regression (y &optional
 278                           (x (let* ((n (length y)))
 279                                (make-array
 280                                 n
 281                                 :element-type 'double-float
 282                                 :initial-contents
 283                                 (loop for i below n collect (* 1d0 i))))))
 284   "Linear regression of the values in Y with the function y=a*x+b. If
 285 X isn't supplied its assumed to be 0,1,2, ... . Returned are the
 286 fitting parameters A and B and their errors DELTA_A and DELTA_B."
 287   (let* ((n (length y))
 288          (xmean (/ (loop for xi across x sum xi) n))
 289          (ymean (/ (loop for xi across y sum xi) n))
 290          (sxx (loop for xi across x sum (square (- xi xmean))))
 291         #+nil (syy (loop for xi across y sum (square (- xi ymean))))
 292          (sxy (loop for i below n sum (* (- (aref x i) xmean)
 293                                          (- (aref y i) ymean))))
 294          (bhat (/ sxy sxx))
 295          (ahat (- ymean (* bhat xmean)))
 296          (var (/ (loop for i below n sum (square (- (aref y i) ahat
 297                                                        (* bhat (aref x i)))))
 298                     (- n 2)))
 299          (vara (* var (+ (/ (square xmean)
 300                             sxx)
 301                          (/ n))))
 302          (varb (/ var sxx)))
 303     (values ahat bhat (sqrt vara) (sqrt varb))))
 304
 305 #+nil
 306 (linear-regression (let* ((ll (list 1d0 2.01d0 3d0 4d0)))
 307                      (make-array (length ll)
 308                       :element-type 'double-float
 309                       :initial-contents ll)))
 310
 311 (declaim (ftype (function (integer)
 312                           (values (unsigned-byte 8) &optional))
 313                 clamp))
 314 (defun clamp (a)
 315   (when (< a 0)
 316     (return-from clamp 0))
 317   (when (< 255 a)
 318     (return-from clamp 255))
 319   a)
 320
 321
 322 (declaim (ftype (function ((simple-array (unsigned-byte 8) 2)
 323                            fixnum fixnum)
 324                           (values (unsigned-byte 8) &optional))
 325                 aref2-zero-ub8))
 326 (defun aref2-zero-ub8 (a j i)
 327   "Like AREF but return zero if subscripts J and I point outside of
 328 the array bounds."
 329   (if (array-in-bounds-p a j i)
 330       (aref a j i)
 331       0))
 332
 333 (declaim (ftype (function ((simple-array double-float 2)
 334                            fixnum fixnum)
 335                           (values double-float &optional))
 336                 aref2-zero-df))
 337 (defun aref2-zero-df (a j i)
 338   "Like AREF but return zero if subscripts J and I point outside of
 339 the array bounds."
 340   (if (array-in-bounds-p a j i)
 341       (aref a j i)
 342       0d0))
 343
 344 (declaim (ftype (function ((simple-array (complex double-float) 2)
 345                            fixnum fixnum)
 346                           (values (complex double-float) &optional))
 347                 aref2-zero-cdf))
 348 (defun aref2-zero-cdf (a j i)
 349   "Like AREF but return zero if subscripts J and I point outside of
 350 the array bounds."
 351   (if (array-in-bounds-p a j i)
 352       (aref a j i)
 353       (complex 0d0)))
 354
 355
 356 (declaim (ftype (function ((unsigned-byte 8)
 357                            (unsigned-byte 8)
 358                            double-float)
 359                           (values double-float &optional))
 360                 interp1-ub8)
 361          (inline interp1-ub8))
 362 (defun interp1-ub8 (a b xi)
 363     "Interpolate between values A and B. Returns A for xi=0 and B for
 364   xi=1."
 365   (+ (* (- 1d0 xi) a) (* xi b)))
 366
 367 (declaim (ftype (function (double-float double-float double-float)
 368                           (values double-float &optional))
 369                 interp1-df)
 370          (inline interp1-df))
 371 (defun interp1-df (a b xi)
 372     "Interpolate between values A and B. Returns A for xi=0 and B for
 373   xi=1."
 374   (+ (* (- 1d0 xi) a) (* xi b)))
 375
 376 (declaim (ftype (function ((complex double-float)
 377                            (complex double-float) double-float)
 378                           (values (complex double-float) &optional))
 379                 interp1-cdf)
 380          (inline interp1-cdf))
 381 (defun interp1-cdf (a b xi)
 382     "Interpolate between values A and B. Returns A for xi=0 and B for
 383   xi=1."
 384   (+ (* (- 1d0 xi) a) (* xi b)))
 385
 386 (declaim (ftype (function ((or (unsigned-byte 8)
 387                                double-float
 388                                (complex double-float))
 389                            (or (unsigned-byte 8)
 390                                double-float
 391                                (complex double-float))
 392                            double-float)
 393                           (values (or double-float
 394                                       (complex double-float)) &optional))
 395                 interp1)
 396          (inline interp1))
 397 (defun interp1 (a b xi)
 398   (etypecase a
 399     ((unsigned-byte 8) (interp1-ub8 a b xi))
 400     (double-float (interp1-df a b xi))
 401     ((complex double-float) (interp1-cdf a b xi))))
 402
 403 (declaim (ftype (function ((simple-array (unsigned-byte 8) 2)
 404                            double-float double-float)
 405                           (values double-float &optional))
 406                 interpolate2-ub8))
 407 (defun interpolate2-ub8 (img x y)
 408   "Bilinear interpolation on an image IMG. The coordinates X and Y can
 409 be floating point values. If they point outside of IMG 0 is returned."
 410   (multiple-value-bind (i ix)
 411       (floor x)
 412     (multiple-value-bind (j jx)
 413         (floor y)
 414       ;; values on grid points, top left is i,j and stored in a
 415       ;; |
 416       ;;-a-b
 417       ;; |
 418       ;; c d
 419       (let ((a (aref img i j))
 420             (b (aref2-zero-ub8 img (1+ i) j))
 421             (c (aref2-zero-ub8 img i (1+ j)))
 422             (d (aref2-zero-ub8 img (1+ i) (1+ j))))
 423         ;; now interpolate verticals
 424         ;; a  b
 425         ;; |  |
 426         ;; e  f
 427         ;; |  |
 428         ;; c  d
 429         (let ((e (interp1-ub8 a c jx))
 430               (f (interp1-ub8 b d jx)))
 431           ;; and now horizontal
 432           ;; e - g - f
 433           (let ((g (interp1-df e f ix)))
 434             g))))))
 435
 436 (declaim (ftype (function ((simple-array double-float 2)
 437                            double-float double-float)
 438                           (values double-float &optional))
 439                 interpolate2-df))
 440 (defun interpolate2-df (img x y)
 441   "Bilinear interpolation on an image IMG. The coordinates X and Y can
 442 be floating point values. If they point outside of IMG 0 is returned."
 443   (multiple-value-bind (i ix)
 444       (floor x)
 445     (multiple-value-bind (j jx)
 446         (floor y)
 447       (let ((a (aref img i j))
 448             (b (aref2-zero-df img (1+ i) j))
 449             (c (aref2-zero-df img i (1+ j)))
 450             (d (aref2-zero-df img (1+ i) (1+ j))))
 451         (let ((e (interp1-df a c jx))
 452               (f (interp1-df b d jx)))
 453           (let ((g (interp1-df e f ix)))
 454             g))))))
 455
 456 (declaim (ftype (function ((simple-array (complex double-float) 2)
 457                            double-float double-float)
 458                           (values (complex double-float) &optional))
 459                 interpolate2-cdf))
 460 (defun interpolate2-cdf (img x y)
 461   "Bilinear interpolation on an image IMG. The coordinates X and Y can
 462 be floating point values. If they point outside of IMG 0 is returned."
 463   (multiple-value-bind (i ix)
 464       (floor x)
 465     (multiple-value-bind (j jx)
 466         (floor y)
 467       (let ((a (aref img i j))
 468             (b (aref2-zero-cdf img (1+ i) j))
 469             (c (aref2-zero-cdf img i (1+ j)))
 470             (d (aref2-zero-cdf img (1+ i) (1+ j))))
 471         (let ((e (interp1-cdf a c jx))
 472               (f (interp1-cdf b d jx)))
 473           (let ((g (interp1-cdf e f ix)))
 474             g))))))
 475
 476
 477 (declaim (ftype (function ((simple-array * 2) double-float double-float)
 478                           (values (or double-float
 479                                       (complex double-float)) &optional))
 480                 interpolate2))
 481 (defun interpolate2 (img x y)
 482   "Bilinear interpolation on an image IMG. The coordinates X and Y can
 483 be floating point values. If they point outside of IMG 0 is returned."
 484   (etypecase img
 485     ((simple-array (unsigned-byte 8) 2) (interpolate2-ub8 img x y))
 486     ((simple-array double-float 2) (interpolate2-df img x y))
 487     ((simple-array (complex double-float) 2) (interpolate2-cdf img x y))))
 488
 489 #+nil
 490 (let ((a (make-array (list 2 2) :element-type '(unsigned-byte 8)
 491                      :initial-contents '((1 2) (2 3)))))
 492   (interpolate2 a .5d0 .2d0))