1 ;;; PHOROS -- Photogrammetric Road Survey
2 ;;; Copyright (C) 2010, 2011 Bert Burgemeister
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.
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.
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.
18 (in-package :phoros-photogrammetry
)
20 #-sbcl
(defun nan-p (x)
23 #+sbcl
(defun nan-p (x)
24 (sb-ext:float-nan-p x
))
26 (defgeneric photogrammetry
(mode photo-1
&optional photo-2
)
27 (:documentation
"Call to photogrammetry library. Dispatch on mode."))
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
*)
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
)
49 for i
= 2d0 then
(* i
1.4) until
(> i
50)
50 do
(set-distance-for-epipolar-line i
)
55 (assert (not (nan-p m
))) ;On some systems, PhoML gives us
56 (assert (not (nan-p n
))) ; quiet NaN instead of erring.
58 collect
(pairlis '(:m
:n
)
59 (list (flip-m-maybe m other-photo
)
60 (flip-n-maybe n other-photo
)))))
62 (defmethod photogrammetry
63 ((mode (eql :reprojection
)) photo
&optional global-point
)
64 "Calculate reprojection from photo."
66 (add-global-measurement-point* global-point
)
67 (add-global-car-reference-point* photo
)
68 (set-global-reference-frame)
72 (assert (not (nan-p m
))) ;On some systems, PhoML gives us
73 (assert (not (nan-p n
))) ; quiet NaN instead of erring.
75 (list (flip-m-maybe m photo
) (flip-n-maybe n photo
)))))
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)
87 (add-global-car-reference-point* photo t
))
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
)
104 x-global y-global z-global
105 stdx-global stdy-global stdz-global
))))
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)."
113 (add-cam* other-photo
)
114 (add-bpoint* other-photo
)
116 (pairlis '(:x-local
:y-local
:z-local
117 :stdx-local
:stdy-local
:stdz-local
)
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))))
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."
128 (add-ref-ground-surface* floor
)
129 (add-global-car-reference-point* photo
)
130 (set-global-reference-frame)
132 (pairlis '(:x-global
:y-global
:z-global
)
134 (get-x-global) (get-y-global) (get-z-global))))
136 (defun point-radians-to-degrees (point)
137 "Convert (the first and second element of) point from radians to
139 (setf (first point
) (proj:radians-to-degrees
(first point
)))
140 (setf (second point
) (proj:radians-to-degrees
(second point
)))
143 (defmethod photogrammetry ((mode (eql :footprint
)) photo
144 &optional
(floor photo
))
145 "Return image footprint as a list of five polygon points, wrapped in
147 (set-global-reference-frame)
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
154 (car (photogrammetry-arglist photo
:cartesian-system
))))
158 for i in
'(0 1 2 3 0) collect
159 (point-radians-to-degrees
160 (proj:cs2cs
(list (get-fp-easting i
)
163 :source-cs source-cs
)))
166 (defun flip-m-maybe (m photo
)
167 "Flip coordinate m when :mounting-angle in photo suggests it
169 (if (= 180 (cdr (assoc :mounting-angle photo
)))
170 (- (cdr (assoc :sensor-width-pix photo
)) m
)
172 (defun flip-n-maybe (n photo
)
173 "Flip coordinate n when :mounting-angle in photo suggests it
175 (if (zerop (cdr (assoc :mounting-angle photo
)))
176 (- (cdr (assoc :sensor-height-pix photo
)) n
)
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
))
183 (defun add-cam* (photo-alist)
184 "Call add-cam with arguments taken from photo-alist."
186 (photogrammetry-arglist
187 photo-alist
:sensor-height-pix
:sensor-width-pix
))
189 (mapcar #'(lambda (x) (coerce x
'double-float
))
190 (photogrammetry-arglist photo-alist
192 :dx
:dy
:dz
:omega
:phi
:kappa
194 :a-1
:a-2
:a-3
:b-1
:b-2
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
))))
201 (defun add-bpoint* (photo-alist)
202 "Call add-bpoint with arguments taken from photo-alist."
204 (coerce (flip-m-maybe (cdr (assoc :m photo-alist
)) photo-alist
)
206 (coerce (flip-n-maybe (cdr (assoc :n photo-alist
)) photo-alist
)
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
215 (apply #'add-ref-ground-surface double-float-args
)))
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
))))
226 (proj:degrees-to-radians
227 (car (photogrammetry-arglist photo-alist
:latitude
))))
229 (car (photogrammetry-arglist photo-alist
:ellipsoid-height
)))
231 (car (photogrammetry-arglist photo-alist
:cartesian-system
)))
232 (cartesian-coordinates
234 (list longitude-radians latitude-radians ellipsoid-height
)
235 :destination-cs destination-cs
))
237 (mapcar #'(lambda (x) (coerce x
'double-float
))
238 (photogrammetry-arglist photo-alist
239 :roll
:pitch
:heading
240 :latitude
:longitude
)))
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
)
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
)))