1 ;;; PHOROS -- Photogrammetric Road Survey
2 ;;; Copyright (C) 2010, 2011, 2012 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.
21 (defparameter *picture-header-length-tolerance
* 20
22 "Amount of leeway for the length of a picture header in a .pictures
25 (defun find-keyword-in-stream (stream keyword
&optional start-position search-range
)
26 "Return file-position in binary stream after first occurence of
28 (unless start-position
(setf start-position
0))
29 (let ((end-position (if search-range
30 (+ start-position search-range
)
31 most-positive-fixnum
)))
34 (file-position stream start-position
)
35 (let ((chunk-size (length keyword
)))
37 for next-chunk
= (let ((result (make-array
39 :element-type
'unsigned-byte
)))
40 (read-sequence result stream
)
41 (coerce (map 'vector
#'code-char result
)
43 if
(string/= next-chunk keyword
) do
44 (let ((next-position (- (file-position stream
) chunk-size -
1)))
45 (if (< next-position end-position
)
46 (file-position stream next-position
)
47 (return-from find-keyword-in-stream
)))
48 else return
(file-position stream
))))
49 (end-of-file () nil
))))
51 (defun find-keyword-value (path keyword
&optional start-position search-range
)
52 "Return value associated with keyword."
54 (find-keyword path keyword start-position search-range
)))
56 (with-open-file (stream path
)
57 (file-position stream start-of-value
)
58 (car (read-delimited-list #\
; stream))))))
60 (defun find-keyword (path keyword
&optional start-position search-range
)
61 "Return file-position after keyword."
62 (with-open-file (stream path
:element-type
'unsigned-byte
)
63 (find-keyword-in-stream stream keyword start-position search-range
)))
65 (defun read-huffman-table (stream &optional start-position
)
66 "Return in a hash table a huffman table read from stream. Start
67 either at stream's file position or at start-position."
68 (let ((huffman-codes-start (if start-position
70 (file-position stream
))))
71 (file-position stream
(+ (* 511 4) huffman-codes-start
)) ; start of lengths
72 (let* ((lengths (make-list 511))
73 (huffman-table (make-hash-table :size
1000 :test
#'equal
)))
74 (read-sequence lengths stream
)
75 (file-position stream huffman-codes-start
)
77 for i from -
255 to
255
79 for key
= (make-array (list length
) :element-type
'bit
)
80 for code
= (let ((raw (make-array '(4) :element-type
'unsigned-byte
))
82 (read-sequence raw stream
)
84 for raw-byte across raw
85 for code-position from
24 downto
0 by
8
86 do
(setf code-part
(dpb raw-byte
87 (byte 8 code-position
)
89 finally
(return code-part
)))
92 for key-index from
0 below length
93 for code-index downfrom
(1- length
)
94 do
(setf (sbit key key-index
)
95 (ldb (byte 1 code-index
) code
)))
97 do
(setf (gethash key huffman-table
) i
))
100 (defun read-compressed-picture (stream start-position length
)
101 "Return a compressed picture in a bit array. Start either at
102 start-position or, if that is nil, at stream's file position."
103 (when start-position
(file-position stream start-position
))
104 (let ((raw (make-array (list length
) :element-type
'unsigned-byte
))
106 (make-array (list (* 8 length
)) :element-type
'bit
)))
107 (read-sequence raw stream
)
110 for byte-position from
0
112 for source-bit from
7 downto
0
113 for destination-bit from
0 to
7
114 do
(setf (sbit compressed-picture
116 (* 8 byte-position
)))
117 (ldb (byte 1 source-bit
) byte
)))
118 finally
(return compressed-picture
))))
120 (defun get-leading-byte (bit-array &optional
(start 0) &aux
(result 0))
121 "Return integer made of eight bits from bit-array."
123 for bit-array-index from start
124 for result-index from
7 downto
0
125 for result
= (dpb (sbit bit-array bit-array-index
)
126 (byte 1 result-index
) 0)
127 then
(dpb (sbit bit-array bit-array-index
) (byte 1 result-index
) result
)
128 finally
(return result
)))
130 (defun uncompress-picture (huffman-table compressed-picture
131 height width channels
&key reversep
)
132 "Return the Bayer pattern extracted from compressed-picture, turned
133 upside-down if reversep is t, in an (array (unsigned-byte 8) (height
134 width channels)), everything in channel 0."
135 (declare (optimize (safety 0))
136 (type (unsigned-byte 16) height width
)
137 (type vector compressed-picture
))
138 (let* ((uncompressed-image
139 (make-array (list height width channels
)
140 :element-type
'(unsigned-byte 8)))
141 (uncompressed-image-vector
142 (make-array (list (* height width channels
))
143 :element-type
'(unsigned-byte 8)
144 :displaced-to uncompressed-image
))
146 (channel (if reversep
147 (1- channels
) ;becomes 0 by reversal
149 (compressed-picture-index 0)
152 for code of-type simple-bit-vector being the hash-key in huffman-table
153 minimize
(length code
)))
156 for code of-type simple-bit-vector being the hash-key in huffman-table
157 maximize
(length code
))))
158 (declare (type (signed-byte 48) compressed-picture-index
)
159 (type (unsigned-byte 8) channels
))
161 for row from
0 below height
163 (setf (aref uncompressed-image row
0 channel
)
164 (get-leading-byte compressed-picture
165 (prog1 compressed-picture-index
166 (incf compressed-picture-index
8))))
167 (setf (aref uncompressed-image row
1 channel
)
168 (get-leading-byte compressed-picture
169 (prog1 compressed-picture-index
170 (incf compressed-picture-index
8))))
172 for column from
2 below width
173 for try-start of-type
(unsigned-byte 48) from compressed-picture-index
176 for key-length from min-key-length to max-key-length
177 for huffman-code
= (subseq compressed-picture
178 try-start
(+ try-start key-length
))
179 for pixel-delta-maybe
= (gethash huffman-code huffman-table
)
180 when pixel-delta-maybe
182 (setf (aref uncompressed-image row column channel
)
183 (- (aref uncompressed-image row
(- column
2) channel
)
184 (the fixnum pixel-delta-maybe
)))
185 and do
(incf try-start
(1- key-length
))
188 "Decoder out of step at row ~S, column ~S. Giving up."
191 (setf compressed-picture-index
(1+ try-start
))))
192 (when reversep
(setf uncompressed-image-vector
(reverse uncompressed-image-vector
)))
195 (defun fetch-picture (stream start-position length height width channels
197 "Return the Bayer pattern taken from stream in an (array (unsigned-byte l8) (height width channels)),
198 everything in color channel 0. Start at start-position or, if that is
199 nil, at stream's file position."
200 (when start-position
(file-position stream start-position
))
202 (make-array (list height width channels
)
203 :element-type
'(unsigned-byte 8)))
205 (make-array (list (* height width channels
))
206 :element-type
'(unsigned-byte 8)
207 :displaced-to image
))
209 (make-array (list length
) :element-type
'unsigned-byte
)))
212 (read-sequence image-vector stream
))
214 (error "Not implemented: fetch-picture for (uncompressed) truecolor images")
215 ;; (read-sequence raw-image stream)
217 ;; for pixel across raw-image and red from 0 by 3 do
218 ;; (setf (svref png-image-data red) pixel))
220 (when reversep
(setf image-vector
(reverse image-vector
)))
223 (defun complete-horizontally (image row column color
)
224 "Fake a color component of a pixel based its neighbors."
225 (declare (optimize (safety 0))
228 (type image-dimension row column
))
229 (setf (aref image row column color
)
230 (round (+ (aref image row
(1- column
) color
)
231 (aref image row
(1+ column
) color
))
234 (defun complete-vertically (image row column color
)
235 "Fake a color component of a pixel based its neighbors."
236 (declare (optimize (safety 0))
239 (type image-dimension row column
))
240 (setf (aref image row column color
)
241 (round (+ (aref image
(1- row
) column color
)
242 (aref image
(1+ row
) column color
))
245 (defun complete-squarely (image row column color
)
246 "Fake a color component of a pixel based its neighbors."
247 (declare (optimize (safety 0))
250 (type image-dimension row column
))
251 (setf (aref image row column color
)
252 (round (+ (aref image row
(1- column
) color
)
253 (aref image row
(1+ column
) color
)
254 (aref image
(1- row
) column color
)
255 (aref image
(1+ row
) column color
))
258 (defun complete-diagonally (image row column color
)
259 "Fake a color component of a pixel based its neighbors."
260 (declare (optimize (safety 0))
263 (type image-dimension row column
))
264 (setf (aref image row column color
)
265 (round (+ (aref image
(1- row
) (1- column
) color
)
266 (aref image
(1- row
) (1+ column
) color
)
267 (aref image
(1+ row
) (1- column
) color
)
268 (aref image
(1+ row
) (1+ column
) color
))
271 (deftype image-dimension
() '(unsigned-byte 16))
272 (deftype image
() '(simple-array (unsigned-byte 8) 3))
274 (defun height (image) (array-dimension image
0))
275 (defun width (image) (array-dimension image
1))
276 (defun channels (image) (array-dimension image
2))
278 (defun demosaic-png (png bayer-pattern color-raiser brightenp
) ;TODO: s/png/image
279 "Demosaic color png whose color channel 0 is supposed to be
280 filled with a Bayer color pattern. Return demosaiced png.
281 bayer-pattern is an array of 24-bit RGB values (red occupying the
282 least significant byte), describing the upper left corner of the
283 image. Currently, only pixels 0, 1 on row 0 are taken into account.
284 And, it's currently not even an array but a vector due to limitations
285 in postmodern. For a grayscale image do nothing. Then, if brightenp
286 is t and the image is too dark, make it brighter."
287 (declare (optimize (safety 0))
290 (when (= 3 (channels png
))
291 (let ((lowest-row (- (height png
) 2))
292 (rightmost-column (- (width png
) 2))
293 (bayer-pattern-red #x0000ff
)
294 (bayer-pattern-green #x00ff00
)
295 (bayer-pattern-blue #xff0000
)
296 (red 0) (green 1) (blue 2) ;color coordinate in PNG array
297 (color-raiser-red (coerce (elt color-raiser
0) '(single-float -
10.0s0
10.0s0
)))
298 (color-raiser-green (coerce (elt color-raiser
1) '(single-float -
10.0s0
10.0s0
)))
299 (color-raiser-blue (coerce (elt color-raiser
2) '(single-float -
10.0s0
10.0s0
)))
300 (pix-depth 255) ;may some day become a function argument
301 complete-even-row-even-column
302 complete-even-row-odd-column
303 complete-odd-row-even-column
304 complete-odd-row-odd-column
305 colorize-even-row-even-column
306 colorize-even-row-odd-column
307 colorize-odd-row-even-column
308 colorize-odd-row-odd-column
)
309 (declare (type image-dimension lowest-row rightmost-column
)
311 (flet ((complete-green-on-red-row (row column
)
312 (complete-horizontally png row column red
)
313 (complete-vertically png row column blue
))
314 (complete-green-on-blue-row (row column
)
315 (complete-horizontally png row column blue
)
316 (complete-vertically png row column red
))
317 (complete-red (row column
)
318 (complete-squarely png row column green
)
319 (complete-diagonally png row column blue
))
320 (complete-blue (row column
)
321 (complete-squarely png row column green
)
322 (complete-diagonally png row column red
))
323 (colorize-red (row column
)
324 (setf (aref png row column red
)
326 (round (* color-raiser-red
329 (colorize-green (row column
)
330 (setf (aref png row column green
)
332 (round (* color-raiser-green
335 (colorize-blue (row column
)
336 (setf (aref png row column blue
)
338 (round (* color-raiser-blue
340 row column red
)))))))
342 ((= (aref bayer-pattern
0) bayer-pattern-red
)
343 (setf colorize-even-row-even-column
#'colorize-red
)
344 (setf colorize-even-row-odd-column
#'colorize-green
)
345 (setf colorize-odd-row-even-column
#'colorize-green
)
346 (setf colorize-odd-row-odd-column
#'colorize-blue
)
347 (setf complete-even-row-even-column
#'complete-red
)
348 (setf complete-even-row-odd-column
#'complete-green-on-red-row
)
349 (setf complete-odd-row-even-column
#'complete-green-on-blue-row
)
350 (setf complete-odd-row-odd-column
#'complete-blue
))
351 ((= (aref bayer-pattern
0) bayer-pattern-blue
)
352 (setf colorize-even-row-even-column
#'colorize-blue
)
353 (setf colorize-even-row-odd-column
#'colorize-green
)
354 (setf colorize-odd-row-even-column
#'colorize-green
)
355 (setf colorize-odd-row-odd-column
#'colorize-red
)
356 (setf complete-even-row-even-column
#'complete-blue
)
357 (setf complete-even-row-odd-column
#'complete-green-on-blue-row
)
358 (setf complete-odd-row-even-column
#'complete-green-on-red-row
)
359 (setf complete-odd-row-odd-column
#'complete-red
))
360 ((= (aref bayer-pattern
0) bayer-pattern-green
)
362 ((=(aref bayer-pattern
1) bayer-pattern-red
)
363 (setf colorize-even-row-even-column
#'colorize-green
)
364 (setf colorize-even-row-odd-column
#'colorize-red
)
365 (setf colorize-odd-row-even-column
#'colorize-blue
)
366 (setf colorize-odd-row-odd-column
#'colorize-green
)
367 (setf complete-even-row-even-column
#'complete-green-on-red-row
)
368 (setf complete-even-row-odd-column
#'complete-red
)
369 (setf complete-odd-row-even-column
#'complete-blue
)
370 (setf complete-odd-row-odd-column
#'complete-green-on-blue-row
))
371 ((=(aref bayer-pattern
1) bayer-pattern-blue
)
372 (setf colorize-even-row-even-column
#'colorize-green
)
373 (setf colorize-even-row-odd-column
#'colorize-blue
)
374 (setf colorize-odd-row-even-column
#'colorize-red
)
375 (setf colorize-odd-row-odd-column
#'colorize-green
)
376 (setf complete-even-row-even-column
#'complete-green-on-blue-row
)
377 (setf complete-even-row-odd-column
#'complete-blue
)
378 (setf complete-odd-row-even-column
#'complete-red
)
379 (setf complete-odd-row-odd-column
#'complete-green-on-red-row
))
380 (t (error "Don't know how to deal with a bayer-pattern of ~A"
382 (t (error "Don't know how to deal with a bayer-pattern of ~A"
384 ;; Recover colors (so far everything is in channel 0)
385 (loop for row from
0 below
(the image-dimension
(height png
)) by
2
386 do
(loop for column from
0 below
(the image-dimension
(width png
)) by
2
387 do
(funcall colorize-even-row-even-column row column
))
388 (loop for column from
1 below
(the image-dimension
(width png
)) by
2
389 do
(funcall colorize-even-row-odd-column row column
)))
390 (loop for row from
1 below
(the image-dimension
(height png
)) by
2
391 do
(loop for column from
0 below
(the image-dimension
(width png
)) by
2
392 do
(funcall colorize-odd-row-even-column row column
))
393 (loop for column from
1 below
(the image-dimension
(width png
)) by
2
394 do
(funcall colorize-odd-row-odd-column row column
)))
397 for row from
2 to lowest-row by
2 do
399 for column from
2 to rightmost-column by
2 do
400 (funcall complete-even-row-even-column row column
))
402 for column from
1 to rightmost-column by
2 do
403 (funcall complete-even-row-odd-column row column
)))
405 for row from
1 to lowest-row by
2 do
407 for column from
2 to rightmost-column by
2 do
408 (funcall complete-odd-row-even-column row column
))
410 for column from
1 to rightmost-column by
2 do
411 (funcall complete-odd-row-odd-column row column
))))))
412 (when brightenp
(brighten-maybe png
))
415 (defun brighten-maybe (png) ;TODO s/png/image-or-something/
416 "Make png brighter if it is too dark."
417 (multiple-value-bind (brightest-value darkest-value
)
419 (when (< brightest-value
200)
420 (let ((image (make-array (list (* (height png
) (width png
) (channels png
))) :element-type
'(unsigned-byte 8) :displaced-to png
)))
422 for i from
0 below
(length image
)
423 do
(setf (aref image i
)
424 (floor (* (- (aref image i
) darkest-value
)
425 (/ 255 (- brightest-value darkest-value
))))))))))
427 (defun brightness (png)
428 "Return brightest value and darkest value of png." ;TODO: s/png/image/
429 (let ((image (make-array (list (* (height png
) (width png
) (channels png
)))
430 :element-type
'(unsigned-byte 8)
433 for brightness across image
434 maximize brightness into brightest-value
435 minimize brightness into darkest-value
436 finally
(return (values brightest-value
439 (defun* send-png
(output-stream path start
440 &key
(color-raiser #(1 1 1))
442 &mandatory-key bayer-pattern
)
443 "Read an image at position start in .pictures file at path and send
444 it to the binary output-stream. Return UNIX trigger-time of image.
445 If brightenp is t, have it brightened up if necessary. If reversep is
446 t, turn it upside-down. Bayer-pattern is applied after turning, which
448 ;; TODO: bayer-pattern should be applied to the unturned image
449 (let ((blob-start (find-keyword path
"PICTUREDATA_BEGIN" start
))
450 (blob-size (find-keyword-value path
"dataSize=" start
))
451 (huffman-table-size (* 511 (+ 1 4)))
452 (image-height (find-keyword-value path
"height=" start
))
453 (image-width (find-keyword-value path
"width=" start
))
454 (compression-mode (find-keyword-value path
"compressed=" start
))
455 (channels (find-keyword-value path
"channels=" start
))
456 (trigger-time (find-keyword-value path
"timeTrigger=" start
)))
457 (assert (member channels
'(1 3)) ()
458 "Don't know how to deal with ~D-channel pixels." channels
)
459 (with-open-file (input-stream path
:element-type
'unsigned-byte
)
460 (let* ((image (demosaic-png
461 (ecase compression-mode
462 ((2 1) ;compressed with individual/pre-built huffman table
463 (uncompress-picture (read-huffman-table input-stream blob-start
)
464 (read-compressed-picture
466 (+ blob-start huffman-table-size
)
467 (- blob-size huffman-table-size
))
468 image-height image-width channels
471 (fetch-picture input-stream blob-start blob-size
472 image-height image-width channels
473 :reversep reversep
)))
477 (zpng:write-png-stream
;TODO: generalize
479 (make-instance 'zpng
:png
480 :height
(height image
)
482 :color-type
(getf '(1 :grayscale
3 :truecolor
)
484 :image-data
(make-array
485 (list (* (height image
) (width image
)
487 :element-type
'(unsigned-byte 8)
488 :displaced-to image
)))
492 (defun find-nth-picture (n path
)
493 "Find file-position of zero-indexed nth picture in in .pictures file
495 (let ((estimated-header-length
496 (- (find-keyword path
"PICTUREHEADER_END")
497 (find-keyword path
"PICTUREHEADER_BEGIN")
498 *picture-header-length-tolerance
*))) ; allow for variation in dataSize and a few other parameters
502 (find-keyword path
"PICTUREHEADER_BEGIN" 0) then
503 (find-keyword path
"PICTUREHEADER_BEGIN"
504 (+ picture-start picture-length estimated-header-length
))
505 for picture-length
= (find-keyword-value path
506 "dataSize=" picture-start
)
507 finally
(return (- picture-start
(length "PICTUREHEADER_BEGIN"))))))
509 (defun* send-nth-png
(n output-stream path
510 &key
(color-raiser #(1 1 1))
511 &mandatory-key bayer-pattern
)
512 "Read image number n (zero-indexed) in .pictures file at path and
513 send it to the binary output-stream. Return UNIX trigger-time of
515 (send-png output-stream path
(find-nth-picture n path
)
516 :bayer-pattern bayer-pattern
:color-raiser color-raiser
))
520 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
521 ;; collect 4 single color pixels into a three-color one
522 ;; enhance contrast of grayscale images