photogrammetry.lisp

   1 ;;; PHOROS -- Photogrammetric Road Survey
   2 ;;; Copyright (C) 2010, 2011 Bert Burgemeister
   3 ;;;
   4 ;;; This program is free software; you can redistribute it and/or modify
   5 ;;; it under the terms of the GNU General Public License as published by
   6 ;;; the Free Software Foundation; either version 2 of the License, or
   7 ;;; (at your option) any later version.
   8 ;;;
   9 ;;; This program is distributed in the hope that it will be useful,
  10 ;;; but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 ;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 ;;; GNU General Public License for more details.
  13 ;;;
  14 ;;; You should have received a copy of the GNU General Public License along
  15 ;;; with this program; if not, write to the Free Software Foundation, Inc.,
  16 ;;; 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  17
  18 (in-package :phoros-photogrammetry)
  19
  20 #-sbcl (defun nan-p (x)
  21           (declare (float x))
  22           (/= x x))
  23 #+sbcl (defun nan-p (x)
  24          (sb-ext:float-nan-p x))
  25
  26 (defgeneric photogrammetry (mode photo-1 &optional photo-2)
  27   (:documentation "Call to photogrammetry library.  Dispatch on mode."))
  28
  29 (defmethod photogrammetry :around (mode clicked-photo &optional other-photo)
  30   "Prepare and clean up a run of photogrammetry."
  31   (declare (ignore other-photo))
  32   (bt:with-lock-held (*photogrammetry-mutex*)
  33     (del-all)
  34     (unwind-protect
  35          (call-next-method)
  36       (del-all))))
  37
  38 (defmethod photogrammetry
  39     ((mode (eql :epipolar-line)) clicked-photo &optional other-photo)
  40   "Return in an alist an epipolar line in coordinates of other-photo
  41 from m and n in clicked-photo."
  42   (add-cam* clicked-photo)
  43   (add-bpoint* clicked-photo)
  44   (add-global-car-reference-point* clicked-photo t)
  45   (add-cam* other-photo)
  46   (add-global-car-reference-point* other-photo t)
  47   (loop
  48      for i = 2d0 then (* i 1.4) until (> i 50)
  49      do
  50        (set-distance-for-epipolar-line i)
  51      when (ignore-errors (calculate))
  52      collect (pairlis '(:m :n) (list (flip-m-maybe (get-m) other-photo)
  53                                      (flip-n-maybe (get-n) other-photo)))))
  54
  55 (defmethod photogrammetry
  56     ((mode (eql :reprojection)) photo &optional global-point)
  57   "Calculate reprojection from photo."
  58   (add-cam* photo)
  59   (add-global-measurement-point* global-point)
  60   (add-global-car-reference-point* photo)
  61   (set-global-reference-frame)
  62   (calculate)
  63   (let ((m (get-m))
  64         (n (get-n)))
  65     (assert (not (nan-p m)))            ;On some systems, PhoML gives us
  66     (assert (not (nan-p n)))            ; quiet NaN instead of erring.
  67     (pairlis '(:m :n)
  68              (list (flip-m-maybe m photo) (flip-n-maybe n photo)))))
  69
  70 (defmethod photogrammetry
  71     ((mode (eql :multi-position-intersection)) photos &optional other-photo)
  72   "Calculate intersection from photos."
  73   (declare (ignore other-photo))
  74   (set-global-reference-frame)
  75   (loop
  76      for photo in photos
  77      do
  78        (add-cam* photo)
  79        (add-bpoint* photo)
  80        (add-global-car-reference-point* photo t))
  81   (calculate)
  82   (pairlis '(:x-global :y-global :z-global
  83              :stdx-global :stdy-global :stdz-global)
  84            (list
  85             (get-x-global) (get-y-global) (get-z-global)
  86             (get-stdx-global) (get-stdy-global) (get-stdz-global))))
  87
  88 (defmethod photogrammetry
  89     ((mode (eql :intersection)) photo &optional other-photo)
  90   "Calculate intersection from two photos that are taken out of the
  91 same local coordinate system.  (Used for debugging only)."
  92   (add-cam* photo)
  93   (add-bpoint* photo)
  94   (add-cam* other-photo)
  95   (add-bpoint* other-photo)
  96   (calculate)
  97   (pairlis '(:x-local :y-local :z-local
  98              :stdx-local :stdy-local :stdz-local)
  99            (list
 100             (get-x-local) (get-y-local) (get-z-local)
 101             (get-stdx-local) (get-stdy-local) (get-stdz-local)
 102             (get-x-global) (get-y-global) (get-z-global))))
 103
 104 (defmethod photogrammetry ((mode (eql :mono)) photo &optional floor)
 105   "Return in an alist the intersection point of the ray through m and
 106 n in photo, and floor."
 107   (add-cam* photo)
 108   (add-bpoint* photo)
 109   (add-ref-ground-surface* floor)
 110   (add-global-car-reference-point* photo)
 111   (set-global-reference-frame)
 112   (calculate)
 113   (pairlis '(:x-global :y-global :z-global)
 114           (list
 115            (get-x-global) (get-y-global) (get-z-global))))
 116
 117 (defun point-radians-to-degrees (point)
 118   "Convert (the first and second element of) point from radians to
 119 degrees."
 120   (setf (first point) (proj:radians-to-degrees (first point)))
 121   (setf (second point) (proj:radians-to-degrees (second point)))
 122   point)
 123
 124 (defmethod photogrammetry ((mode (eql :footprint)) photo
 125                            &optional (floor photo))
 126   "Return image footprint as a list of five polygon points, wrapped in
 127 an alist."
 128   (set-global-reference-frame)
 129   (add-cam* photo)
 130   (add-global-car-reference-point* photo t)
 131   (add-ref-ground-surface* floor)
 132   (set-distance-for-epipolar-line 20d0)
 133   (calculate)
 134   (let ((source-cs
 135          (car (photogrammetry-arglist photo :cartesian-system))))
 136     (acons
 137      :footprint
 138      (loop
 139         for i in '(0 1 2 3 0) collect
 140           (point-radians-to-degrees
 141            (proj:cs2cs (list (get-fp-easting i)
 142                              (get-fp-northing i)
 143                              (get-fp-e-height i))
 144                        :source-cs source-cs)))
 145      nil)))
 146
 147 (defun flip-m-maybe (m photo)
 148   "Flip coordinate m when :mounting-angle in photo suggests it
 149 necessary."
 150   (if (= 180 (cdr (assoc :mounting-angle photo)))
 151       (- (cdr (assoc :sensor-width-pix photo)) m)
 152       m))
 153 (defun flip-n-maybe (n photo)
 154   "Flip coordinate n when :mounting-angle in photo suggests it
 155 necessary."
 156   (if (zerop (cdr (assoc :mounting-angle photo)))
 157       (- (cdr (assoc :sensor-height-pix photo)) n)
 158       n))
 159
 160 (defun photogrammetry-arglist (alist &rest keys)
 161   "Construct an arglist from alist values corresponding to keys."
 162   (mapcar #'(lambda (x) (cdr (assoc x alist))) keys))
 163
 164 (defun add-cam* (photo-alist)
 165   "Call add-cam with arguments taken from photo-alist."
 166   (let ((integer-args
 167          (photogrammetry-arglist
 168           photo-alist :sensor-height-pix :sensor-width-pix))
 169         (double-float-args
 170          (mapcar #'(lambda (x) (coerce x 'double-float))
 171                  (photogrammetry-arglist photo-alist
 172                                          :pix-size
 173                                          :dx :dy :dz :omega :phi :kappa
 174                                          :c :xh :yh
 175                                          :a-1 :a-2 :a-3 :b-1 :b-2
 176                                          :c-1 :c-2 :r-0
 177                                          :b-dx :b-dy :b-dz :b-ddx :b-ddy :b-ddz
 178                                          :b-rotx :b-roty :b-rotz
 179                                          :b-drotx :b-droty :b-drotz))))
 180     (apply #'add-cam (nconc integer-args double-float-args))))
 181
 182 (defun add-bpoint* (photo-alist)
 183   "Call add-bpoint with arguments taken from photo-alist."
 184     (add-bpoint
 185      (coerce (flip-m-maybe (cdr (assoc :m photo-alist)) photo-alist)
 186              'double-float)
 187      (coerce (flip-n-maybe (cdr (assoc :n photo-alist)) photo-alist)
 188              'double-float)))
 189
 190 (defun add-ref-ground-surface* (floor-alist)
 191   "Call add-ref-ground-surface with arguments taken from floor-alist."
 192   (let ((double-float-args
 193          (mapcar #'(lambda (x) (coerce x 'double-float))
 194                  (photogrammetry-arglist floor-alist
 195                                          :nx :ny :nz :d))))
 196     (apply #'add-ref-ground-surface double-float-args)))
 197
 198 (defun add-global-car-reference-point* (photo-alist
 199                                         &optional cam-set-global-p)
 200   "Call add-global-car-reference-point with arguments taken from
 201 photo-alist.  When cam-set-global-p is t, call
 202 add-global-car-reference-point-cam-set-global instead."
 203   (let* ((longitude-radians
 204           (proj:degrees-to-radians
 205            (car (photogrammetry-arglist photo-alist :longitude))))
 206          (latitude-radians
 207           (proj:degrees-to-radians
 208            (car (photogrammetry-arglist photo-alist :latitude))))
 209          (ellipsoid-height
 210           (car (photogrammetry-arglist photo-alist :ellipsoid-height)))
 211          (destination-cs
 212           (car (photogrammetry-arglist photo-alist :cartesian-system)))
 213          (cartesian-coordinates
 214           (proj:cs2cs
 215            (list longitude-radians latitude-radians ellipsoid-height)
 216            :destination-cs destination-cs))
 217          (other-args
 218           (mapcar #'(lambda (x) (coerce x 'double-float))
 219                   (photogrammetry-arglist photo-alist
 220                                           :roll :pitch :heading
 221                                           :latitude :longitude)))
 222          (double-float-args
 223           (nconc cartesian-coordinates other-args)))
 224     (apply (if cam-set-global-p
 225                #'add-global-car-reference-point-cam-set-global
 226                #'add-global-car-reference-point)
 227            double-float-args)))
 228
 229 (defun add-global-measurement-point* (point)
 230   "Call add-global-measurement-point with arguments taken from point."
 231   (let ((double-float-args
 232          (mapcar #'(lambda (x) (coerce x 'double-float))
 233                  (photogrammetry-arglist point
 234                                          :x-global :y-global :z-global))))
 235     (apply #'add-global-measurement-point double-float-args)))