home *** CD-ROM | disk | FTP | other *** search
/ vis-ftp.cs.umass.edu / vis-ftp.cs.umass.edu.tar / vis-ftp.cs.umass.edu / pub / Software / ASCENDER / ascendMar8.tar / UMass / Epipolar / epimatch.lisp < prev    next >
Lisp/Scheme  |  1994-01-29  |  7KB  |  207 lines
  1. ;;; EPIMATCH.LISP
  2. ;;;
  3. ;;; Top-level routines for epipolar line matching
  4. ;;;
  5. ;;; Author: Robert T. Collins
  6. ;;; Date: Mar 1, 1995
  7. ;;; based on my earlier epipolar.lisp, written Dec 28, 1993
  8. ;;;
  9. ;-----------------------------------------------------------------
  10. ; (c) Copyright 1995 by The University of Massachusetts
  11. ;------------------------------------------------------------------
  12.  
  13. (in-package 'epipolar :nicknames '(epi))
  14.  
  15.  
  16. ;;;;================================================================
  17. ;;;; INITIALIZE MATCHING SYSTEM
  18.  
  19. (defun init-epipolar-histograms (&optional (numbuckets *num-z-buckets*)
  20.                      (lowvalue *lowz*) (highvalue *highz*))
  21.   (setf *current-histogram* 
  22.     (init-height-histogram numbuckets lowvalue highvalue))
  23.   (setf *accum-histogram*
  24.     (init-height-histogram numbuckets lowvalue highvalue))
  25.   t)
  26.  
  27.  
  28. (defun init-epipolar-matcher (all-views)
  29.   (init-epipolar-histograms)
  30.   (when *demo-mode*
  31.       (init-epipolar-screens all-views)))
  32.  
  33.  
  34. ;;;;================================================================
  35. ;;;; MULTI-IMAGE EPIPOLAR MATCHING 
  36.  
  37. (defstruct (epimatch (:print-function print-epimatch-struct))
  38.   (reference-view nil)
  39.   (line-list nil)
  40.   (rough-zvalue nil)
  41.   (list-of-viewmatches nil)
  42.   (peak-confidence nil))
  43.  
  44. (defun print-epimatch-struct (epimatch stream &rest ignore)
  45.   ignore
  46.   (format stream "<EPIMATCH ~a>"
  47.      (view-label (epimatch-reference-view epimatch))))
  48.  
  49. (defstruct viewmatch
  50.   (view nil)
  51.   (match-list nil))
  52.  
  53. (defun generate-indices (low high)
  54.   (let ((result nil))
  55.     (do ((i high (- i 1)))
  56.     ((<= i low) (push low result))
  57.       (push i result))))
  58.      
  59. (defun interpolate-peak (histogram &optional
  60.                (peak-index (hist::find-highest-peak histogram)))
  61.   (let* ((array (hist::histogram-array histogram))
  62.      (min-index (max (- peak-index 1) 0))
  63.      (max-index (min (+ peak-index 1) (- (length array) 1)))
  64.      (sum 0.0)
  65.      (sum-weights 0.0)
  66.      (numbuckets (+ 1 (- max-index min-index))))
  67.     (dotimes (i numbuckets)
  68.       (incf sum-weights (aref array (+ i min-index)))
  69.       (incf sum (* (aref array (+ i min-index)) 
  70.            (hist::index-to-value histogram (+ i min-index)))))
  71.     (values
  72.       (if (zerop sum-weights) -9999.9 (/ sum sum-weights))
  73.       (aref array peak-index)
  74.       (generate-indices min-index max-index))))
  75.  
  76.  
  77.  
  78. (defun epipolar-match-linelist (reference-view linelist other-view-list
  79.                   &key (endpoint-slop 1.0)(delta-theta .1)
  80.                   &aux save-pause-mode)
  81.   (when *demo-mode*
  82.     (setf save-pause-mode *pause-mode*)
  83.     (let ((window (view-window reference-view)))
  84.       (activate-window window)
  85.       (let ((minx (reduce #'min linelist :key #'car))
  86.         (miny (reduce #'min linelist :key #'cadr))
  87.         (maxx (reduce #'max linelist :key #'car))
  88.         (maxy (reduce #'max linelist :key #'cadr)))
  89.     (cme::zoom-to-bounding-box window minx miny maxx maxy 10))
  90.       (dolist (line linelist)
  91.     (display-line window (car line) (cadr line) (third line) (fourth line)
  92.               :color *match-color* :thickness *match-thickness*))))
  93.   (clear-height-histogram *accum-histogram*)
  94.   (dolist (other-view other-view-list)
  95.     (clear-height-histogram *current-histogram*)
  96.     (histogram-linelist-candidates
  97.        reference-view linelist other-view
  98.        :histogram *current-histogram*
  99.        :deltatheta delta-theta
  100.        :endpoint-slop endpoint-slop
  101.        :peak-weight? t)
  102.     (add-to-height-histogram *accum-histogram* *current-histogram*)
  103.     (when *demo-mode*
  104.        (synch-epipolar-screen (view-window other-view))
  105.        (when (eq *pause-mode* :views)
  106.       (setf *pause-mode* 
  107.         (and (yes-or-no-p "Pause between views?") :views))))
  108. ;          (plot-histogram *plot1* *accum-histogram* :max 3)
  109.     )
  110.   (setf *pause-mode* :views)
  111.   (let* ((peak-index (hist::find-highest-peak *accum-histogram*))
  112.      (epimatch (make-epimatch :reference-view reference-view
  113.                   :line-list linelist
  114.                   :list-of-viewmatches nil))
  115.      (halfwidth (/ (hist::histogram-bucket-width *accum-histogram*) 2.0))
  116.      (zpeak nil)
  117.      (zconf nil))
  118.     (multiple-value-setq (zpeak zconf) 
  119.     (interpolate-peak *accum-histogram* peak-index))
  120.     (setf (epimatch-rough-zvalue epimatch) zpeak)
  121. ; Normalized by number of edges, Jan. 30, 1996
  122.     (setf (epimatch-peak-confidence epimatch) (/  zconf (length linelist)))
  123.     (dolist (view other-view-list)
  124.       (push (make-viewmatch
  125.          :view view 
  126.          :match-list
  127.          (collect-linelist-matches
  128.              reference-view linelist view
  129.          :lowz (- zpeak halfwidth) :highz (+ zpeak halfwidth)
  130.          :midz zpeak
  131.          :deltatheta delta-theta
  132.          :endpoint-slop endpoint-slop
  133.          :only-display-matches t))
  134.         (epimatch-list-of-viewmatches epimatch))
  135.       (when *demo-mode*
  136.     (synch-epipolar-screen (view-window view))
  137.     (when *pause-mode*
  138.       (setf *pause-mode* 
  139.         (yes-or-no-p "Pause between views?")))))
  140.     (setf *pause-mode* save-pause-mode)
  141.     epimatch))
  142.  
  143. (defun count-matchlist-numlines (matchlist)
  144.   (apply #'+ (mapcar #'(lambda (x) (length (cadr x))) matchlist)))
  145.  
  146.  
  147. (defun write-proj-info (file view)
  148.   (ecase (intern (format nil "~a" (type-of (view-projection view))) :keyword)
  149.      (:FAST-BLOCK-INTERPOLATION-PROJECTION
  150.       (format file "1~%~a~%" (view-projfile view))
  151.       (format file "0~%"))
  152.      (:COMPOSITE-COORDINATE-PROJECTION
  153.       (format file "1~%~a~%" (view-projfile view))
  154.       (format file "0~%"))
  155.      (:4X4-COORDINATE-PROJECTION
  156.       (format file "0~%")
  157.       (format file "1~%")
  158.       (let ((mat (cme::projection-matrix (view-projection view))))
  159.     (dotimes (i 4)
  160.       (dotimes (j 4)
  161.          (format file " ~,8f" (aref mat i j)))
  162.       (format file "~%"))))))
  163.  
  164.   
  165. (defun save-matches-to-file (epimatch filename)
  166.   (let ((linelist (epimatch-line-list epimatch))
  167.     (refview (epimatch-reference-view epimatch))
  168.     (zvalue (epimatch-rough-zvalue epimatch))
  169.     (list-of-matches (epimatch-list-of-viewmatches epimatch)))
  170.     (with-open-file (file filename :direction :output)
  171.     ;;number of lines followed by initial estimates of each (two 3d endpoints)
  172.      (format file "~d~%" (length linelist))   ;number of polygon lines
  173.      (let ((refproj (view-projection refview)))
  174.        (dolist (line linelist)
  175.      (let ((x1y1 (backproject-point
  176.                 refproj (car line) (cadr line) zvalue))
  177.            (x2y2 (backproject-point 
  178.                 refproj (third line) (fourth line) zvalue)))
  179.        (format file "~,3f ~,3f ~,3f   ~,3f  ~,3f  ~,3f~%"
  180.            (car x1y1) (cadr x1y1) zvalue
  181.            (car x2y2) (cadr x2y2) zvalue))))
  182.     ;;number of images
  183.      (format file "~d~%" (+ 1 (length list-of-matches)))  ;number of images
  184.     ;;first image is the reference view
  185.      (write-proj-info file refview)
  186.      (format file "~d~%" (length linelist))
  187.      (dotimes (i (length linelist))
  188.       (let ((line (elt linelist i)))
  189.     (format file "~d ~,3f ~,3f ~,3f ~,3f~%"  ;write one line match
  190.         (+ 1 i) (car line) (cadr line) (third line) (fourth line))))
  191.      (dolist (match (reverse list-of-matches))
  192.        (write-proj-info file (viewmatch-view match))
  193.        (format file "~d~%"                                        ;number of line matches
  194.            (count-matchlist-numlines (viewmatch-match-list match)))
  195.        (dolist (linematch (viewmatch-match-list match))
  196.      (let ((index (car linematch)))
  197.        (dolist (line (cadr linematch))
  198.          (let ((pt1 (car line))
  199.            (pt2 (cadr line)))
  200.            (format file "~d ~,3f ~,3f ~,3f ~,3f~%"  ;write one line match
  201.                index (car pt1) (cadr pt1) (car pt2) (cadr pt2))))))))))
  202.  
  203.  
  204.  
  205.  
  206.  
  207.