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      with m and n
  49      for i = 2d0 then (* i 1.4) until (> i 50)
  50      do (set-distance-for-epipolar-line i)
  51      when (ignore-errors
  52             (calculate)
  53             (setf m (get-m))
  54             (setf n (get-n))
  55             (assert (not (nan-p m)))  ;On some systems, PhoML gives us
  56             (assert (not (nan-p n)))  ; quiet NaN instead of erring.
  57             t)
  58      collect (pairlis '(:m :n)
  59                       (list (flip-m-maybe m other-photo)
  60                             (flip-n-maybe n other-photo)))))
  61
  62 (defmethod photogrammetry
  63     ((mode (eql :reprojection)) photo &optional global-point)
  64   "Calculate reprojection from photo."
  65   (add-cam* photo)
  66   (add-global-measurement-point* global-point)
  67   (add-global-car-reference-point* photo)
  68   (set-global-reference-frame)
  69   (calculate)
  70   (let ((m (get-m))
  71         (n (get-n)))
  72     (assert (not (nan-p m)))          ;On some systems, PhoML gives us
  73     (assert (not (nan-p n)))          ; quiet NaN instead of erring.
  74     (pairlis '(:m :n)
  75              (list (flip-m-maybe m photo) (flip-n-maybe n photo)))))
  76
  77 (defmethod photogrammetry
  78     ((mode (eql :multi-position-intersection)) photos &optional other-photo)
  79   "Calculate intersection from photos."
  80   (declare (ignore other-photo))
  81   (set-global-reference-frame)
  82   (loop
  83      for photo in photos
  84      do
  85        (add-cam* photo)
  86        (add-bpoint* photo)
  87        (add-global-car-reference-point* photo t))
  88   (calculate)
  89   (let ((x-global (get-x-global))
  90         (y-global (get-y-global))
  91         (z-global (get-z-global))
  92         (stdx-global (get-stdx-global))
  93         (stdy-global (get-stdy-global))
  94         (stdz-global (get-stdz-global)))
  95     (assert (not (nan-p x-global)))
  96     (assert (not (nan-p y-global)))
  97     (assert (not (nan-p z-global)))
  98     (assert (not (nan-p stdx-global)))
  99     (assert (not (nan-p stdy-global)))
 100     (assert (not (nan-p stdz-global)))
 101     (pairlis '(:x-global :y-global :z-global
 102                :stdx-global :stdy-global :stdz-global)
 103              (list
 104               x-global y-global z-global
 105               stdx-global stdy-global stdz-global))))
 106
 107 (defmethod photogrammetry
 108     ((mode (eql :intersection)) photo &optional other-photo)
 109   "Calculate intersection from two photos that are taken out of the
 110 same local coordinate system.  (Used for debugging only)."
 111   (add-cam* photo)
 112   (add-bpoint* photo)
 113   (add-cam* other-photo)
 114   (add-bpoint* other-photo)
 115   (calculate)
 116   (pairlis '(:x-local :y-local :z-local
 117              :stdx-local :stdy-local :stdz-local)
 118            (list
 119             (get-x-local) (get-y-local) (get-z-local)
 120             (get-stdx-local) (get-stdy-local) (get-stdz-local)
 121             (get-x-global) (get-y-global) (get-z-global))))
 122
 123 (defmethod photogrammetry ((mode (eql :mono)) photo &optional floor)
 124   "Return in an alist the intersection point of the ray through m and
 125 n in photo, and floor."
 126   (add-cam* photo)
 127   (add-bpoint* photo)
 128   (add-ref-ground-surface* floor)
 129   (add-global-car-reference-point* photo)
 130   (set-global-reference-frame)
 131   (calculate)
 132   (pairlis '(:x-global :y-global :z-global)
 133           (list
 134            (get-x-global) (get-y-global) (get-z-global))))
 135
 136 (defun point-radians-to-degrees (point)
 137   "Convert (the first and second element of) point from radians to
 138 degrees."
 139   (setf (first point) (proj:radians-to-degrees (first point)))
 140   (setf (second point) (proj:radians-to-degrees (second point)))
 141   point)
 142
 143 (defmethod photogrammetry ((mode (eql :footprint)) photo
 144                            &optional (floor photo))
 145   "Return image footprint as a list of five polygon points, wrapped in
 146 an alist."
 147   (set-global-reference-frame)
 148   (add-cam* photo)
 149   (add-global-car-reference-point* photo t)
 150   (add-ref-ground-surface* floor)
 151   (set-distance-for-epipolar-line 20d0) ;how far ahead we look
 152   (calculate)
 153   (let ((source-cs
 154          (car (photogrammetry-arglist photo :cartesian-system))))
 155     (acons
 156      :footprint
 157      (loop
 158         for i in '(0 1 2 3 0) collect
 159           (point-radians-to-degrees
 160            (proj:cs2cs (list (get-fp-easting i)
 161                              (get-fp-northing i)
 162                              (get-fp-e-height i))
 163                        :source-cs source-cs)))
 164      nil)))
 165
 166 (defun flip-m-maybe (m photo)
 167   "Flip coordinate m when :mounting-angle in photo suggests it
 168 necessary."
 169   (if (= 180 (cdr (assoc :mounting-angle photo)))
 170       (- (cdr (assoc :sensor-width-pix photo)) m)
 171       m))
 172 (defun flip-n-maybe (n photo)
 173   "Flip coordinate n when :mounting-angle in photo suggests it
 174 necessary."
 175   (if (zerop (cdr (assoc :mounting-angle photo)))
 176       (- (cdr (assoc :sensor-height-pix photo)) n)
 177       n))
 178
 179 (defun photogrammetry-arglist (alist &rest keys)
 180   "Construct an arglist from alist values corresponding to keys."
 181   (mapcar #'(lambda (x) (cdr (assoc x alist))) keys))
 182
 183 (defun add-cam* (photo-alist)
 184   "Call add-cam with arguments taken from photo-alist."
 185   (let ((integer-args
 186          (photogrammetry-arglist
 187           photo-alist :sensor-height-pix :sensor-width-pix))
 188         (double-float-args
 189          (mapcar #'(lambda (x) (coerce x 'double-float))
 190                  (photogrammetry-arglist photo-alist
 191                                          :pix-size
 192                                          :dx :dy :dz :omega :phi :kappa
 193                                          :c :xh :yh
 194                                          :a-1 :a-2 :a-3 :b-1 :b-2
 195                                          :c-1 :c-2 :r-0
 196                                          :b-dx :b-dy :b-dz :b-ddx :b-ddy :b-ddz
 197                                          :b-rotx :b-roty :b-rotz
 198                                          :b-drotx :b-droty :b-drotz))))
 199     (apply #'add-cam (nconc integer-args double-float-args))))
 200
 201 (defun add-bpoint* (photo-alist)
 202   "Call add-bpoint with arguments taken from photo-alist."
 203     (add-bpoint
 204      (coerce (flip-m-maybe (cdr (assoc :m photo-alist)) photo-alist)
 205              'double-float)
 206      (coerce (flip-n-maybe (cdr (assoc :n photo-alist)) photo-alist)
 207              'double-float)))
 208
 209 (defun add-ref-ground-surface* (floor-alist)
 210   "Call add-ref-ground-surface with arguments taken from floor-alist."
 211   (let ((double-float-args
 212          (mapcar #'(lambda (x) (coerce x 'double-float))
 213                  (photogrammetry-arglist floor-alist
 214                                          :nx :ny :nz :d))))
 215     (apply #'add-ref-ground-surface double-float-args)))
 216
 217 (defun add-global-car-reference-point* (photo-alist
 218                                         &optional cam-set-global-p)
 219   "Call add-global-car-reference-point with arguments taken from
 220 photo-alist.  When cam-set-global-p is t, call
 221 add-global-car-reference-point-cam-set-global instead."
 222   (let* ((longitude-radians
 223           (proj:degrees-to-radians
 224            (car (photogrammetry-arglist photo-alist :longitude))))
 225          (latitude-radians
 226           (proj:degrees-to-radians
 227            (car (photogrammetry-arglist photo-alist :latitude))))
 228          (ellipsoid-height
 229           (car (photogrammetry-arglist photo-alist :ellipsoid-height)))
 230          (destination-cs
 231           (car (photogrammetry-arglist photo-alist :cartesian-system)))
 232          (cartesian-coordinates
 233           (proj:cs2cs
 234            (list longitude-radians latitude-radians ellipsoid-height)
 235            :destination-cs destination-cs))
 236          (other-args
 237           (mapcar #'(lambda (x) (coerce x 'double-float))
 238                   (photogrammetry-arglist photo-alist
 239                                           :roll :pitch :heading
 240                                           :latitude :longitude)))
 241          (double-float-args
 242           (nconc cartesian-coordinates other-args)))
 243     (apply (if cam-set-global-p
 244                #'add-global-car-reference-point-cam-set-global
 245                #'add-global-car-reference-point)
 246            double-float-args)))
 247
 248 (defun add-global-measurement-point* (point)
 249   "Call add-global-measurement-point with arguments taken from point."
 250   (let ((double-float-args
 251          (mapcar #'(lambda (x) (coerce x 'double-float))
 252                  (photogrammetry-arglist point
 253                                          :x-global :y-global :z-global))))
 254     (apply #'add-global-measurement-point double-float-args)))